Editor: Erik Bodin Version 1 · 3System Overview The entire system consists of three main parts: • The Balrog - the autonomous tracked vehicle on which the project is based, the

Technical Documentation

Editor: Erik Bodin

Version 1.0

Status

Reviewed Erik Bodin 2016-12-19Approved Martin Lindfors 2016-12-19

Autonomous Minesweeper 2016-12-19

PROJECT IDENTITY2016/HT, TIGER

Linkoping University, Dept. of Electrical Engineering (ISY)

Group membersName Responsibility Phone Email

Alfred Patriksson Head of Software 076 210 94 30 [email protected] Reizenstein Head of Hardware 070 279 29 00 [email protected] Bodin Head of Documentation 076 241 11 32 [email protected] Ling Head of Testing 073 525 72 76 [email protected] Watz Head of Information 076 817 57 75 [email protected] Sjoberg Project Manager 070 652 14 01 [email protected] Morhed Head of Design 073 520 84 59 [email protected]

Group e-mail: [email protected]

Customer: Saab DynamicsContact at customer: Torbjorn Crona, [email protected]

Technical expert at customer: Erik Ekelund, [email protected]

Examiner: Daniel Axehill, [email protected]: Martin Lindfors, [email protected]: Per Bostrom, [email protected]

TSRT10Design Specification

Page I [email protected]


Contents

Document history V

1 Introduction 1

1.1 Project History . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1

2 Definitions 3

3 System Overview 4

3.1 Balrog . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

3.1.1 Code generation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

3.1.2 Daemons . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5

3.2 Base station . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6

3.2.1 GUI . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6

4 Hardware 7

4.1 Platform . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

4.2 Hand controller and receiver . . . . . . . . . . . . . . . . . . . . . . . . . . 7

4.3 Sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

4.3.1 LIDAR . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

4.3.2 IMU . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

4.3.3 Ultrasonic rangefinders . . . . . . . . . . . . . . . . . . . . . . . . . 8

4.3.4 Odometers . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

4.3.5 Camera . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

4.4 Computers . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

5 Positioning 10

5.1 Computer vision . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

5.2 Dead reckoning . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

5.3 Extended Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

5.4 Alternative implementations . . . . . . . . . . . . . . . . . . . . . . . . . . 15

6 Mapping 17

6.1 Explored Map . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17

6.2 Obstacle Map . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18

6.3 Alternative algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19


Page II [email protected]


7 Navigation 21

7.1 Search algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21

7.2 Cost function . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22

7.3 Avoidance of obstacles . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23

7.3.1 Definition of an obstacle in the proximity . . . . . . . . . . . . . . . 24

7.4 Future work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24

8 Control 26

8.1 Basic function . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26

8.2 Features . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27

8.2.1 Turn first, then move . . . . . . . . . . . . . . . . . . . . . . . . . . 27

8.2.2 Constant speed . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27

8.2.3 Low-Pass Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27

8.2.4 Stop when reasonably close . . . . . . . . . . . . . . . . . . . . . . 28

8.2.5 Scaling . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28

9 Coordinator 29

10 Communication Module 31

10.1 TCP-thread . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31

10.2 UDP-thread . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31

10.3 Lost connection . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31

10.4 Protocol . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32

10.5 Future development . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32

11 Logging 34

12 GUI 35

12.1 Dependencies . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35

12.2 Interface . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35

12.3 Configuration files . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36

12.4 Cross-thread communication . . . . . . . . . . . . . . . . . . . . . . . . . . 37

12.5 Controller Thread . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37

12.6 Opportunities for future improvement . . . . . . . . . . . . . . . . . . . . . 37

13 Recommendations for future work 39

13.1 ROS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39

13.2 Parallelization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39

13.3 Code generation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39


Page III [email protected]


A Deduction of probabilistic time updating algorithm 41


Page IV [email protected]


Document history

Version Date Changes Changed by Reviewed

0.1 2016-11-15 Creation of document IS

0.2 2016-12-15 Updated according to comments - IS

0.3 2016-12-18 Updated according to comments - IS

1.0 2016-12-19 Approved by customer IS ML


Page V [email protected]


1 Introduction

This document will describe the implementations of the 2016 minesweeper CDIO project.The primary purpose of this project was to use the available sensors to estimate theposition of the Balrog as accurately as possible, and to map an area containing obstaclesat unknown locations. To do this, functionality to autonomously navigate a specifiedarea was also implemented. During this project, the goal was to avoid working with low-level programming as much as possible, and instead implement high-level algorithms thatutilized previously implemented low-level functionality.

1.1 Project History

This section is based on documentation from earlier projects, combined with informationabout the work from this year.

The history of the minesweeper project begins in 2009, when the project first came intoexistence as a CDIO project at Linkoping University. The first group to work on theproject was called O’Hara’s. The group worked with a terrain robot called ”MobileScout”, and the aim of their work was to define the system, and work on control, networkcommunication and navigation.

In the autumn of the same year the next project group, Carpe Locus, resumed the workwith the Mobile Scout. This group provided the robot with remote control via a laptop,and automatic control for navigating between predefined reference points. At this time,a GPS is also added to the robot.

A year later, in the autumn of 2010 the group was called 8Yare. This time, the robotwas equipped with an industrial computer, and all the old code transferred. The otherfocus of this group’s work was to integrate a Bumblebee2 BB2-08S2c stereo camera andevaluate the performance of such a camera in this setting.

The following group, iMAP from 2011, continued the previous year’s work and used thesensors to create a 3D map of the surrounding area. [2]

The autumn of 2012 was the year when the robot received its name ”Balrog”. Thisname was given by the group Minenmarker, who improved the positioning of the robot.In addition to the refined positioning, minesweeper functionality ensuring that an entirearea had been searched, and functionality indicating where mines had been found wasimplemented.

In 2013 the Balrog was equipped with a wireless hand control for manual control. Thegroup, called Ostende Abscondita, advanced the work of the previous year in improvingthe search algorithm and positioning of the robot. [5]

Invenire Periculosa took up the work in the autumn of 2014. This year the Balrog hadreceived a 360° laser sensor, and a new, more effective industrial computer. With the newhardware the group further ameliorated the positioning of the robot. [1]

In the autumn of 2015, a new IMU containing a barometer was integrated, to once againmake the positioning better. The group, called Ross Haj, also implemented a new sub-system for signal processing and sensor diagnosis. [4]


Page 1 [email protected]


This year, the Balrog has been equipped with a high performance LIDAR and a camera.The focus of the project has been to improve the positioning and mapping of the Balrog, inan indoor environment. In addition the TIGER group has made the Balrog autonomousby providing it with a navigation and a control module.




2 Definitions

• Balrog or Platform - The tracked vehicle on which the project is based.

• Extended Kalman Filter (EKF) - A Kalman filter that, in this case, uses firstorder approximations of nonlinear functions.

• HAL - A Hardware Abstraction Layer provides a unified high-level interface toaccess low-level hardware functionality. This is so that programmers do not have toconsider the details of signal timing, polling, or similar hardware considerations.

• Indoor test area - A horizontal rectangular indoor area with side lengths from5 to 25 meters. The test area will be static over time.

• IMU - Inertial Measurement Unit, used to measure angular velocity and accel-eration.

• LIDAR - Light Detection And Ranging, A laser rangefinder used to measurelong distances.

• LQR - Linear-quadratic regulator.

• Map - A local made-to-scale representation of the indoor test area. It mayinclude selected obstacles.

• Obstacle - An object that the platform cannot pass through.

• Position - The center of the IMU unit placed on the platform, which is also thecenter of rotation.

• Software in the loop (SIL) - A simulation of the complete system running locallyon a host computer, often used for quick, iterative testing of implementations.

• Uniform-cost search - A search algorithm that maximizes short term instead oflong term profit. ’Profit’ might represent time saved, money saved, distance traveledetc.




Figure 1: The Balrog, the platform used during this project.

3 System Overview

The entire system consists of three main parts:

• The Balrog - the autonomous tracked vehicle on which the project is based, theBalrog is shown in Figure 1.

• The base station - a laptop from which the Balrog is started. The base station alsocontains the GUI described in section 12.

• The hand controller - used for manual control of the Balrog, it is shown in Figure 3.

3.1 Balrog

The Balrog consists of a tracked vehicle equipped with different types of hardware. Dif-ferent aspects of the platform are controlled by several single-board computers, but inthis project only the code on the Raspberry Pi has been modified. The Raspberry Piruns one main program with several daemons running alongside it. How these programsinteract can be seen in Figure 2.

3.1.1 Code generation

The primary program is written in MATLAB, but transformed to C++ code using codegeneration. This simplifies the coding of complex mathematical functions and array oper-ations. The main application is divided into several smaller components called modules.The primary modules are:




Figure 2: Overview of the interactions between programs and hardware

• Positioning, estimates state.

• Mapping, creates maps of the environment.

• Navigation, determines destinations and calculates paths.

• Control, follows reference paths.

These modules are not separate programs, but instead separate MATLAB functions withdefined inputs and outputs. This means that any of these modules can be replaced withan entirely different function as long as it has the same input and output format.

3.1.2 Daemons

There are several programs running parallel to the main program. These programs are:

• Lidar, acts as a buffer for the LIDAR measurements.

• Logger, logs data created during operation.

• ComputerVision, uses camera to estimate positions of specified images.

• Communicator, uses wi-fi to communicate with base station.




Of these daemons, Lidar and Logger were provided while ComputerVision and ComputerVision

were created as part of the project. Since they were included with the platform, Lidarand Logger are considered to be part of the hardware in Figure 2.

3.2 Base station

The base station consists of a PC. This PC can connect to the Raspberry Pi and replace,add or update programs. There is also a Graphical User Interface from which statusinformation from the platform can be viewed.

3.2.1 GUI

The GUI is a program written in C++. A communication protocol has been defined thatallows the platform to send various messages with sensor data, text messages or mapsthat can be viewed in the GUI. This allows a user to supervise autonomous operationsand evaluate performance.




4 Hardware

The platform consists of a tracked vehicle, on which a large variety of sensors, computersand other hardware has been mounted. These will be described in more detail in thissection.

4.1 Platform

The platform is a MMP-30 tracked vehicle. It has two 24 V motors which are controlledby a Sabertooth2x25 motordriver. It is powered by two battery packs, each with 20 1.2V, 3000 mAh NiMH batteries. A quadratic aluminum plate has been mounted on top ofthe platform where various hardware can be attached.

4.2 Hand controller and receiver

The platform can be manually controlled using a 4 channel Futaba 4PLS T-FHSS Com-puter Radio System. This system consists of a hand controller seen in Figure 3 and aFutaba R304SB receiver mounted on the Balrog. The controller makes a beeping soundwhen being inactive for too long or the battery runs low.

Figure 3: The remote control used to steer the platform.




4.3 Sensors

In order to track the movement and position of the platform and to detect obstacles, awide array of sensors are integrated in the platform. These are listed in Table 1 anddescribed below.

Table 1: Short summary of sensors present on the platformSensor type Measures Estimates # ofOdometer Wheel rotation Platform movement 2IMU Acceleration, angular velocity Platform movement 1LIDAR Distance Obstacle distance 1Ultrasonic rangefinders Distance Obstacle distance 4Camera AprilTag locations Platform position 1

4.3.1 LIDAR

For accurate obstacle detection, a SF30-B LIDAR has been mounted at the front of thealuminium plate. It can detect objects at a range of 50 meters. The resolution of thedistance measurements are discretized, this resolution can be set to values from 0.03 to0.5 meters. Currently it is set to a resolution of 0.03 meters. With this resolution theupdate frequency could be 2289 Hz if it was connected to a serial port, but with a USBconnection it is limited to 286 Hz.

4.3.2 IMU

There is a MTi 100 Inertial Measurement Unit located at the midpoint of the platform,place upside down. It contains accelerometers, magnetometers and gyroscopes for mea-suring acceleration, angle and angular velocity in all three dimensions. It operates at afrequency of 50 Hz.

4.3.3 Ultrasonic rangefinders

For close range obstacle detection there are four SRF10 ultrasonic rangefinders, one lo-cated at each corner of the platform. They have a max detection distance of 11.29 metersand communicate using I2C.

4.3.4 Odometers

To measure how far the platform has traveled there are two E5 series rotary encoders,one attached to each front wheel. They have 500 marks on a circular disc, and with awheel diameter of 0.18 meters it can detect movements of 1.131 millimeters or more.




4.3.5 Camera

A Raspberry Pi Camera Module v2 is located on the front right on top of the platform.It is capable of capturing video up to a resolution of 1080p. Depending on video format,it can capture video at as high as 90 frames per second. Because of limited processingpower video is currently captured at a resolution of 320x240 and then sampled at around10 frames per second.

4.4 Computers

To communicate with and control the various hardware on the platform there are threesingle-board computers, one Raspberry Pi 3 Model B and two Arduinos. The RaspberryPi is the primary computer. It runs the majority of the programs and is responsible fortasks such as mapping, computer processing, position estimation and navigation. Thecamera is connected to the CSI port on the Raspberry, and both Arduinos, the LIDARand the IMU are connected to the USB ports. It is powered through the Micro-USB portby a cable connected to the platforms internal battery.

Of the two Arduinos, one is external and one is internal. The internal Arduino is re-sponsible for controlling the motors and reading the odometers. It receives motor signalsfrom the Raspberry, and sends how many radians each wheel has turned since the lastreading. The external Arduino reads data from the R304SB receiver and the ultrasonicrangefinders, and relays this data to the Raspberry Pi. The ultrasonic rangefinders areconnected to the I2C pins on the external Arduino, and the receiver is connected to aserial pin.




5 Positioning

The positioning module is a part of the main application, responsible for continuouslyestimating the position of the platform in the area being searched. To do this, direct esti-mates of the position using computer vision are combined with dead reckoning. Figure 4shows how data is exchanged to estimate the position.

Figure 4: The flow of information and data during state estimation.

5.1 Computer vision

In order to avoid accumulation of errors in positioning estimates over time, a measurementof the platforms position in the world is necessary. This is achieved by using a Raspberrycamera to detect AprilTags [6]. A number of AprilTags from the family 36h11 are placedat known locations in the area to be searched. Using the AprilTags library, the positionand angle of visible AprilTags relative to the camera can be calculated.

A program called ComputerVision is running parallel to the main application. It has twothreads, one that communicates with the main application and one that performs AprilTagdetections. For each AprilTag that is detected ComputerVision sends the rotation of thetag relative to the camera as a rotation matrix Qt|c, and the x, y and z coordinates of thetag relative to the camera pdet,x, pdet,y and pdet,z.

When the main application receives these measurements it can calculate the distance tothe AprilTag Dtag, the angle of the robot in the global coordinate system θtag and theangle between the tags position and the platforms position φtag, as well as the rotation ofthe tag relative the camera βtag. βtag is the pitch angle in the rotation matrix Qt|c, thisangle is computed by calculating the Euler angles of the rotational matrix Qt|c. Dtag andφtag are used to represent the robots global position using polar coordinates.

θtag, Dtag and φtag are calculated using the following equations:




Dtag =√

(px,det + px,cam)2 + (py,det − py,cam)2, (1)

φtag = ψtag − βtag − arctan

(pdet,y − py,campdet,x + px,cam

), (2)

θtag = ψtag − βtag − π. (3)

θtag can be used directly as an estimate of the robot’s angle in the global coordinatesystem. Using the known coordinates of the tag in the global coordinate system ptag,xand ptag,y, the robot’s position can be estimated as

px = px,tag +Dtag cos (φtag), (4)

py = py,tag +Dtag sin (φtag). (5)

However, because of internal workings in the AprilTags library the angles from the Qt|crotation matrix may be mirrored. To determine the sign of φtag, the sign is chosen tominimize

φ∗tag = argminφ′tag=±φtag

((px,tag +Dtag cos (φ′tag)− px)2 + (py,tag +Dtag sin (φ′tag)− py)2). (6)

where px and py are the previously estimated robot position.

When determining the estimated robot pose in the global coordinate system using April-Tags, there are 3 coordinate systems involved: the global coordinate system, the robot-fixcoordinate system and the camera-fix coordinate system. The only differences between therobot- and camera-fix coordinate systems are that the y-axis is inverted, since AprilTags

uses a left oriented coordinate system, and that they are translated, i.e. the camera-fixcoordinate system has its center in the camera, and the robot-fix coordinate system hasits center in the center of the robot plate. The different coordinate systems and anglescan be seen in Figure 5.

In the family of AprilTags used there are 256 possible unique tags. The main applicationhas access to the file CV tagmap.m which contains the ID:s, rotations and poses of knowntags. However, additional tags may be placed on the map. These will be detected byComputerVision but will be marked as invalid and ignored by the main application.The total number of tags on the map, ntotal, is not known by the platform. Since thecamera was not expected to detect more than a few tags at the time, ComputerVisionsends detection in a message with constant size. This message is a struct containing thetranslation and rotation for nmax tags. In this project the value of nmax was set to 4.If more than nmax tags are detected arbitrary nmax detections are sent to the Matlabimplementation. If there were less than nmax detections the remaining fields are set tozero. Each detection has an associated flag valid, describing whether there actuallywas a detection so these zeros can be ignored. This implementation was chosen so thatthe existing HAL implementation could be used for communication between the C++implementation and the implementation in MATLAB. A reason for doing this was thatthe measurement model would be constant size and not become to large.




Figure 5: The different coordinate systems used when converting AprilTag measurementsto position estimates.

5.2 Dead reckoning

The dead reckoning motion model is used to estimate how the platform has moved sincethe last time instance. The sensors available are the odometers and the IMU. The odome-ters and IMU update at around 50 Hz, considerably faster than the ComputerVision

measurements. Additionally, there will always be new measurements available at eachtime instance unlike the AprilTag measurements. They can therefore be used to estimatehow the platform has moved between AprilTag measurements.

The odometers measure how many radians each wheel has turned since the last odometermeasurement. The IMU measures acceleration, angular velocity and the angle using themagnetic field. The odometer measurements are significantly less noisy than the IMUmeasurements. The magnetic field measurements were discarded very early, since thelarge amount of outside interference from the engines and other sources makes the dataunusable. Eventually, it was decided not to use the acceleration or angular velocitymeasurements either since, with the current models, they did not improve the estimatesfrom the odometers.




At first, the dead reckoning motion model estimated position, speed, angle and angularvelocity. The velocity and angular velocity from the odometers were measured accordingto

vodometer =dleft ∗ wr + dright ∗ wr

2dt(7)

ωodometer =dleft ∗ wr − dright ∗ wr

wbdt(8)

where dright and dright are how many radians each wheel has turned since the last mea-surement, dt is the time since the last measurement and wr and wb is the wheel radiusand distance between the wheels.

There were however some complications from the varying run time of the program. Whilethe platform is supposed to update at a frequency of 50 Hz, in reality there are variationsin the run time each loop, sometimes faster and sometimes slower with a mean of 50Hz. This had some implications on the original dead reckoning algorithm that neededto be handled. If the sampling frequency was assumed to always be 50 Hz, this madeestimates of speed and angular velocity using the odometers incorrect, since the angleturned is associated with a time interval of incorrect length. Setting the sampling timeto the actual time since the last measurement gave more correct velocity measurements,but the combinations of different sample times resulted in a noisy signal.

The decision was made to instead measure the distance traveled and angle turned accord-ing to

Dtraveled =dleft ∗ wr + dright ∗ wr

2, (9)

θturned =dleft ∗ wr − dright ∗ wr

wb, (10)

discarding the sample time entirely. This resulted in a less noisy signal. These wherethen added as states in the motion model. The reason for doing this was to keep the timeupdate and measurement update in the Extended Kalman Filter separate. Dtraveled andθturned are added as measurements in the measurement update, which are then added tothe current position and angle estimates in the time update. The resulting motion modelhas similarities with a coordinated turn, constant velocity motion model and is given by

xk+1 =

px,k+1

py,k+1

θk+1

Dtraveled,k+1

θturned,k+1

=

px,k + cos (θk)Dtraveled,k

py,k + sin (θk)Dtraveled,k

θk + θturned,kDtraveled,k

θturned,k

+ vk = f(xk) + vk, (11)

where the process noise is included in vk.

5.3 Extended Kalman Filter

An EKF was implemented to fuse the dead reckoning estimations with the AprilTagpositions. The motion model used is the one described in (11). The measurementswere distance traveled and angle turned from the odometers and varying amounts of tag




detections from computer vision. The computer vision module has a max number of tagsnmax that it sends to the positioning module each time instance. The measurement modelconsiders each possible tag detection as a separate sensor, with each sensor measuringposition and angle. The measurement model was implemented as

yk =

dright∗wr+dleft∗wr

2dright∗wr−dleft∗wr

wb

p1x,tag +D1tag cos (φ1

tag)p1y,tag +D1

tag sin (φ1tag)

θ1tag...

pnmaxx,tag +Dnmax

tag cos (φnmaxtag )

pnmaxy,tag +Dnmax

tag sin (φnmaxtag )

θnmaxtag

=

Dtraveled,k

θturned,kpx,kpy,kθk...px,kpy,kθk

+ ek = h(xk) + ek. (12)

where ek contains the measurement noise. If less than nmax AprilTags are detected,the remaining fields are filled with zeros. To handle such missing measurements, eachmeasurement has a corresponding valid flag, denoting whether it contains an actualmeasurement or not. When comparing the estimated state with the measurements themeasurement update, all rows in h(xk) corresponding to a measurement with valid = 0

are set to[0 0 0

]Tinstead of

[px,k py,k θk

]T.

To implement the EKF, the functions f ′(xk) and h′(xk) had to be derived.

f ′(xk) =∂f

∂xk=

1 0 − sin (θk)Dtraveled,k cos (θk) 00 1 cos (θk)Dtraveled,k sin (θk) 00 0 1 0 10 0 0 1 00 0 0 0 1

(13)

h′(xk) =∂h

∂xk=

0 0 0 1 00 0 0 0 11 0 0 0 00 1 0 0 00 0 1 0 0...

......

......

1 0 0 0 00 1 0 0 00 0 1 0 0

(14)

Just like for h(xk), any rows in h′(xk) corresponding to an AprilTag measurement withvalid = 0 are set to zero. Technically, the measurement update is entirely linear, butit is implemented in EKF form to make integration of other measurements easier in thefuture.

The errors in the dead reckoning estimates are small, but accumulate over time. Anexample of this can be seen in Figure 6, where the platform was remotely controlled todrive several laps in a roughly L-shaped path. The AprilTags have larger errors aroundrelatively constant values. As such, the R and Q matrices, representing measurementnoise covariance and process noise covariance respectively, where chosen such that the




odometer noise and process noise are low, with the position estimation being very similarto an unfiltered dead reckoning estimate when no AprilTags are seen. When one or moreAprilTags are detected they are considered to have more noise, and are used to slowlycompensate for deviations that have occurred since the last AprilTag detection.

4 4.5 5 5.5 6 6.5 7 7.5 8

4.5

5

5.5

6

6.5

7

7.5

8

8.5

Figure 6: An example of the positioning drift using only dead reckoning. The red star isthe starting position.

5.4 Alternative implementations

The measurement model is implemented so that there are nmax AprilTag measurements,no matter how many tags are actually on the map. Since placement of detected tagsin the sent struct depend on which tags were detected, different tags can be consideredmeasurements for the same sensor at different times. This means that when a tag isdetected and placed in a certain position in the measurement model it will inherit theuncertainties of whatever tag was in that position previously. A better implementationwould be to have the number of tags on the map ntotal clearly defined. The measurementmodel could then be created with the appropriate number of measurements and associateeach tag to the same measurements every time. When tags are not detected, their noiseshould be considered to be infinite. Because of time limitations this was not implemented.

In the measurement model used, the angle and distance to the AprilTags are combinedto create estimates of coordinates. This does not take the statistical properties of tagdistance and angle into account, where the distance measurement is more precise than




the angle measurement. Currently, the uncertainties in angle and distance are mixed whencreating coordinate estimates. We suspect that better positioning could be achieved byinstead taking the tag distance and angle as measurement and express them in termsof the coordinates in the motion model, resulting in the positioning uncertainties beingcurved instead of circular. We were not successful in implementing this however, and dueto time constraints this approach was abandoned. Preliminary equations for this purposeare

Dtag =√

(px,tag − px,k)2 + (py,tag − py,k)2,

φtag = arctan

(py,tag − py,kpx,tag − px,k

),

∂Dtag

∂x=[

px,tag−px,k√(px,tag−px,k)2+(py,tag−py,k)2

py,tag−py,k√(px,tag−px,k)2+(py,tag−py,k)2

0 0 0],

∂φtag∂x

=[− py,tag−py,k

(px,tag−px,k)2+(py,tag−py,k)2px,tag−px,k

(px,tag−px,k)2+(py,tag−py,k)20 0 0

].

(15)




6 Mapping

The mapping subsystem is responsible for recording information about explored areas anddetected obstacles and placing them in a global environment. To do this it generates anobstacle map given sensor readings and estimated position, and an explored map keepingtrack of which parts of the map have been explored, i.e. driven over by the robot.

Figure 7: An instance of mapping in progress. Green areas are considered explored. Blackcorresponds to zero obstacle probability and white to obstacle probability 1.

The maps are represented as MATLAB matrices with predefined dimensions. The userspecifies the dimensions of the map and the resolution of the grid in meters, which deter-mines the size of the map matrix. An example of maps created during remote controlledoperation can be seen in Figure 7.

6.1 Explored Map

The explored map keeps track of which grids have been explored by the robot. A grid isconsidered explored if it has been below the robot, i.e. the robot has driven over it.

In order to determine which grids have been driven over, a square of grids equal to the sizeof the robot is created and the coordinates of each square is stored. The square is placedwith its center at the origin. The square is then rotated by applying a rotation matrixR, a standard 2D rotation matrix, and then translated to the position of the robot. Therotation angle is equal to the angle of the robot.

When the coordinates of the grids below the robot are known, the explored map is updatedby setting explored grids to ”true”. The obstacle map is also updated by setting theprobability that the explored grids are occupied to a low value. Ideally the probabilitywould be set to zero, but a grid with obstacle probability zero cannot be set to any other




Table 2: CaptionConstant Value

P (Yt = 1|& = 1) 0.9P (Yt = 1|& = 0) 0.1P (Yt = 0|& = 1) 0.4P (Yt = 0|& = 0) 0.6P (& = 1|Y1) 0.5

value with the current algorithm. Since the positioning is not completely accurate themapping module needs to be able to set obstacle probabilities even in squares it considersexplored.

6.2 Obstacle Map

The position estimates and LIDAR measurements contain varying degrees of errors. Thismeans that the obstacle map cannot simply be determined by placing obstacles wherethe platform perceives a LIDAR detection. Instead, a probability based obstacle map isimplemented, taking into consideration the existing errors and using multiple measure-ments to allow probabilities to converge towards 0 or 1. The algorithm works by updatingnot only the probabilities where there was a detection, but also updating grids which theLIDAR ray traversed without a detection.

Each grid is initialized to a probability of 0.5 meaning that it is just as likely to beoccupied as it is to be free. This essentially means that there is no information aboutthe state of that grid. As the robot explores the area it receives measurements from theLIDAR. By knowing its estimated state and the distance measured by the LIDAR, thestate of the grids that the LIDAR ”ray” passes over can be updated. The grid the LIDARactually hits has the probability that it is occupied increased, and all the other concernedgrids have their probabilities decreased.

We are interested in the probability that there is an obstacle in a grid given a sample ofmeasurements in that grid, that is P (& = 1|Y1:t). Where & ∈ {0, 1} and denotes if thegrid is occupied (& = 1) or free (& = 0). Yt ∈ 0, 1 and denotes if the LIDAR detected anobstacle in the grid at time instance t (Yt = 1) or not (Yt = 0). The formula to updatethe probability of an obstacle in the grid given a new measurement is given by (16). Theprobabilities P (Yt = 1|& = 1), P (Yt = 1|& = 0), P (Yt = 0|& = 1) and P (Yt = 0|& = 0)are user defined constants and are dependent on the quality of the LIDAR and have tobe designed. The algorithm also requires all grids to have an initial value, P (& = 1|Y1).How this time updating algorithm was derived can be seen in Appendix A. The values ofthe constants can be seen in Table 2.

P (& = 1|Y1:t) =P (& = 1|Y1:t−1)P (Yt|& = 1)

P (& = 1|Y1:t−1)P (Yt|& = 1) + P (& = 0|Y1:t−1)P (Yt|& = 0)(16)

Figure 8 shows an example of the mapping algorithm in action. The red grid indicatesthe grid hit by the LIDAR, this grid has its probability updated according to (16) usingthe constants where Yt = 1. The green grids, i.e. the grids that the ray passes over,are found using Bresenham’s Line Algorithm [3]. They have their probabilities updated




according to (16) using the constants where Yt = 0. If the LIDAR detection is outside ofthe defined map, the line is drawn to the LIDAR detection but all elements outside theactual map are discarded. If the LIDAR is outside of the map, no line is drawn.

Figure 8: LIDAR ray tracing using Bresenham’s Line Algorithm.

The LIDAR operates around 280 Hz, while the main application runs at 50 Hz. Thismeans that when the mapping function is running there will be more than one LIDARmeasurement available and each of these correspond to an intermediate state between theprevious state and the current state.

In order to make use of these additional LIDAR measurements, the main algorithm up-dating the map given a LIDAR measurement and the robot state is run once for eachnew LIDAR measurement available. The state of the robot corresponding to each LIDARmeasurement is estimated using linear interpolation between the previous state and thecurrent state.

While this method is of course not completely accurate it has been proven to work wellin practice, drastically increasing the map convergence rate and general quality.

6.3 Alternative algorithm

Instead of running the entire mapping function for each new LIDAR measurement, themapping could use a recursive function to update for all new measurements at once. Inorder to implement this, the angle and position differences between each measurement




are assumed to be small enough that all measurements can be approximated as beingdone in the same pose. Then, a single line can be drawn using Bresenham’s algorithmto the furthest LIDAR measurement. A mapping function called mapping recursive.m

has been implemented and is available for use by future groups. How this function wasderived is described below.

By using the fact that P (& = 0|Y1:t−1) = 1 − P (& = 1|Y1:t−1), equation (16) can berewritten as

P (& = 1|Y1:t) =1

1 + 1P (&=1|Y1:t−1)

P (Yt|&=0)P (Yt|&=1)

− P (Yt|&=0)P (Yt|&=1)

. (17)

P (Yt|&=0)P (Yt|&=1)

can be expressed as a constant Cprob. Going 1 step backwards, P (& = 1|Y1:t−1)can be expressed as

P (& = 1|Y1:t−1) =1

1 + 1P (&=1|Y1:t−2)

Cprob − Cprob. (18)

Inserting equation (18) into equation (17) results in

P (& = 1|Y1:t) =1

1 + (1 + 1P (&=1|Y1:t−2)

Cprob − Cprob)Cprob − Cprob=

=1

1 + Cprob + 1P (&=1|Y1:t−2)

C2prob − C2

prob − Cprob=

=1

1 + 1P (&=1|Y1:t−2)

C2prob − C2

prob

.

(19)

This can be repeated for any amount of steps. That means that if for a grid there hasbeen m detections or m non-detections, the grid can be updated according to

P (& = 1|Y1:t) =1

1 + 1P (&=1|Y1:t−m)

Cmprob − Cm

prob

(20)

with Cprob using either Yt = 0 or Yt = 1. If a grid has both detections and non-detections,the algorithm has to be applied twice, once for detections and once for non-detections.However, since there are usually around six detections, it would still mean that most gridsare updated once and about six grids are updated twice instead of a lot of grids beingupdated around six times.

The downsides of this algorithm is that it is assumed that all measurements are made fromthe same pose and that fewer lines are drawn. Fewer lines results in the updates beingless spread out, meaning that the total number of grids updated is decreased. While thisalgorithm runs slightly faster than the interpolating one this increase was not consideredto be worth the decrease in map accuracy. However, for larger maps or smaller grids, itis possible that this algorithm might be worth implementing.




7 Navigation

The navigation acts as the brain in the robot and decides where it should go next. Theavailable information for the function is the map, described in section 6 and the state ofthe robot, described in section 5. The output of the function is an array containing anumber of connected coordinates which corresponds to a route leading to an unexplorednode. While constructing this kind of a function there are two main questions one has totake into consideration:

• Which node should be chosen as goal node?

• Are there any obstacles obstructing the route?

These questions are used to formulate the basic properties of the navigation module:

• The goal node is the best unexplored node with respect to cost.

• The route to the goal node should keep a safe distance from objects and the edgeof the map.

• The route is allowed to diverge in eight directions, which is all available directionsin a two-dimensional grid map.

This part of the documentation will describe how the navigation accomplish these prop-erties by providing a smooth understanding of how the navigation of the robot works.

7.1 Search algorithm

The search algorithm used by the robot to find the next node to which to travel is basedon the uniform-cost search algorithm. This algorithm finds the cheapest node to satisfya goal requirement, which in this case is unexplored. Listed below is a sequence of eventsthat roughly describe the syntax of actions used by the implemented search algorithm, avisualization can be seen in Figure 9.

1. Create a list of nodes, nodesToSearch, where each node has the following properties:

• xPosition: The position of the node in the x-direction.

• yPosition: The position of the node in the y-direction.

• pathToNode: The nodes which correspond to the route from the start node tothis node.

• pathToNodeLastIndex: pathToNode is a long array and this index points atthe last node in that array.

• directionOfBalrog: The direction of the robot in this node.

• costOfNode: The cost of the node.

2. Add the current node to the list with zero as the cost.




3. Pick the cheapest node in nodesToSearch.

4. Check if the node is unexplored and at a safe distance from any objects, if true thenreturn from the function with pathToNode as an output. If false then continue inthis list.

5. Remove the node from nodesToSearch. If the node is at a safe distance from anyobjects add all adjacent nodes which has not been searched more than a certainnumber of times. If the node is close to the edge of the map, add the nodes whichlie inwards from the edge to nodesToSearch.

6. Iterate from 3.

Figure 9: Flow chart which roughly visualize the search algorithm.

7.2 Cost function

The cost function evaluates the cost of a node when it is added to the nodesToSearch,depending on how this function is designed one will get very different navigation patterns.The general formula is:

costnext = costcurrent + f(nodecurrent, nodenext) (21)

The function f is designed by the following criteria:

• It should cost to move forward.




• It should cost to turn.

• It should penalize diagonal movement over horizontal/vertical movement.

The last criterion is what is most diffuse, however, it will make the explored areas of themap more connected. This in turn leads to less widespread unexplored nodes when themap is almost fully explored, which means that it will take less time to completely explorethe whole map. The criterion achieves this by letting the navigation module prioritizerectangular areas. Placing a lot of rectangular areas in a rectangular map will lead to lessgrids not covered by the rectangular robot than one would get if it used other arbitraryshapes.

By translating these criteria into a function we get the following:

f(nodecurrent, nodenext)

= αcost-forward(if diagonal: · αcost-diagonal-factor) + αcost-turning · direction-difference(22)

One can argue that there does not need to be three parameters as it is only the ratio inbetween them that are of importance. But by having all three there gives more clarityabout how the parameters affect the search algorithm. In Table 3 there is an explanationof each of the design parameters together with the currently chosen values.

Table 3: Descriptions of the design parameters in the navigation module.

Parameter Description Valueαcost-forward Cost of moving forward 1αcost-diagonal-factor Ratio between the cost of moving

horizontal/vertical and diagonal4

αcost-turning Cost of turning the robot one direction 2

7.3 Avoidance of obstacles

For the robot to be able to navigate through a map containing obstacles it has to be ableto avoid them. This avoidance of obstacles is implemented by following these two simplerules:

• Any node in which the robot is allowed to search has to be at least 35 cm from anyobstacle, this guarantees that no route will be closer than 35 cm from an obstacle.

• Any goal node has to be at least 50 cm from any obstacle.

These rules imply that the robot is allowed to travel closer to an object than the alloweddistance between the goal destination and any obstacle. This is appreciated becauseinsecurity in the position will be less likely to suddenly set the position of the robot at adistance closer to an object than it is allowed to navigate at, causing the robot to not beallowed to move in any direction. These rules are illustrated in Figure 10.




Figure 10: White areas indicate obstacles, red areas illustrate where the robot is notallowed to navigate, yellow area illustrates where the robot can navigate but not use asan goal node, green areas are available for everything.

7.3.1 Definition of an obstacle in the proximity

To make sure that a node has no obstacles in its surroundings the probability that thereare no obstacles in the proximity is compared against a threshold, if it is above thethreshold it is assumed to be true. For example if we want to control that a node has noobstacles in a distance of 35 cm we can use (23). If that expression is true, where pthresholdis designed after the security in the map, the node is assumed to have no obstacles in aproximity of 35 cm.

∏i∈M

(1− Pi) > pthreshold (23)

Where M is the set of nodes closer than 35 cm from the original node and Pi is theprobability of an obstacle which is stored in the map given as an input to the navigationalgorithm.

7.4 Future work

The algorithm implemented is fully capable of searching all safe areas of a map. However,a uniform-cost search will only look one unexplored node into the future instead of severalnodes which might result in complete searches that take more time and effort than whatmight be desired. For the future, an algorithm that optimizes the total travel time overseveral unexplored nodes might be worth considering. An algorithm like this is harder toimplement and it will most likely require significantly more computing power.




There are currently two critical problems regarding the navigation which should be solvedin the future:

• Navigation through nodes with no information about obstacles.

• The robot does not reevaluate the navigation route even if the map or robot stateschanges.

The first item regards the early stage of the exploring, when there is little informationabout the map. This will lead to the robot having to travel through nodes which theLIDAR has not been directed to, which means that there might be obstacles there. Thismeans that the navigation might return a route where there are obstacles in the proximitywhich the robot will collide with. The second item regards the fact that the navigationwill never be reevaluated even if an obstacle would be seen in the path of the robot. Thisproblem might occur if the states of the robot drifts a lot.




8 Control

The controller used is a simple proportional controller with some ”features” added. Thebasic controller structure will be described in the first section and the ”features” in thesecond.

Only the fundamental ideas behind each part of the controller are presented here. Fordetails and implementation please check the code, toolbox/Control/control law.m inthe repository.

8.1 Basic function

The motion model used is the so called ”Unicycle Model”, which is given by

x = v cosφ,

y = v sinφ,

φ = ω.

(24)

The controller has two variables to adjust while operating. The speed of the robot in thebody-fixed coordinate system denoted v and the angular velocity of the robot denotedω. The commanded forward velocity and the commanded angular velocity are calculatedaccording to

vc = Kv

√(xG − x)2 + (yG − y)2,

φε = φ− atan2(yG − y, xG − x) + j2π,

ωc = Kωφε,

(25)

where subscript G denotes goal position and j is chosen such that −π < φε ≤ π. Theparameters Kv and Kω are tunable controller gains. Another equivalent motion model isthe ”Differential Drive Model” given in (26). Here the angular velocities of the individualwheels are used as well as the parameters B denoting wheel base (distance between wheels)and the wheel radius RL and RR.

x =RLωL +RRωR

2cosφ

y =RLωL +RRωR

2sinφ

φ =RLωL −RRωR

B

(26)

Noting that equations (24) and (26) are different representations of the same system onecan go ahead and solve for ωL and ωR given vc and ωc resulting in

ωL =2vc + ωcB

2RL

,

ωR =2vc − ωcB

2RR

.

(27)




8.2 Features

In order to get a controller producing smooth movement as well as good path followingwhen integrated with the other modules in the system, especially the Navigation module,a few ”features” had to be added to the basic controller structure.

8.2.1 Turn first, then move

The desired behaviour of the robot would be to take the direct path from its currentposition to the provided goal position. However if the goal position is not in line with thecurrent orientation of the robot, which is quite often the case, the robot would initiallyturn while simultaneously going straight ahead causing it to deviate substantially fromthe direct path.

One way to mitigate this would be to use a high angular gain, Kω, but this would causeundesirable behaviour once aligned towards the goal position. Another option is to usea programmatic approach simply saying that as long as the size of the angular error islarger than a certain threshold, do not move forward. Only turn with a fixed angularvelocity. This is implemented with a simple if-else statement in the controller code.

In order to get a continuous commanded angular velocity, the angular velocity gain hasto be a function of the threshold angle and the fixed angular velocity during turningaccording to

Kω =Kω,f

εφ(28)

where Kω,f is the commanded angular velocity when the angular error is greater than thethreshold value εφ.

8.2.2 Constant speed

The desired behaviour of the robot would be to move with constant speed when travellingbetween two points until it is within a certain distance of the goal position where it wouldgradually slow down. However as the commanded speed of the robot is a function of thedistance between the goal position and the actual position it would change continuously,assuming no controller saturation. In order to mitigate this the distance between currentposition and goal position used in the calculation of commanded speed is saturated to acertain threshold distance if it becomes too large.

8.2.3 Low-Pass Filter

In order to get smooth movement of the robot a (pseudo) low-pass filter preventing thecontrol signals to change arbitrarily fast was implemented. The implementation is simple.If the change in commanded angular velocity and/or linear velocity is greater than acertain value it is only allowed to change a certain step size per iteration.




8.2.4 Stop when reasonably close

In order to not continuously put load on the motors the controller was programmed tostop the motors when the robot was within a certain distance of the commanded goalposition. This is implemented as a simple if-else statement in the controller code.

8.2.5 Scaling

Before sending the controls to the motor driver they are saturated into the interval [−1, 1]since the motor controller expects the commanded speed of each motor to be within thatinterval. This is accomplished by dividing both control signals by the largest absolutevalue of the two, if it is greater than one.




9 Coordinator

In order to facilitate smooth operation and synchronization between the subsystems Nav-igation and Control a subsystem coordinating the inputs, outputs and operation of thesesubsystems had to be implemented.

When calling the control function, a goal position has to be passed which will cause thefunction to return control outputs that can be fed to the motor drivers. When callingnavigation it will return a sequence of positions constituting a path to a desired goalposition. The main purpose of the coordinator is to feed the positions given by thenavigation system to the controller on by one and call navigation to request a new pathat the appropriate time.

Figure 11 shows a flow chart of the coordinator.




Figure 11: A chart describing the coordinator flow.




10 Communication Module

In order to communicate with an external PC the Communicator program was written,implementing sockets and TCP/IP (Transmission Control Protocol/Internet Protocol).Communication between programs on the Raspberry Pi is done using UDP (User Data-gram Protocol), which is blocking when reading. For programs running in sequence thisis not a problem, but since reading from TCP is also blocking it is more of an issue forthe Communicator. It was therefore implemented as a threaded program, with one threadcommunicating with the main program using UDP, and the other communicating withan external PC connected to the Raspberry Pi’s network using TCP/IP. It uses mutex toprotect data shared by the threads.

10.1 TCP-thread

The purpose of the TCP-thread is to open a socket and to receive data from an outsidesource. When the Communicator is started it acts as a server, with the TCP-threadopening a socket and waiting for a client to connect. When a client has connected, theTCP-thread continuously reads data from the TCP buffer. It will first read 4 bytes,corresponding to an int describing the size of the coming message. It will then readthat amount of bytes from the TCP-buffer. At the beginning of each loop the TCP-thread sends a ping message, since the only way to detect a lost TCP connection isby sending. Otherwise, reading might return a zero length message immediately, andthe thread basically becomes stuck in a while(true) loop. When the TCP-thread hasreceived data, it copies it to a shared buffer from which the UDP-thread can read andrelay it to the main program.

10.2 UDP-thread

The purpose of the UDP-thread is to relay the latest data the TCP-thread has receivedand to send messages using TCP. At the start of each loop the UDP-thread waits for amessage from the main program. If this message is read, it reads 4 bytes from the UDPbuffer corresponding to an int. It then send this amount of bytes from the read-bufferto the main program using UDP. If the message is send it reads first the length of thecoming message from the UDP-buffer as an int and then the message itself, placing it ina send-buffer. If a connection has been established to a client, this message is sent to theclient using TCP. Since sending with TCP is not blocking, this is handled by the UDP-thread. If a connection is not established, the message is not sent and will be overwrittenby the next message.

10.3 Lost connection

Every time one of the threads reads or sends using TCP they check if an error occurred.Normally the TCP read and send functions return the number of bytes read or sent, but if-1 is returned there was an error. As stated before however, only sending will guaranteethat errors are detected, while reading might simply return a 0. Reading might also return0 when there is no error, so for detecting errors while reading another approach must be




Table 4: Descriptions and contents of all different types of messages that can be sent.Descriptor(hex)

Descriptor(double)

Type Content

0x3ff0000000000000 1 Sensor data double[1]: LIDARdouble[4]: Ultrasonic readingsdouble[3]: Angular velocitydouble[3]: Accelerometersdouble[3]: Magnetometersdouble[2]: Odometersdouble[1]: Time

0x4000000000000000 2 Obstacle map int[2]: Map size (I,J)double[2]: Coordinates (X,Y)double[1]: Angledouble[I][J]: Map

0x4008000000000000 3 Explored map int[2]: Map size (I,J)int8[I][J]: Map

0x4010000000000000 4 Text char[x]

0xffffffffffffffff NaN Ping Nothing

used. The current solution is that if a 0 is returned when reading 10 times in a row anerror is assumed to have occurred. If there was an error, the current socket is closed.When the TCP-thread reaches the start of its loop it will open a new socket and wait fora client to connect.

10.4 Protocol

The Communicator is unaware of the contents of the data it sends or receives. It receivesan int describing how many bytes is in the message it will send or read, and then sendsor reads that amount of bytes. The messages are defined outside of the Communicator

on both ends of the communication link. Excluding the int describing the message size,all messages sent by the platform begins with a double which describes with the messagetype. The current messages defined are described in Table 4.

Text messages have no defined length and depend only on the message size. Ping messagesconsists only of the message descriptor with no content and should be ignored by the client.The message descriptor is always placed first in the message, so no matter what messageis sent it can be identified by reading the first 8 bytes. By using the message structuredefined on both ends the client can then allocate the received data accordingly.

Currently, only one type of message is received by the platform. This message consists ofsix doubles, representing various instructions.

10.5 Future development

Implementing functionality for receiving more than one type of message should be rela-tively easy, and could be implemented the same way as sent messages. Received messagecould start with a message descriptor, so the Communicator could send the first 8 bytes




over UDP, and then the remaining bytes of the message. The main program would handlethe message depending on what double the first 8 bytes corresponded to.

Currently the Matlab code only supports sending text, sensor readings and maps. Itshould however be trivial to add functions to send messages containing information aboutany variable in the platform such as estimated state or AprilTags.

The message structures could be stored in an external MATLAB or C++ file shared byboth the client and the platform, making it easier to synchronise message definitions.Alternatively, see subsection 12.6 for suggestions regarding restructuring of the commu-nication protocol.




11 Logging

In order to review sensor readings, state estimations and decisions at a later date, a log fileis created every time the main application is started. This log contains a data structurecontaining fields defined in the main application which are logged for each loop. The logis saved as a binary file that can be unpacked with the provided unpack.m file, providedthat a data structure on the same format as the one in the main program is defined. Ifthey are not the same the unpacked data will be corrupted.




12 GUI

The GUI is a multi-threaded program written in C++. The graphical components usethe GTKmm libraries. It implements the core functionality that is necessary for furtherdevelopment into a full fledged multi-robot control platform.

The code is written to follow the Google C++ code standard and has comments formattedfor use with the documentation generation tool doxygen.

12.1 Dependencies

The program uses features from the C++11 standard and needs to be compiled in a modernenvironment. This means that the project needs to be compiled on a g++ version newerthan 4.8 which guarantees standards compliance. Other compilers that support C++11

are available but have not been tested.

The user interface has a set of dependencies separate from those of the robot. There is ascript in the Jungle git repository that installs the dependencies, but they are listed herefor completeness.

Table 5: Dependencies for the user interface.Library Version Dependencies Sourcegtkmm 3.0 Provided by package manager. Package Managerglibmm 2.4 Provided by package manager. Package Managersigc++ 2.0 Provided by package manager. Package Managerxml++ 2.7 Provided by package manager. Package Managerplplot - Provided by package manager. Package Managerboost-system - Provided by package manager. Package Managerboost-filesystem - Provided by package manager. Package Managerprotobuf-dev - Provided by package manager. Package Managergtkmm-plplot 2.1 Provided by package manager. Special Repository

Note that the gtkmm-plplot library is not available through the default package reposi-tories on Ubuntu. An up to date set of instructions for manually installing this library isavailable at:

https://tschoonj.github.io/gtkmm-plplot/installation instructions.html

12.2 Interface

The interface portion is minimalistic, but is written to conform to standard user expec-tations. Clicking and dragging on the map pans, mouse wheel zooms or scrolls whenapplicable. This is shown in figure 12. There are four main components: The controls inthe top left, the display in the top right, the log in the bottom left and the sensor datafeeds in the bottom right.

The map display appears when the GUI connects to the robot and starts to receive data.The command log currently only displays internal messages but is capable of printing




Figure 12: The GUI in a disconnected state.

messages received from the Balrog as well. The graphs display the last thirty seconds ofreceived sensor data.

The GUI layout was created using the glade layout tool. It provides a nice tool for dragand drop creation of GTKmm graphical interfaces. The interface settings are saved in thelayout.glade file. This file can easily be extended to look different without necessarilychanging any code.

The gtkmm library uses the libsigc++ library in order to propagate UI events, such asthe user clicking, to the callback functions. This is elegant in practice, with a ’when thishappens - call this function’ structure. It is combined with C++11 lambda functions inorder to keep the GUI events and their handling code closely coupled.

Three custom GTKmm widgets are implemented. The MapWidget draws the map, theConsoleWidget provides a command-line style text output, and the SensorWidget classdisplays one internal feed of sensor data.

12.3 Configuration files

Sensor information is defined in the external file sensors.xml. The XML formattingstructure is fairly self-documenting. Each category defines a tab of sensors in the GUI.Within each category sensors are listed separately and with the relevant attributes defined.This is easily expandable to add further sensors or sensor attributes when necessary.




The interface includes a bare-bones settings saver. The settings are provided in a globalobject with thread safe locking. This is the only globally available object. Extending theamount of available settings is a simple matter of updating the Settings class to containmore fields.

The sensor data logging functionality is also a part of the interface. The directory and filemanagement is implemented using the boost::filesystem library. Every SensorWidget

display opens its own log file and outputs CSV formatted data into it.

12.4 Cross-thread communication

The internal Feed:: namespace objects were implemented in order to provide a unified in-terface for cross-thread communication. They provide FIFO buffer objects (Feed::Hooks)that can be written to, accessed and deleted in a thread safe manner.

To make a class capable of pushing data to Feed objects it should inherit the Feed::Out

class. This defines the interface functions for connecting Feed::Hooks and pushing datathrough them. To make a class capable of receiving this data it has to inherit fromFeed::Out in a similar fashion. Feed::Out implements a pure virtual callback functionOnFeedUpdate() which is called when the buffer has been updated. Once the receivingclass has implemented OnFeedUpdate() it is ready to receive data.

The data type that the Feed:: library transmits is Protocol Buffer objects called Messages.These are defined in the message.proto file. Adding new fields to the message class isstraightforward because of this.

Each Feed::Hook object hooks onto a Feed::Out object and listens to a specific predefinedID. These are defined in the sensors.xml file.

12.5 Controller Thread

The Controller class provides a thread-safe interface for opening a TCP/IP network con-nection with a robot. The GUI operates the client side of this connection, while the robotis the server.

Each Controller object spawns a child thread when start() is called. This child thread,the sender, attempts to open a network connection to the given IP:port. If the connectionfails to open the Controller simply stops. If the connection is successfully opened the childthread spawns another child thread, the listener, that synchronously reads network dataand passes it on as Messages to the the sender for delivery to the relevant GUI component.

The sender thread operates a traditional message pump. It waits until a Message isplaced in the internal queue and then sends it on to the target defined in the receiver

field.

12.6 Opportunities for future improvement

The original intent was to use the Protocol Buffer Message class for network communi-cation as well. Because of time constraints this could not be implemented, but replacingthe current situation of weakly defined structs with a clearly defined message protocol




is the primary constraint preventing stable future growth of the network functionality.The GUI currently receives each special case of message type manually by copying therelevant fields via memcpy(). The primary function that protocolbuffers would provideis high quality serialization so transmission of data would be greatly simplified once it’simplemented on both the robot and GUI side. This in turn would allow further extensionof the message format.

The protocol buffer library suffers from a de-allocation bug on program end. The libraryattempts to de-allocate previously de-allocated memory and suffers from a segmentationfault. This happens after all other object destructors have been called and is becauseof this not a practical issue. It is still an ugly bug and fairly disheartening to end theprogram with a core dump error message and should be fixed.

The primary functional bug currently in the program is that the Controller child threadrefuses to die once started. The ’listener’ child thread performs a blocking read() oper-ation once open. While killing the ’sender’ child thread works as intended it .joins()

on the listener thread before returning. Because the listener thread performs a blockingnetwork operation this means that the program currently doesn’t stop once the GUI iskilled.

Once the listener bug has been fixed the Connect / Disconnect button needs to beupdated. The button currently only changes text depending on the connected state. Italso needs to actually call disconnect() on the Controller thread.

The map drawing function could be optimized. A redraw currently forces the entire mapto redraw but the Cairo library supports partial redraws. This could be implementedfairly easily and should reduce processor load quite a bit. There are also some expensivememory copy operations in the current code. These exist because the GUI internally usesMessages for communication but receives blocks of data from the network. Usage of theprotobuf serialization functions would help alleviate these issues.




13 Recommendations for future work

Aside from the suggestions associated with each separate module and program, there aresome further suggestions for future work regarding the overarching system and architec-ture. In this section various experiences and suggested changes will be discussed.

13.1 ROS

Robot Operating System is a framework that can be implemented on a Raspberry Pi.With ROS it is possible to make use of libraries such as Gmapping and ArUco. Whileimplementing ROS is not strictly necessary, and may result in unforeseen incompatibilitiesand issues, it could most likely expand on the functionality that future groups can use.

13.2 Parallelization

The modules in the main program written in MATLAB are currently compiled into asingle program and run in sequence. This means that if one module is slow the wholeprogram is slowed down. It may be advantageous to instead implement each moduleas a separate code generated function. These can then run in parallel as threads. Apreliminary version of this was written but not implemented since the Raspberry Pi iscapable of running the current program at the required frequency. However, if functionsthat require more processing power are implemented it may become necessary to switchto this parallel version.

13.3 Code generation

Code generation is a powerful tool that allows functions to be written in MATLAB andexecuted as C-code, which is especially useful when dealing with arrays. However, it isin many cases not as forgiving as MATLAB, and in this section the group will discussexperiences that may make the use of code generation easier.

It is the opinion of the group that allowing for variable sizing makes many implementationsmuch easier, but careful consideration of array sizes may work as well.

While this may be obvious to some, applying the function .ˆ2 on arrays makes programssubstantially slower and should be avoided at all costs. Multiplying the array with itselfelement wise is preferable.

Be careful when defining array sizes using input arguments. There may not be a warningabout conflicting array sizes during operations and the program may compile with strangebehaviours.




References

[1] Marcus Back. Technical Documentation. Invenire Periculosa. Dec. 11, 2014. url:http://www.isy.liu.se/edu/projekt/tsrt10/2014/bandvagn/Documents/

TechnicalDocumentation.pdf.

[2] Jacob Bernhard. iMap. Nov. 22, 2011. url: http://www.isy.liu.se/edu/projekt/tsrt10/2011/bandvagn/doc/Anvandarhandledning.pdf.

[3] Colin Flanagan. The Bresenham Line-Drawing Algorithm. url: https : / / www .

cs . helsinki . fi / group / goa / mallinnus / lines / bresenh . html (visited on11/22/2016).

[4] Oscar Horberg. Technical Documentation. Ross Haj. Dec. 7, 2015. url: http://www.isy.liu.se/edu/projekt/tsrt10/2015/bandvagn/TD.pdf.

[5] Emmeline Kemperyd. Teknisk Dokumentation Minrojningsbandvagn. Ostende Ab-scondita. Dec. 16, 2013. url: http://www.isy.liu.se/edu/projekt/tsrt10/2013/bandvagn/teknisk_dokumentation.pdf.

[6] Edwin Olsson. AprilTags. APRIL Laboratory, University of Michigan. Dec. 1, 2016.url: https://april.eecs.umich.edu/software/apriltag.html.



http://www.isy.liu.se/edu/projekt/tsrt10/2014/bandvagn/Documents/TechnicalDocumentation.pdf

http://www.isy.liu.se/edu/projekt/tsrt10/2014/bandvagn/Documents/TechnicalDocumentation.pdf

http://www.isy.liu.se/edu/projekt/tsrt10/2011/bandvagn/doc/Anvandarhandledning.pdf

http://www.isy.liu.se/edu/projekt/tsrt10/2011/bandvagn/doc/Anvandarhandledning.pdf

https://www.cs.helsinki.fi/group/goa/mallinnus/lines/bresenh.html

https://www.cs.helsinki.fi/group/goa/mallinnus/lines/bresenh.html

http://www.isy.liu.se/edu/projekt/tsrt10/2015/bandvagn/TD.pdf

http://www.isy.liu.se/edu/projekt/tsrt10/2015/bandvagn/TD.pdf

http://www.isy.liu.se/edu/projekt/tsrt10/2013/bandvagn/teknisk_dokumentation.pdf

http://www.isy.liu.se/edu/projekt/tsrt10/2013/bandvagn/teknisk_dokumentation.pdf

https://april.eecs.umich.edu/software/apriltag.html


A Deduction of probabilistic time updating algorithm

We are interested in the probability that there is an obstacle in a grid given all measure-ments in that grid, that is P (& = 1|Y1:t). Where & ∈ {0, 1} and denotes if the grid isoccupied (& = 1) or free (& = 0). Yt ∈ 0, 1 and denotes if the LIDAR detected an obstaclein the grid at time instance t (Yt = 1) or not (Yt = 0). The probabilities P (Yt = 1|& = 1),P (Yt = 1|& = 0), P (Yt = 0|& = 1), P (Yt = 0|& = 0) and P (& = 1|Y1) are constants.

P (&|Y1:t) = P (&|Y1:t−1, Yt) = / Using Bayes formula / =P (&, Y1:t−1, Yt)

P (Y1:t−1|Yt)P (Yt)= / Using B.f. /

=P (Y1:t−1|&, Yt)P (Yt|&)P (&)

P (Y1:t−1|Yt)P (Yt)= /P (Y1:t−1|&, Yt) = P (Y1:t−1|&)/

=P (Y1:t−1|&)P (Yt|&)P (&)

P (Y1:t−1|Yt)P (Yt)= / Using B.f / =

P (Y1:t−1,&)P (Yt|&)

P (Y1:t−1|Yt)P (Yt)

= / Using B.f / =P (&|Y1:t−1)P (Y1:t−1)P (Yt|&)

P (Y1:t−1|Yt)P (Yt)(29)

Let the variables that are independent of & be represented as α:

α =P (Y1:t−1)

P (Y1:t−1|Yt)P (Yt)(30)

We can now represent the probability that there is/is not an obstacle in the grid,using(29) and (30), as the following:

{P (& = 1|Y1:t−1, Yt)P (& = 0|Y1:t−1, Yt)

=

{αP (& = 1|Y1:t−1)P (Yt|& = 1)αP (& = 0|Y1:t−1)P (Yt|& = 0)

(31)

The sum of the probability of all possible outcomes should be 1:

P (& = 1|Y1:t−1, Yt) + P (& = 0|Y1:t−1, Yt) = 1 (32)

(31) and (32) yields the following result:

α =1

P (& = 1|Y1:t−1)P (Yt|& = 1) + P (& = 0|Y1:t−1)P (Yt|& = 0)(33)

(31) and (33) yields the final result:

P (& = 1|Y1:t−1, Yt) =P (& = 1|Y1:t−1)P (Yt|& = 1)

P (& = 1|Y1:t−1)P (Yt|& = 1) + P (& = 0|Y1:t−1)P (Yt|& = 0)(34)

