technical documentation minesweeper · technical documentation minesweeper version 0.4 editor: elin...

Technical DocumentationMinesweeper

Version 0.4

Editor: Elin NäsholmDate: December 17, 2017

Status

Reviewed ReviewerName Date1Approved ApproverName Date2

Course name: Automatic Control - Project Course Course code: TSRT10Project group: Smaugs E-mail: [email protected]: Minesweeper Document name: Technical Documentation

Minesweeper II

Project Identity

Linköping University, Department of Electrical Engineering (ISY)Autumn 2017

Client: Martin Lindfors, Linköping UniversityPhone: +46 13 28 13 65, E-mail: [email protected]

Customer: Torbjörn Crona, Saab DynamicsE-mail: [email protected]

Course Examiner: Daniel Axehill, Linköping UniversityPhone: +46 13 28 40 42, E-mail: [email protected]

Project Manager: Hampus AnderssonAdvisors: Per Boström-Rost, Linköping University

E-mail: [email protected] Ekelund, Saab DynamicsE-mail: [email protected] Reizenstein, Saab DynamicsE-mail: [email protected]

Group members

Name LiU-id Phone number ResponsibilityAndreas Hägglund andha796 072-713 38 70 Head of HardwareAndreas Lundgren andlu901 070-022 56 44 Head of DesignElin Näsholm elina044 073-094 94 03 Head of DocumentationFredrik Gustafsson fregu856 070 578 63 48 Head of Slam ImplementationFredrik Tormod freto995 073-775 05 36 Head of Control and Route PlanningHampus Andersson haman657 073-675 93 56 Project ManagerJonathan Jerner jonje173 070 296 61 50 Head of SoftwareMattias Andreasson matan461 070-822 53 67 Head of Testing

Group E-mail: [email protected]: http://www.isy.liu.se/edu/projekt/reglerteknik/2017/minesweeper/

Mail to each individual can be sent to “LiU-id”@student.liu.se.


http://www.isy.liu.se/edu/projekt/reglerteknik/2017/minesweeper/

Minesweeper III

Contents

1 Introduction 1

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

1.2 Definitions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

2 System Overview 4

3 Balrog 6

3.1 Funtionality . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6

3.2 Hardware . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

3.2.1 Computing Hardware . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

3.2.2 Sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

3.3 Software Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

3.4 Simultaneous Localization & Mapping (SLAM) . . . . . . . . . . . . . . . . . . . . . . . . 10

3.4.1 Pose-graph SLAM & OpenKarto . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

3.4.2 ROS Implementation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

3.5 Mine Detection . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15

3.6 Navigation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16

3.6.1 A* . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17

3.6.2 Map Processing . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20

3.6.3 Mapping . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23

3.6.4 Covering Tiles . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26

3.6.5 Disarming . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29

3.7 Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29

3.7.1 Reference Signals . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30

3.7.2 Main Part of the Controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30

3.7.3 Features in the Controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31


3.8 Communication . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32

3.8.1 Protocols . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32

3.9 Simulator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33

4 Sauron 36

4.1 Hardware . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36

4.1.1 Computing Hardware . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37

4.1.2 Sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37

4.1.3 Communication Hardware . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38

4.2 Communication . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38

4.3 Localization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39

4.3.1 AprilTags . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39


4.4 Track Moving Balrog . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42

4.4.1 Controller theory . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42

4.4.2 ROS implementation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44

4.5 Assumptions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 48

5 BaseStation 49


Minesweeper IV

5.1 Balrog GUI . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49

5.2 Sauron GUI . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50

6 Further Development 52

6.1 Balrog . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52

6.1.1 Improve the controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52

6.1.2 Tracking an AprilTag . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52

6.1.3 Filtering of Mine Detections . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52

6.1.4 Improving the Disarming Mode . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53

6.2 Sauron . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53

6.2.1 Processing and communication . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53

6.2.2 Controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54

6.2.3 GUI . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54

6.2.4 Detection and AprilTags . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54

6.2.5 Different detection system . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55

7 List of ROS Topics 56


Minesweeper V

Document History

Version Date Changes made Sign Reviewer0.1 10/12-17 First draft. - EN0.2 12/12-17 First revision - EN0.3 13/12-17 Second revision - EN0.4 17/12-17 Third revision - EN


Minesweeper 1

1 Introduction

This document is a technical report for the Minesweeper project in the course TSRT10:Automatic Control - Project Course at Linköping University. Minesweeper is a collabora-tion between Saab Dynamics and Linköping University that has been ongoing since 2009.The long-term project goal is to develop a fully autonomous minesweeping system that isable to detect landmines safely, efficiently and accurately.

The purpose of the project was to further develop the autonomous minesweeping system.The system was to be improved by implementing a SLAM (Simultaneous LocalizationAnd Mapping) algorithm utilizing a new rotating LiDAR sensor mounted on the groundvehicle (Balrog), for more accurate localization and mapping. The path planning andnavigation of Balrog were also to be improved for more efficient exploration of unknownareas. A UAV (Unmanned Ariel Vehicle) named Sauron was to be integrated in the systemto assist Balrog with its minesweeping tasks. Finally, an implementation based on ROS(the Robot Operating System) was to be investigated.

The purpose of the document is to provide an overview of the minesweeping system andto describe its subsystems and functionality in detail.

1.1 Project History

In order to better understand the project and the work described in this document, thissection is dedicated to explain the history of it.

It started in 2009 as a CDIO project and the name of the project was at the time“O’Hara’s”. At the time the platform was called “mobile scout”. Their main purposewas to define the system and work on control, network communication and navigation.

In the autumn the same year another group by the name “Carpe Locus” continued thework with “mobile scout”. They implemented GPS and automatic control for navigationbetween reference points. They also provided the platform with a remote control througha laptop.

The autumn of 2010 the work continued by the group called “8Yare”. They integratedan industrial PC into the system and ported all previous functionality to the new systemsetup. A Bumblebee2 BB2-08S2c stereo camera was also integrated and evaluated.

Next year the group “iMAP” continued the previous year’s work and used the robot’ssensors to construct a 3D map of the surrounding area.

In the autumn of 2012 the platform received its current name Balrog. This name wasgiven by the group “Minenmarker”. They where first with the functionality of makingsure an area is searched for mines. They where also first with “detecting mines”. Asidefrom this functionality the main performance upgrade was a more accurate positioning.

The following year’s work was a continuation of the work done by “Minenmarker”. By thename “Ostende Abscondita”, they refined the search algorithms and positioning.

“Invenire Periculosa” continued the work with Balrog in the autumn of 2014. This yearthey integrated a 360◦ laser sensor and a new more efficient industrial PC. Using this newhardware they further improved the positioning of the Balrog.

The autumn of 2015 a barometer and IMU was implemented to further improve thepositioning. The group went under the name “Ross Haj”. They also implemented a newsubsystem for signal processing and sensor diagnosis.

Last year, autumn of 2016, the group “TIGER” equipped Balrog with a high performancelaser and a camera. The camera is used to recognize apriltags which are used as reference


Minesweeper 2

points for positioning. They also implemented a fully autonomous mode with an improvedcontroller and navigation module.

This year’s project group, SMAUGS, improved mapping and positioning by using SLAM.This was made possible by mounting an Ubuntu computer on Balrog, using ROS andutilizing a new rotating LiDAR sensor provided by Saab. The navigation of Balrog wasimproved by separating the exploration of an unknown area into a mapping and a coveringphase. This is also the year when Sauron is introduced, a UAV which can autonomouslytrack Balrog. Finally, all new functionality was implemented using ROS which now ishighly integrated into the system.


Minesweeper 3

1.2 Definitions

AprilTags A matrix of bar codes used to identify predefined objects.Balrog The ground vehicle aimed for minesweeping used in the

project.EKF Extended Kalman Filter.GUI Graphical user interface.HAL Hardware abstraction layer.MAV-link Micro Air Vehicle Link.Mine An object or image on the ground acting as a mine.Object Any physical item in the testing area such as a mine, an

obstacle or a wall.Obstacle An object in the test area that acts as an obstacle for the

Balrog.ROS Robot Operating System. A set of open source software

libraries and tools enabling communication between differ-ent modules in robotic systems.

ROS nodes Units of code that can be run independently of each otherand communicate via ROS topics.

ROS topics Named data channels that ROS nodes can send messagesto by publishing on the topic, and read messages from bysubscribing on the topic.

Route A set of way-points created by the user or the platformitself.

Goal Node The final way-point in a given route.rqt Qt-based framework for GUI development in ROS.rviz Visualization tool for ROS [1].Sauron The drone used in the project for collaborating with the

ground vehicle.Search area The area that has not yet been searched for mines by Bal-

rog.Searched area The area that has been visited by Balrog.SLAM Simultaneous localization and mapping.TCP Transmission Control Protocol.Test area An indoor predefined area where the platform will be

tested.The project group The group of students involved in the project.UAV Unmanned Aerial Vehicle.UDP User Datagram Protocol.User The human interacting with the system in any way.PPM-signal Pulse position modulation. A way of transferring infor-

mation in binary. The number represents how many mi-croseconds the signal is in high state. Every high state issucceeded by 300 microseconds of low state. A value of1500 represents the middle position of every channel onthe remote controller, 1000 the lower limit and 2000 theupper limit.


Minesweeper 4

2 System Overview

The minesweeping system consists of three main subsystems: Balrog, Sauron andBaseStation. Balrog is a tracked robot equipped with various sensors and computinghardware, designed to autonomously map, cover and secure an assigned area on theground. Sauron is a UAV equipped with additional onboard computing hardware anda camera, designed to autonomously track Balrog in order to provide an aerial view ofthe assigned area. Both platforms can also be manually controlled by a human user viaRC hand controllers and they are both connected to BaseStation over WiFi. BaseStationis a computer running ROS that communicates with the platforms and displays relevantsensor data in separate GUIs. Balrog and Sauron can be seen together in Figure 1 below,whereas a schematic overview of the complete system is provided in Figure 2.

Figure 1: Sauron and Balrog, the two main components of the minesweeping system.

Figure 2: Schematic overview of the minesweeping system.


Minesweeper 5

Each subsystem does in turn consist of a number of software modules implementing dif-ferent key functionality.

Balrog consists of the following five software modules:

SLAM Estimation of the current Balrog position (localization)and map creation of the surrounding area including ob-stacles and visited areas (mapping).

Mine Detection Detection of mines represented by AprilTags.

Navigation Decision making and computation of paths to explore andcover the assigned area in an efficient way.

Control Path following.

Communication Communication between computing hardware.

Sauron consists of the following software modules:

Capture Capturing of images onboard Sauron.

Detection Detection of AprilTags in images.

Controller Computation of control signals according to data from thedetection module.

Communication Communicating the control signals to the onboard flightcontroller.

BaseStation consists of the following software modules:

Balrog GUI Visualization of the created map and various sensor data,including a live stream from the camera onboard Balrog.Reading of user input.

Sauron GUI Visualization of various data, including a live stream fromthe camera onboard Sauron. Reading of user input.

The Balrog and Sauron subsystems are designed to be deployed together in order tosecure an assigned area in the most efficient way possible. To simplify the developmentprocess and to ensure modularity, the subsystems are however designed to also functionindependently. In this mode, two separate base station computers can be utilized to runthe corresponding GUI software.


Minesweeper 6

3 Balrog

In this section, the Balrog subsystem is described in detail. Its functionality is described,a high-level hardware schematic is presented, its main hardware components are listedand implementation details are provided for its different software modules. An image ofBalrog is provided in Figure 3 below.

Figure 3: Balrog, the tracked robot.

3.1 Funtionality

Balrog has two modes: manual and autonomous. In manual mode, the user can controlBalrog using an RC hand controller, see the user manual [2] for details.

In autonomous mode, Balrog will map and cover all free regions of an area, while detectingany mines represented by AprilTags. Balrog will then visit each detected mine in orderto “disarm” it. The area is assumed to be located on a flat surface delimited by walls. Anexample of a typical test environment for which the autonomous mode is designed is seenin Figure 4 below.

In the autonomous mode, Balrog can be in three different states: mapping, covering ordisarming. When the autonomous mode is initialized, Balrog is in the mapping state. Inthis state, Balrog will explore and map the entire area. When the entire area is mapped,it switches to the covering state. In this state, Balrog follows a path in order to cover allfree regions of the mapped area and search it for mines. When the complete area has beensearched for mines, the state is switched to disarming. In this final state, Balrog visitseach mine that has been detected in mapping or covering for disarming, before finallyreturning to the starting position.

Details on the navigation algorithms utilized in the autonomous mode are provided inSection 3.6 below. Instructions for the practical usage of this mode is found in the usermanual [2].


Minesweeper 7

Figure 4: Typical test environment containing obstacles and AprilTags representing mines.

3.2 Hardware

The main hardware component of Balrog is its tracked robot platform with motors andspeed controllers, onto which all other components are mounted. The robot platform hasbeen developed in previous years’ projects and is considered as one integrated component.No further description of the tracked platform will thus be provided in this section; thefocus will instead be on Balrog’s sensor setup and computing hardware. An overview ofthe Balrog hardware is seen in Figure 5.

Figure 5: Schematic overview of the Balrog subsystem hardware.


Minesweeper 8

3.2.1 Computing Hardware

Of the hardware components seen in Figure 5 above, the following constitutes Balrog’sonboard computing capabilities:

Computer An Ubuntu computer running ROS placed on the plat-form to run all five Balrog software modules, to communi-cate with RPi and to communicate with BaseStation overWiFi. It receives wheel encoder data and a video feed fromRPi, and sensor data from LiDAR.

RPi A Raspberry Pi 3 model B+ that is the main communica-tion hub on Balrog. It communicates with Arduino-Int tosend control signals and read wheel encoder data. It re-ceives control signals from the RC controller via Arduino-Ext in manual mode and from Computer in autonomousmode. It also has a camera attached from which it for-wards a video feed to Computer.

Arduino-Int An Arduino that reads control signals from RPi and trans-forms these into motor commands which it sends to theplatform motors. It also reads data from the wheel en-coder sensors and forwards this to RPi.

Arduino-Ext An Arduino used for manual control of Balrog. It readsdata from the RC controller and forwards this to RPi.

3.2.2 Sensors

Of the hardware components seen in Figure 5, the following constitutes Balrog’s sensorsetup. Data for some sensors are taken from the Technical Documentation of the 2016Minesweeper project [3].

LiDAR A Scanse Sweep v1 360◦ scanning LiDAR mounted on topof the robot platform. A range sensor that functions bytransmitting a laser beam towards a target and detectingthe reflected light. The sensor has a maximum range of40 m, a rotation frequency of 5 Hz and a sample rate of1000 samples per second. It is used as the primary inputsensor for SLAM.

Wheel encoders Two E5 series rotational encoders mounted on each of thetwo front wheels that turns the tracks of the platform. Theencoder contains a circular disk with 500 marks and canthus theoretically detect movements of 1.2 mm or morefor wheels of diameter 0.18 m. The encoders are used toestimate the distance that each track has travelled, whichis utilized in the SLAM module.


Minesweeper 9

Camera A forward-facing Raspberry Pi Camera Module v2 that ismounted on Balrog and connected directly to RPi. It iscapable of capturing video with a resolution up to 1080p,and is used to capture live video used for AprilTag detec-tion and for streaming to BaseStation.

IMU An MTi 100 Inertial Measurement Unit mounted in thecenter of the robot platform. It communicates with RPivia USB and operates at a frequency of 50 Hz. It containsaccelerometers, gyroscopes and magnetometers for estima-tion of the platform acceleration and angular velocity inall three dimensions. It could potentially be utilized to im-prove SLAM, but was not found particularly useful in theprevious year’s project. The sensor is thus not currentlyused.

3.3 Software Overview

Balrog’s five software modules (SLAM, Mine Detection, Navigation, Control and Com-munication) have all been implemented by the project group using ROS, and they all runentirely on Computer.

All software running on RPi, Arduino-Int and Arduino-Ext was developed in previousyears’ projects. The RPi software has only been slightly modified by the project group,whereas the Arduino software has been left completely unmodified. RPi and the twoArduinos is thus considered a given system, which Computer communicates with via theCommunication module. For a detailed description of this given system, the reader isreferred to the 2016 Technical Documentation [3].

SLAM, Mine Detection, Control and Communication are all implemented directly as anumber of ROS nodes, described in the respective module subsections found below.

Navigation contains one ROS node named nav_covering_map, which is used for mapprocessing, see Section 3.6.2. The main navigation functionality is implemented as anumber of regular Python functions which are then utilized in the Python ROS nodemain.The main node implements the main autonomous behaviour of Balrog. It subscribes totopics outputted by SLAM and Mine Detection, contains a simple state machine formapping, covering and disarming, and computes suitable paths for Balrog to follow usingthe navigation functions. These paths are not sent directly to Control, but to a PythonROS node named coordinator. This node simply feeds Control with the positions alongthe path one by one. When Balrog has reached the end of a path, coordinator asks mainfor a new one.

A schematic overview of the software modules, including the communication links con-necting different modules and external hardware, is provided in Figure 6 below.


Minesweeper 10

Figure 6: Schematic overview of the software modules.

3.4 Simultaneous Localization & Mapping (SLAM)

The SLAM software module is responsible for estimating the current Balrog position andcreating a map of its surrounding area, including any obstacles. It is also responsible forkeeping track of which parts of the map that have been visited by Balrog. To do this, itutilizes the 360◦ rotating LiDAR that is connected to Computer, and the wheel encoderreadings that is transmitted to Computer from RPi. A schematic overview of the SLAMmodule is seen in Figure 7.

Figure 7: Schematic overview of the SLAM module.

A typical test environment is seen together with the corresponding map created by SLAMin Figure 8. Balrog was running autonomously along the yellow track to create the map.


Minesweeper 11

Figure 8: Typical test area (left) and the corresponding map outputted by SLAM (right).

The SLAM module is based on a ROS implementation of the OpenKarto SLAM systemavailable in the nav2d ROS package [4]. An overview of OpenKarto is thus providedin Section 3.4.1. Implementation details for the SLAM module, including a detaileddescription of all its ROS nodes, are then provided in Section 3.4.2.

3.4.1 Pose-graph SLAM & OpenKarto

OpenKarto is an example of an optimization based SLAM approach usually referred to aspose-graph SLAM [5]. In this approach, one extracts relative spatial constraints betweendifferent robot poses from the available sensor data, treats the history of poses as variablesof an optimization problem and then tries to find the pose history that best fits all poseconstraints.

Relative pose constraints are extracted from odometry measurements between consecutiveposes, and from matching pairs of range scans. With an estimate of the pose trajectory,one can build an occupancy grid map of the environment by projecting each range scaninto the world frame.

One key concept in this approach is that of loop closures. A loop closure is when a mobilerobot is able to detect that it has re-entered a previously visited area, enabling the creationof relative pose constraints between temporally separated poses. This in turn can greatlyimprove the mapping quality by correcting drift, for instance connecting the endpoints ofa circular corridor.

More formally, the approach corresponds to creating a graph consisting of a set of nodesand a set of edges between pairs of nodes. Each node corresponds to a robot pose at whicha range scan was taken, whereas each edge corresponds to a derived measurement of therelative pose between the pair of corresponding robot poses. A minimization problemover the robot poses is then defined from the graph by constructing an objective functionwhere each relative pose measurement is translated into an error term.

One usually decouples this problem into two separate tasks: creating the graph from theavailable sensor data (graph construction), and determining the most likely configurationof the nodes given the edges of the graph (graph optimization). The system dealing withgraph construction is called the SLAM front-end and its design depends on the specificset of sensors being used. The optimization system on the other hand is referred to as theSLAM back-end and deals with a general data representation which is sensor agnostic.

For its SLAM front-end, OpenKarto utilizes the work presented in [6], which is a methodfor matching 2D LiDAR scans using a probabilistic framework and cross-correlation.

To efficiently perform the graph optimization in the SLAM back-end, the most com-


Minesweeper 12

mon method is to use techniques for nonlinear least-squares optimization, such as Gauss-Newton and Levenberg-Marquardt. These methods consist in iteratively improving aninitial guess by solving a large linear system obtained from linearization of the originalobjective function. An open source implementation of this approach is presented in [7],which constructs the linearized system in an efficient way and utilizes sparse Choleskydecomposition to compute its solution. This implementation is utilized in OpenKarto’sback-end system.

Compared to commonly used filter-based SLAM methods such as GMapping [8], it isnot obvious which approach that generally will provide the best mapping quality. Forour specific use case OpenKarto does however have one key advantage, which is that itexplicitly outputs an estimate of the entire pose history and not only the current robotpose. This enables us to keep track of which parts of the map that have been visited byBalrog, even if the map were to change significantly during operation.

3.4.2 ROS Implementation

The SLAM module consists of five different ROS nodes:

• sweep_node

• slam_odom

• mapper

• slam_pose

• slam_visited

The nodes sweep_node and mapper are members of external ROS packages and have notbeen modified in the project, whereas slam_odom, slam_pose and slam_visited have beenimplemented from scratch in C++. A detailed description of each of these five nodes isfound in the respective paragraph below. A schematic overview of the nodes is seen inFigure 9.

Figure 9: Schematic overview of the five SLAM ROS nodes.

sweep_nodeThis node is a part of the external ROS package sweep-ros, which is officially supportedby the LiDAR manufacturer [9]. The node reads data from the LiDAR and publishesmessages of type sensor_msgs:LaserScan on the topic /scan. Each published messagerepresents one complete 360◦ scan of the sensor and contains both the raw range mea-surements and various metadata. A schematic overview of the node is seen in Figure 10.


Minesweeper 13

Figure 10: Schematic overview of sweep_node.

slam_odomThe node reads messages of type std_msgs:Float64MultiArray from the ROS topic/encoder_data. Each read message contains the most recent measurements of the wheelencoders connected to RPi. The node transforms these measurements into the odometrymeasurement format expected by the SLAM node. The encoder data from RPi is receivedas the angle that each wheel powering the tracks has rotated since the last measurement,∆θ, with index left and right for each track.

Many different approaches to transform encoder readings into a robot pose exist. Thechosen method is a compromise on accuracy and complexity. The chosen algorithm,as well as a more accurate one, have been evaluated in [10]. With a sampling time of0.02 seconds for sensor data, the chosen methods present good results and are easier toimplement.

Equations (1) - (2) describes how to get the change in distance, ∆s for each track, giventhe wheel radius rw. The model then estimates the change in angle and distance traveled,(3) - (4). The numerical constant b is the track width, i.e. the distance between thetracks. Using (5) - (7), a translation to change in global coordinates and angle is achievedand given the previous pose, the current pose is estimated.

∆sleft = ∆θleft · rw (1)∆sright = ∆θright · rw (2)

∆θ =∆sleft −∆sright

b(3)

∆s =∆sleft + ∆sright

2(4)

xt+1 = xt + ∆s cos(θ +∆θ

2) (5)

yt+1 = yt + ∆s sin(θ +∆θ

2) (6)

θt+1 = θt + ∆θ (7)

The estimated pose (x, y, θ) is published as a message of type std_msgs:Float64MultiArrayon the topic /odom_pose, which is used for debugging purposes. It is also broadcasted asa transform from the /odom coordinate frame to the /base_footprint coordinate frame,where /base_footprint is the coordinate frame whose origin is located in the center ofBalrog. This transform is what the SLAM node takes as input. A schematic overview ofthe node is seen in Figure 11.


Minesweeper 14

Figure 11: Schematic overview of slam_odom.

mapperThis node is a ROS implementation of OpenKarto, available in the nav2d ROS pack-age [4]. It takes as input LiDAR range scans read from the /scan topic and odometrymeasurements extracted from the /odom to /base_footprint transform. It outputs anestimate of the current robot pose (x, y, θ) by broadcasting a transform from the /mapcoordinate frame to the /odom coordinate frame, which corrects the transform from /odomto /base_footprint outputted by slam_odom. The actual robot pose estimate (x, y, θ) isthen obtained as the transform from /map to /base_footprint. It also outputs sampledpoints along the robot’s estimated pose trajectory which it publishes as a message of typevisualization_msgs:Marker on the /Mapper/vertices topic, and a map of the robot’s sur-rounding area in the form of an occupancy grid map. This map describes the area bydividing it into 5 cm by 5 cm grid cells and assigning each cell a value q ∈ {−1, 0, 100},where the different values of q are defined according to:.

• q = -1: Unknown cell.

• q = 0: Free cell.

• q = 100: Obstacle cell.

The map is published as a nav_msgs:OccupancyGrid message on the topic /map, togetherwith various map metadata such as the map width and height. A schematic overview ofthe node is seen in Figure 12.

Figure 12: Schematic overview of mapper.

slam_poseThe node extracts the robot pose estimate (x, y, θ) outputted by mapper by looking up thetransform from the /map coordinate frame to the /base_footprint coordinate frame, andpublishes this information as a message of type std_msgs:Float64MultiArray on the topic/estimated_pose. The node functionality is not strictly necessary since the current poseestimate always is available in implicit form as a transform between coordinate frames. Tohave this estimate available on a separate topic does however simplify the communicationwith external modules. A schematic overview of the node is seen in Figure 13.


Minesweeper 15

Figure 13: Schematic overview of slam_pose.

slam_visitedThis node reads messages from the /map and /Mapper/vertices topics. Based on thesampled pose trajectory read from /Mapper/vertices and the robot platform’s knownphysical dimensions, it computes which grid cells that at some point have been withina certain distance of the robot. These cells are set to being visited cells by assigning avalue of −2 in map_visited, which is a map of the same size and resolution as the oneoutputted by the SLAM node. All other cells in map_visited have the same value as inthe map read from /map. The resulting map_visited is published as a message of typenav_msgs:OccupancyGrid on the /map_visited topic. A schematic overview of the nodeis seen in Figure 14.

Figure 14: Schematic overview of slam_visited.

3.5 Mine Detection

The mine detection module is responsible for detecting mines represented by AprilTags,which are detected using Balrog’s onboard camera. A typical test environment containingmines is seen in Figure 16 (left) below. For a detailed description of the AprilTag system,see [11] and [12]. A schematic overview of the module is seen in Figure 15.

Figure 15: Schematic overview of the mine detection module.

A video feed is streamed from RPi to Computer via Ethernet. On Computer, the PythonROS node stream_to_ROS is used to read the video feed and publish the video frameson the topic /balrog_camera/image_raw. It also publishes camera info and calibrationparameters on the topic /balrog_camera/camera_info.

The node republish on Computer is then used to republish the video frames in a compressedformat on the /balrog_camera/image_raw/compressed topic. The republish node is partof the external ROS package image_transport [13].

Finally, the node apriltag_detector_node on Computer is used to detectany AprilTags present in the video frames. The node is part of the external packageapriltags_ros [14] and subscribes to the /balrog_camera/image_raw/compressed and


Minesweeper 16

/balrog_camera/camera_info topics. The node outputs a video feed in which detectedtags are marked with their ID, see Figure 16 (right). It also outputs an estimate of the 3Dposition and orientation of the tag relative to the camera. Since the camera has a knownoffset with respect to the center of Balrog, an estimate of the tag position (xtag, ytag) inthe map coordinate frame is obtained. Saving this estimated tag position every time atag is detected thus enables Balrog to visit each detected tag at any time. This is exactlywhat is done when Balrog enters the disarming state in its autonomous mode.

Figure 16: Left: Test area containing mines. Right: Visualization of a mine detection.

Currently, no form of filtering is applied to the tag detections. The estimated position(xtag, ytag) is saved in an associative container indexed by tag ID, and is simply overwrittenevery time a new detection occurs. This solution was found to provide good enoughaccuracy for the simple current use case, but it is definitely a reasonable candidate forfurther development.

3.6 Navigation

The navigation module is used in Balrog’s autonomous mode, it determines where Balrogshould go and outputs a goal position to the control module. As described in Section 3.3,it consists of the ROS nodes nav_covering_map, main and coordinator, together witha number of Python functions utilized in main. A schematic overview of the module isprovided in Figure 17.

Figure 17: Schematic overview of the navigation module.

Different algorithms are used in the three different autonomous modes described in Sec-tion 3.1, which are:


Minesweeper 17

1. Mapping

2. Covering

3. Disarming

The algorithms for these modes are described in detail in Section 3.6.3 - 3.6.5 below. Inall three modes, an implementation of the A* algorithm is used. A description of A* isthus provided in Section 3.6.1. These modes run sequentially starting with Mapping modewhich leads to Covering mode and ends with Disarming mode.

The navigation module is written entirely in Python. The output of all modes are twopaths; one which contains all nodes along the path, and one filtered path which only savesthe nodes where a new direction is taken. The filtered path is the one outputted to thecoordinator.

3.6.1 A*

A* is used when getting from point A to point B in a static environment, see Figure 18.The algorithm keeps track of how far away each node is from the starting positions, aswell as how far away it is from the goal node.

Figure 18: A* finding the way among obstacles (marked in black). Starting in the leftbottom corner it generates a path (green) to the goal node (blue).

The A* algorithm is based on the f-cost of each node which is calculated as:

fCost = gCost+ hCost (8)

Each node is assigned a static h-cost, which in this case is the euclidean distance from thenode to the goal. This is used as an heuristic to indicate the length of the path betweenthem. The g-cost on the other hand is updated in every loop and is a way to keep trackof how far the node is from the start node. Step by step, starting with the start node,the adjacent nodes gets a cost. For all horizontal/vertical movements the cost is increasedwith 1 and for diagonal movements with

√2. The f-cost is thus an estimate of the total

path length. Globally, the node with the lowest cost will then be chosen as the new one,


Minesweeper 18

which gives a cost to its neighbours.

The node for which the cost is based on is then stored in a matrix, this can be updated ifduring a later iteration it is discovered a cheaper way to get there. In the list below thereare some names described which are used in the code and block diagram.

• checkedMap (often called closed set or list), all nodes which has been found as theminimum and has given the adjacent nodes a g-value

• fValueMap (often called open set or list), all the nodes with a f-cost that can besearched for minimum value

• fromMap, shows from which x- and y-coordinate each node has got their cost basedon

• currentNode , the current minimum value node that is added to the checkedMapmatrix

When the goal node is found, by walking back step by step, looking at which node thatwas the previous one, the path can be reconstructed. A flow chart of the algorithm withthe backtracking of path is seen in Figure 19 and 20.


Minesweeper 19

Figure 19: The A* algorithm.


Minesweeper 20

Figure 20: Chart to find the path for the A* algorithm.

3.6.2 Map Processing

The map from SLAM is processed to reduce the resolution and add a safety margin aroundobstacles. Map processing is also done within the algorithms for navigation, however isimplemented as a separate ROS-node for covering tiles mode to keep the functionalityseparated and generate a map to be presented in rviz.

A resolution of 5 centimeters and a map with a size of 5x5 meters requires 10 000 cells.During covering mode a optimal path is calculated at start, which would be very compu-tationally heavy and might result in a route that includes a lot of turning due to the largenumber of cells to visit. Balrog is estimated to cover a square of 90 centimeters arounditself, which implies that a map resolution of 90 centimeters would be favorable to justnavigate between cells that has not been covered. That gives problems both in decidingwhat kind each cell will be and require a very large test area to prevent almost all cellsfrom being marked as obstacles. A value of 30 cm is decided to be a good choice, whichplaces Balrog in the middle of an area of 3 by 3 nodes, Figure 30.

To present what kind of cell there is, the following number are used in the matrices andthe corresponding colour in rviz in parenthesis:

-2 Cell visited (yellow)

-1 Cell not known (light grey)

0 Cell free of obstacle (not visited) (white)

70 Cell within the safety margin from an obstacle (dark grey)

100 Cell with an obstacle (black)

The map processing is done in Python as a ROS node named nav_covering_map thatsubscribes to the topic /map_visited, described in the SLAM section and seen in Figure 21.


Minesweeper 21

Figure 21: map_visited presented in rviz.

The script is run each time a new map_visited is received and publishes two maps asOccupancyGrids, /map_covering_large with a resolution of 15 cm and /map_coveringwith 30cm in resolution. For both maps the obstacles are expanded and the event ofremoving rows or columns when merging cells handled. All steps are described more indetail below.

First step of map processing

In the first, /map_visited is received as a OccupancyGrid object and transformed to anumpy array to be represented as a matrix with rows and columns. A new matrix is createdwhere the number of rows and columns is divided by a constant; NR_OF_CELLS, in ourcase 3. The size of the map is rounded down to a integer, where the case of potential lossof cells is handled later. A cell in the new map is called box, that will represent the datafrom the original 9 cells, Figure 22.

Figure 22: The first step of map processing where a 3 by 3 area is turned into one cell,described as box in the code. A black cell represents an obstacle, light grey not explored,yellow visited and white not visited.

Every cell to be part of the box is looped through and its data determines what kind ofcell the box shall be. The following priority order is used, where obstacles have the highestpriority, meaning that if an obstacle is found the box will be an obstacle no matter whatthe the other cells are:

1. Obstacle


Minesweeper 22

2. Visited

3. Not visited

4. Not explored

A simple filter is applied that counts the number cells with obstacles and not visited cells.Only if the number of a certain kind exceeds a threshold, the box is possible to set to thatkind. The result of this can be seen in Figure 24, though together with the expansion ofobstacles described below.

Obstacle expansion

In order to navigate on the map, all obstacles needs to be expanded since the coordinatecenter is placed in the middle of Balrog and cannot be too close to an obstacle. All cellswith an obstacle are looped through and expanded by 2 cells in all directions if possible,Figure 23. An expanded obstacle is represented by the number 70, which is displayed asdark grey in rviz, Figure 24.

Figure 23: How the expansion of an obstacle is done. The black cell is an obstacle andthe surrounding cells in dark grey indicate the safety margin.

Figure 24: The result from the first step of map processing and obstacle expansion. Ablack cell represents an obstacle, dark grey expanded obstacle, light grey not explored,yellow visited and white not visited. Published as /map_covering_large.

Second step


Minesweeper 23

In this step the resolution is increased by a factor 2. Since we used 15 cm in the firststep, the final map will have a resolution of 30 cm. For this step no filters are used anda slightly different priority order to avoid setting cells as visited that contain not visitedcells to ensure that all parts of the map will be covered:

1. Obstacle

2. Expanded obstacle

3. Not visited

4. Visited

5. Not explored

A map from the step above with odd number of columns or rows is handled. The re-sult can be seen in Figure 25 and the matrix is flattened before being published as anOccupancyGrid on the topic /map_covering.

Figure 25: The final map from the processing. A black cell represents an obstacle, darkgrey expanded obstacle, light grey not explored, yellow visited and white not visited.Published as /map_covering.

3.6.3 Mapping

Mapping is the first mode that the Balrog enters into when starting. Within the mappingmodule there is a predefined maximum size of the area set by: xmin, xmax, ymin, ymax.When starting, everything in the predefined area is unknown. To navigate in this condi-tion the goal node is set by an algorithm frontier based exploration [15], see the separateparagraph below. In simple terms the frontier sets the goal node to be the closest unex-plored node by euclidean distance. In order to navigate to this goal node A* algorithm isused which generates a path of way-points. Balrog remains in this mode until the frontierbased exploration can no longer find a goal node, view Figure 26 for an example map.Which in turns means that there are no more unexplored nodes in the area. At the pointwhen the area is fully mapped Balrog moves into next phase: Covering tiles.


Minesweeper 24

Figure 26: Map from rviz when running mapping mode. Yellow area denotes where Balroghas been. Black dots represent obstacles, white area represents empty area and gray areaunknown.

Frontier based explorationFrontier is an algorithm which defines a goal node for Balrog. The purpose is to alwaysreach for an unknown area and therefore map an area in an efficient manner. In theimplementation the frontier based exploration algorithm feeds the goal node to the A*algorithm. The definition of a frontier node is every known node that is neighbouring aunknown node. All frontier nodes are gathered in a vector. From this vector the frontiernode which has the least euclidean distance to the current position is chosen as goal node.

As mentioned in Section 3.6.2, the map created by SLAM is discretized where each ele-ment in the map-matrix corresponds to a real world area and characteristic. For instancean unknown area is denoted by the value -1 and a free area is represented by the value0. Since the map is created in such a way, finding frontier nodes can easily be quantifiedby finding all zeros neighbouring -1. This problem can be solved in an efficient mannerusing matrix operations. Since the map created by SLAM has the potential of becomingvery large, it is important to avoid for-loops since the algorithm should be running in realtime. By using matrix operations the solution becomes scalable for very large maps andtherefore is applicable for running in real time. Figure 27 shows the structure of frontierbased exploration.


Minesweeper 25

Figure 27: Flowchart over frontier based exploration algorithm.

The purpose of block #2 is to create a slamMapNoObstacles containing only known andunknown nodes. This is done by converting all elements to either -1 or 0. More specifi-cally obstacles (represented by 100) are converted -1. Covered area (represented by 1) isconverted to zero. The purpose of doing it is to be able to find all frontiers even thosewho are adjacent to obstacles. Block #3 makes four copies of slamMapNoObstacles andexpands them. For instance if the map created by SLAM consist of 16x16 nodes thenthese expanded maps will be 17x17. Where the extra row and column consists of thevalue NaN , which doesn’t have a real world interpretation. The only difference betweenthe four tempMatrix is were NaN values are implemented.

These expansions are created in order to find neighbouring frontiers in all eight directionsfor every node. Once these matrices are created block #4 performs the matrix operationstempMatrix2 − tempMatrix1 and tempMatrix3 − tempMatrix4. The result of theseoperations will give frontier nodes. Any frontier node will be created by either 0−(−1) = 1or −1 − 0 = −1. This is intuitive since these operations indicates if an known node isadjacent to an unknown node. So in the resulting matrix from these operations 1 and-1 correspond to a frontier node which is stored in a matrix frontierMap. Block #6simply loops through 30 nodes (30*0.05 = 1.5 m) in each direction from current positionand removes any frontiers in this area. This is done in order to achieve a smootherbehavior for Balrog. Block #7 removes any frontier in frontierMap which corresponds toan obstacle in the real world environment. Block #10 finds the closest frontier node andthis is quantified as shown in Equation 9:


Minesweeper 26

arg min

N∑i

(x− frontierXi)2 + (y − frontierYi)2 (9)

were frontierXi and frontierYi are the coordinates of each frontier.

3.6.4 Covering Tiles

Covering tiles is the next mode which Balrog enters after having mapped the entire area.A path is generated only once, when starting the mode. At this point the map might looklike in Figure 28 with a big covered (yellow) area with some uncovered (light grey) areaat the edges. Firstly some changes is made to the map, where the 0.05x0.05 m tiles arelumped together to 0.3x0.3 m tiles. If a majority of them are an obstacle it all would beconsidered as one. If not as many, the node are probably only safety distances and stillokay to pass. See Section 3.6.2 for more information.

Figure 28: Map from rviz when running covering tiles mode. Yellow area representscovered area and white area uncovered.

Cost FunctionThe algorithm for covering all the tiles is developed from the paper [16]. The algorithmis based on two maps. The first one gives a cost depending on how close the node is toa chosen goal node, see Figure 29. It is not the euclidean distance that is used but theactual path length. The goal node was chosen as the original start node, this making ableto pick up Balrog close to where you left it when on minesweeping mission.


Minesweeper 27

In the other map the nodes get values depending on how near each node is to an obstacle.(A high value is given to ones next to an object.) These two are weighted togetheraccording to:

nodeCost = distToGoal + α · distToObstacle (10)

where α is a tuning parameter. After some simulations and real testing α was set to 0.9for a good behaviour not making too many turns.

Figure 29: The weighted maps for covering tiles, goal node marked in red and start nodein blue.

When starting, all the nodes around the start node, in total 8 tiles, marked with yellow inFigure 30 are set as visited in a map called visitedMap. All nodes visited in earlier modeswill already be stored here. The blue ones in the same figure is then examined to findthe node with highest nodeCost which is unvisited and is not an obstacle. This node isthereby the first node in the path and is stored in a path-matrix.

Figure 30: Nodes round the current one seen as visited and searchable.

The same procedure is repeated until all tiles has been visited. Step by step a new highestcost node is found adjacent to the latest node in the path. If all the adjacent nodes havebeen visited the closest free node is chosen, looking at the full map, based on euclideandistance. When going to the closest node A* is used for getting a path. For flowchart seeFigure 31.

It was also considered not only to go the closest node when all nodes in the area alreadyhad been searched, but also to look at the cost of the node. This did not yield animprovement and was not used in the final setting.


Minesweeper 28

Figure 31: Covering tiles algorithm.


Minesweeper 29

3.6.5 Disarming

During mapping and covering mode the front facing RPi camera looks for AprilTags whichrepresent mines. The AprilTags that are detected are visited in Disarming mode in orderto “disarm” the mines. This mode has a lot of potential for further development. Thereis no optimized or sub-optimized algorithm which chooses what mine to disarm in whatorder. Disarming simply uses A* to go to each mine in an arbitrary order.

3.7 Control

The control module is designed to take Balrog to a goal coordinate as accurately andsmooth as possible. The controller is based on the one from previous year in Matlab andimplemented as a ROS node named controller. Input is the current pose of Balrog pub-lished by the SLAM module on /estimated_pose and the current goal position publishedon /goal_pos either automatically by coordinator or manually by manual_goal_pos. Aschematic overview of the module is provided in Figure 32. How the coordinate system isdefined can be seen in Figure 33.

Figure 32: Schematic overview of the control module.

Figure 33: The coordinate system and how the angle of Balrog is defined.

The controller consists of two parts, one for linear velocity and one for angular velocity,that are combined to give the control signal to the motor of each track, see Figure 34 foran illustration. The output from the controller is a Float64MultiArray with the controlsignals for the left and right track, being read by the RPI and sent to the speed controllersvia the internal Arduino.


Minesweeper 30

Figure 34: The definition of velocities which are the reference signals to the regulator.

3.7.1 Reference Signals

The two parts of the controller need reference signals. For the linear velocity the referencesignal is a simple euclidean distance between the current position of Balrog and the goalposition. This value is also what determines if the goal position is reached by comparingto the TOLERANCE parameter. Currently a tolerance of 7 cm is used where a smallervalue might cause Balrog to miss its target and thus needs to rotate heavily to reach thatposition.

The controller for the angular velocity acts on the angular error between Balrog and astraight line towards the goal position. Since a regular arctan only is defined in the range−π/2 to π/2 and Balrog can rotate in any direction, arctan2 is used. arctan2 is definedin all four quadrants and gives an angle error no greater than 180 degrees causing Balrogto always rotate in the direction that is closest to the desired angle.

3.7.2 Main Part of the Controller

The controller is a P-regulator with a few features to give Balrog a behaviour that isdesired. Balrog is controlled in a way best described as “turn first, then move”. Be-fore moving forward the angle error has to be lower than a value set in the code; AN-GLE_ERROR_THRESHOLD. When the error is lower than that both the controller forlinear and angular velocity are active.

The proportional gain, noted as K in the code, can be changed for both regulators. Whenthe reference signals are large due to Balrog being far away from its goal, the referencevalues on the velocities are saturated. These are important parameters to control themaximum speed when driving forward and angular velocity when rotating. A more ag-gressive angular controller is used when driving forward to steer Balrog towards its goalposition.

Velocity to Control Signal

The code on the RPI was modified to simply read the output from the controller andforward that to the internal Arduino. The code on the internal Arduino was not modifiedat all, which set the requirement on the output from the controller.

The equations for odometry, presented in the slam_odom paragraph of Section 3.4.2,are solved for angular velocity of each track. Equation (11) - (12) is from the Technical


Minesweeper 31

Documentation from previous year[3].

ωleft =2 · vlinear − ωangular · b

2 · rw(11)

ωright =2 · vlinear + ωangular · b

2 · rw(12)

That velocity is scaled to give the control signal.

control_signalleft = ωleft · SCALING (13)control_signalright = ωright · SCALING (14)

A scaling factor of ten has been found to give a good control signal.

3.7.3 Features in the Controller

A simple acceleration limiter is introduced for both linear and angular velocity. The usercan select how many iterations it should take for Balrog to reach its maximum velocity.This year the controller was run at 10 Hz and the parameter CYCLES_TO_MAX_VELOCITYset to 7, meaning the velocity being ramped from zero to maximum in 0.7 seconds.

Since Balrog is intended to operate rather slowly, the control signal is saturated if theabsolute value exceeds a certain threshold.

control_signal ∈ [−0.4, 0.4] (15)

The control signal should during normal usage never reach this threshold, however is avaluable safety precaution to prevent any unexpected behaviour.

It was noted that with a low reference signal, Balrog did not move with the linear regulatorfrom reference to control signal. The control signal is correlated to the pulse width to thespeed controllers and thus the power to the motors. Due to friction, there is a minimumpower required to turn the tracks, meaning a dead band in the control signal. This wasidentified to be in the range at around [-0.15, 0.15]. If the following was true for anycontrol signal:

|control_signal| ∈]0, 0.16] (16)

The signal was set to 0.16 (with correct sign) to that motor before being published onthe topic. Another solution to this problem is to increase all control signals with 0.16 tokeep the linear behaviour in the regulator. Though when this problem was identified, allparameters in the controller had been trimmed and creating a non-linear regulator wasdecided as a the most simple and quickest solution.


The functionality described above is implemented as a ROS node written in python. Torun the controller a string “start” has to be published once on the topic /start_topic. Ifanything is written on the topic /node_status, Balrog is stopped and is started once a


Minesweeper 32

new goal position is received. During every loop of the script a lot of data is printed tothe terminal, including position, angle, velocity and control signal. The control signal ispublished on the topic /control_signals.

The maximum linear velocity-parameter can be updated by publishing a new value onthe topic /change_velocity as a string. Currently a value of 0.3 is used with good results.If a higher value than 0.3 is desired, the saturation, Equation (15), might need to accepthigher control signals for good results.

3.8 Communication

The communication module is responsible for the communication between Computer andRPi, and consists of the C++ ROS node communicator on Computer. Computer andRPi communicates via Ethernet using a TCP socket, with RPi acting as a server andComputer acting as a client.

The server functionality on RPi was implemented in the previous year’s project and hasonly been slightly modified by the current project group in order to adapt the commu-nication protocol. No implementation details for the RPi side of the communicationlink is thus provided in this section. For a detailed description, see the 2016 TechnicalDocumentation [3].

communicator connects to the TCP socket opened by RPi. It subscribes to the/control_signals topic, transforms these messages into the protocol format and transmitsthe data to RPi. It receives the most recent measurements of the two wheel encoders fromthe TCP socket and publishes this information as a message of typestd_msgs:Float64MultiArray on the /encoder_data topic. A schematic overview of com-municator and its interface is seen in Figure 35.

Figure 35: Schematic overview of communicator and its interface.

3.8.1 Protocols

Every message that is sent from RPi to communicator begins with a message type de-scriptor in the form of a double, followed by three doubles containing data. Every singlemessage thus contains four doubles, which gives a fixed message size of 32 bytes. The pro-tocol from last year allowed variable sized messages, but this was found to cause frequentcommunication errors.

RPi can only send two kind of messages to communicator. The first one is a ping messagewhich only contains void data. The second one is sensor data which contains the latestmeasurement of the right and left wheel encoders, and a checksum of the two values. Thereceived measurements are considered valid only if the received checksum matches thecomputed sum of the received measurements. The communication protocol from RPi tocommunicator is summarized in Table 3 below.


Minesweeper 33

Table 3: Defined message types in the communication protocol from RPi to communicator.

Descriptor(double)

Type Content

0 Ping double 1-3: 0xffffffffffffffff1 Sensor data double 1: odoRight

double 2: odoLeftdouble 3: checksum

RPi can only receive control signals, i.e. the commanded angular velocity of the left andright wheels, from communicator. Since the protocol from last year only allowed RPi toreceive messages of exactly six doubles, extra safety checks were added to the protocolfrom communicator to RPi.

Every message sent to RPi contains the commanded angular velocity of the left andright wheels, three error check values and a checksum. The received control signals areconsidered valid only if the received error check values match their known true values,and the received checksum matches the computed sum. The exact form of the message isdefined in Table 4 below.

Table 4: Definition of the message sent from communicator to RPi.

Contentdouble 1: omega_ldouble 2: omega_rdouble 3: 10double 4: 20double 5: 30double 6: checksum

3.9 Simulator

Essentially all of Balrog’s functionality can also be tested in a simulator developed bythe project group. The simulator is a custom Gazebo [17] environment utilizing a modelof the popular TurtleBot2 [18] robot, whose motion model is similar to Balrog’s. Therobot model has also been equipped with a simulated rotating LiDAR with specificationschosen to roughly match those of the LiDAR mounted on Balrog, including its accuracyand precision. The developed simulation environment can be seen in Figure 36.


Minesweeper 34

Figure 36: The developed simulation environment.

Gazebo is an open source robot simulator which is tightly integrated with ROS. Thesimulator can thus be used to test Balrog both in manual and autonomous mode. Inmanual mode, the simulated robot can be controlled using the keyboard in order to e.g.evaluate the SLAM module. LiDAR and odometry data is then provided directly by thesimulator, thus making the sweep_node and slam_odom nodes redundant, but all otherSLAM nodes are run just like in a real life environment.

In autonomous mode, the complete mission including all three states (mapping, coveringand disarming) can be tested. The simulation environment even contains AprilTags whichare detected in a virtual video feed provided by the simulator. A simplified controller isused in the simulator since the TurtleBot model is controlled directly with commandedangular and linear velocity.

The visualization tool rviz, utilized in the Balrog GUI (see Section 5.1) can also be usedto display e.g. data from the simulated LiDAR and the maps created by the SLAMmodule. An example of what it looks like when Balrog’s autonomous mode is tested inthe simulator is seen in Figure 37.


Minesweeper 35

Figure 37: Balrog’s autonomous mode is tested in the simulator.

The simulator has been a useful tool in the development process, especially for testingthe different navigation algorithms used in Balrog’s autonomous mode. Those algorithmswere first developed in Matlab using very simple test environments. They were thenported to Python and tested in the simulator, and finally tested on the physical robot.In this process, the simulator was found to be surprisingly realistic. Making the Matlabalgorithms function properly in the simulator required a number of iterations and a fairamount of work. Once the algorithms worked well in the simulator, however, it was quitestraightforward to get them to work in the real world as well.

The test environment seen in Figure 36 above is a simple 4 by 4 meter square containingthree obstacles, but it is highly customizable. The square can be made bigger, more obsta-cles can be added and their size and shape can be modified. Setting up more complicatedtest environments should thus be straightforward.

Detailed instructions on how to install and use the simulator are found in the user man-ual [2].


Minesweeper 36

4 Sauron

In this section Sauron’s subsystems are described in detail. A high-level hardware schematicis presented, its main hardware components are listed and the implementation of the soft-ware module is described in detail. An image of Sauron is provided in Figure 38 below.

Figure 38: Sauron, the UAV.

Sauron is a commercial UAV acquired by Saab that was integrated in to the minesweepingsystem. There were two main objectives that were prioritized when integrating Sauron into the system.

• Sauron shall be able to estimate its relative position and orientation with respect toa known object on the ground.

• Sauron shall be able to track a moving Balrog.

4.1 Hardware

Sauron is equipped with computing hardware, sensors and hardware used for communi-cation. Listed below are the significant hardware related to Sauron. Sauron is a X8+

quadcopter made by 3D Robotics. An overview of the hardware for Sauron is seen inFigure 39.


Minesweeper 37

Figure 39: Schematic overview of the Sauron subsystem hardware.

4.1.1 Computing Hardware

The onboard computing hardware are listed below.

PixHawk The PixHawk is a flight controller that runs the opensource software Ardupilot. The PixHawk with Ardupi-lot enables advanced tools for data-logging, analysis andsimulation as well as navigation and control of Sauron.

RPi A Raspberry Pi model 3 is connected to the PixHawk on-board Sauron to enable more advanced missions such astracking of a known object using computer vision. TheRaspberry is running Ubuntu Mate Xenial with ROS asoperating system and communicates with the ground sta-tion using WiFi.

4.1.2 Sensors

The image and flight sensors used are:

Integrated sensors Sauron is equipped with several integrated sensors forflight control. These include an IMU, GPS, barometerand a compass. These sensors are currently not used, butmay be accessed through Ardupilot if necessary.

Camera Sauron is equipped with a Raspberry Pi Camera moduleV2 aimed downwards, used for detecting and tracking theAprilTag mounted on Balrog.


Minesweeper 38

4.1.3 Communication Hardware

The communication hardware are listed below.

WiFi Range Extender Mounted on Sauron. Enables increased range for the WiFiused to transmit/receive data to/from the BaseStation. Itsupports both 2.4 GHz and 5 GHz WiFi networks.

RC controller FS-TH9X PCM1024 controller with a FrSKY DJT teleme-try rf module.

Router ASUS RT-AC51U, 300 Mbps in 2.4 GHz frequency and upto 433 Mbps in 5 GHz dual-band connectivity.

4.2 Communication

The communication between nodes is done using ROS on a local network. The base stationis connected to the router with ethernet cable and Sauron is connected to the 5 GHz WiFinetwork using the WiFi Extender mounted on Sauron. The camera onboard Sauron isdirectly connected to the Raspberry Pi with a flex cable. The Raspberry communicateswith the onboard flight controller (Pixhawk with Ardupilot) using UART protocol withtwo dedicated wires directly connected to the Pixhawk. A schematic of the communicationcan be seen in figure 40.

Figure 40: Communication and messages for Sauron.


Minesweeper 39

4.3 Localization

The localization software is responsible for detecting an object as well as estimatingSaurons position and orientation with respect to the detected object. This is done usinga visual fiducial system called AprilTags in ROS.

4.3.1 AprilTags

To detect objects on the ground, AprilTags were used. A detailed explanation of theAprilTag system can be found in [11] and [12]. The theory behind camera calibrationusing AprilTags is presented in [19].

There are several different existing AprilTag implementations for ROS. The one used forSauron in this project can be found at [20]. The AprilTag detection software calculatesthe correct 3D position, orientation and identity of the tag with respect to the camera.

The AprilTag with id 12 from the 36h11 family, represented in Figure 41, was usedthroughout development and testing.

Figure 41: AprilTag ID 12.


ROS, Robot Operating System, is the basis for communication in this project and consistsof several nodes communicating with each other by publishing on, and subscribing to,different topics. The nodes related to localization are:

• raspicam_node

• republish

• apriltags

A schematic overview of the ROS nodes used in the localization module can be seen inFigure 42. These nodes are described in detail further below.


Minesweeper 40

Figure 42: Schematic overview of the ROS nodes in localization module.

raspicam_node

The raspicam_node runs on the Raspberry Pi mounted on Sauron and executes the fileraspicam_node.cpp. It captures video-frames using the Pi camera and compress themin to the JPEG format. It creates two publishers, one is named image_pub that pub-lishes these compressed images as messages of type sensor_msgs :: CompressedImageon the topic /image/compressed. The second publisher is named camera_info_puband publishes camera information such as time stamp and frame id. These are of messagetype sensor_msgs :: CameraInfo and are published on the topic /camera_info. Aschematic overview of the raspicam_node is represented in Figure 43.

The source code for raspicam_node was taken from [21] and offers several options interms of resolution and framerate for video capturing. These were however not optimalfor the purpose of tracking Balrog due to either too low resolution, too low framerate ortoo large files causing delays when sending these within the network. After experimentingwith both different resolutions and framerates a custom launch file using 600x600 reso-lution recording at 15 fps was chosen. This was deemed a reasonable trade off betweenlatency and resolution. However, there is room for improvement by additional optimiza-tion of the different camera parameters.

Figure 43: Schematic overview of raspicam_node.

republish

The republish node is operated on the base station and subscribes to the topic /image/compressedpublished by raspicam_node running on the RPi on board Sauron. republish unpacks theJPEG images published on /image/compressed and publishes RAW images on /usb_cam/image_raw.The apriltags node will only accept RAW images. Sending compressed images over thenetwork is much better than sending RAW images as less data is required to be transferred.See Figure 44 for a schematic overview of the republish node.


Minesweeper 41

Figure 44: Schematic overview of republish.

apriltags

The apriltags node is operated on the base station and subscribes to the topics/usb_cam/image_raw published by the republish node and to /camera_info publishedby raspicam_node. The purpose of apriltags is to achieve image tracking of the knownimage (the AprilTag) and to estimate the position and orientation of the Pi cameramounted on Sauron with respect to the AprilTag.

apriltags publishes its processed data to the topic /apriltags/marker_array in the formof a visualization_msg :: MarkerArray containing tag information with tag-IDs as wellas the estimated position and orientation. A schematic overview of the apriltags node isrepresented in Figure 45.

The source code from [20] was modified so that a message is always sent even if no tagis detected. The message will, in the case of no detection, only contain a time-stampof when the image was taken. Thus all messages includes a time-stamp which can becompared with the time-stamp of the previous message to determine the framerate. Thisway, the calculated framerate (which later on is inverted and used as sample-time in thecontrollers) is not disrupted as detection is temporarily lost.

Figure 45: Schematic overview of apriltags.


Minesweeper 42

4.4 Track Moving Balrog

To be able to autonomously track the moving Balrog four controllers are implementedthat use data from the localization module. The theory behind these controllers, as wellas how they were implemented in ROS, is explained below.

4.4.1 Controller theory

In the early stage of the project it was thought that a rotational matrix was needed forSauron to operate in its own local coordinate system instead of the local coordinate sys-tem of the AprilTag. This turned out to not be the case due to the way apriltags outputsvalues. The raw distance values returned from apriltags are in the local system of Sauronas seen in figure 46. ∆z is not shown in the figure but is the vertical distance from Sauronto the AprilTag.

The controllers implemented for Sauron are controlling throttle, pitch, roll and yaw, in-dependently. All controllers are based on the same time discrete PID controller, on theform (19) with further explanation below. The roll controller takes ∆x as input, the pitchcontroller takes ∆y as input and the controller for yaw takes the ϕ as input. The throttlecontroller takes ∆z as input.

The reference is set to 0 for the pitch, roll and yaw controllers. For the throttle controllerthe reference is set to a value between 3-4, representing the vertical distance between theAprilTag and Sauron in meters. These reference values translate to, when the controlerror is 0, Sauron hovering straight above the AprilTag at a height set by the referencefor the throttle controller (3-4 meters).

Figure 46: Schematic overview of Sauron and AprilTag

Controller algorithm

Since Sauron should be able to run both manually and autonomously two importantfactors to consider was integral windup and bumpless transfer when switching between


Minesweeper 43

modes. Below the equations used for implementing the controller algorithm are explainedas well as the potential issues that could arise.

Table 5: Controller Parameter List

Parameter DescriptionKp Proportional ConstantIk Integrating contribution to outputTi Integrating ConstantDk Derivative Variableµ Filtering for the derivativeTd Derivative ConstantKd Derivative contribution to outputet Control errorTs Sample Timeytmp Temporary outputyt Output

Each controller follow the same general algorithm pattern, the only internal differencebeing the parameter values and for the throttle also a dead-band compensation (see sec-tion 4.4.2, pid). This algorithm consists of several equations, updating the integratingand derivative parts individually. Then adding them together with the proportional partto get the output, ytmp, see Equation (17) - (19). Finally this output is run through asaturation function which limits the output and ensures a safer flight behaviour of Sauron.

Derivative action:

Dk,t =µTdDk,t−1

µTd + Ts+et − et−1

µTd + Ts(17)

Integral action:

Ik,t = Ik,t−1 +KpetTsTi

(18)

Output before saturation:

ytmp = Kpet + Ik,t +KpTdDk,t (19)

The derivative part is using the constant µ as a filtering constant. By decreasing µ a goodderivation can be obtained even for higher frequencies but with the cost of little noisesuppression of high frequencies [22].

The output is then limited (different for each controller) with both a minimal and maximalallowed value, using Equation (20).

yt = min(max(ytmp, ymin), ymax) (20)

To counter integral windup in case of the output exceeding its limits, the integrating partis updated again using Equation (21).

Ik,t = Ik,t +(yt − ytmp)Ts

Ti(21)


Minesweeper 44

4.4.2 ROS implementation

The ROS nodes implemented to achieve autonomous tracking are:

• sauron

• pid

• pilot

A schematic overview of the nodes used in the autonomous tracking module is representedin Figure 47. These nodes are described in detail further below.

Figure 47: Schematic overview of the nodes used in the autonomous tracking module.

sauron

The sauron node is operated on the basestation and running the python script sauron.py.It is subscribing to the topic /apriltags/marker_array where it receives messages ofthe type visualization_msgs :: MarkerArray. The messages include information aboutthe AprilTag such as AprilTag id and header information. If the AprilTag detected isof tag id 12 the node extracts more information from the message such as distance andorientation of the Raspberry Pi camera with respect to the AprilTag. It only extractsthis information when the AprilTag is of id 12 to minimize the chance of erroneouslydetecting a tag. The node then transforms the relative distance and orientation so thatit corresponds to the local coordinate system of Sauron. The correctly defined relativedistance and orientation are then published on the topic /sauron/output as messages oftype std_msgs :: Float64MultiArray. A schematic overview of the sauron node is rep-resented in Figure 48.

If an AprilTag other than tag id 12 is discovered, no information will be extracted andpassed along to the controller. This was implemented to make Sauron safer to fly in areaswhere other AprilTags might be present. There have also been cases when the apriltagsnode have detected AprilTags out of the blue. Since there are about 500 different AprilTagsID:s, the chances of erroneously detecting a tag with id 12 is small. The tag id 12 waschosen randomly.

Figure 48: Schematic overview of sauron.


Minesweeper 45

pid

The node pid is operated on the basestation and running the python script pid.py.This node is subscribing on the topic /sauron/output where it receives messages oftype std_msgs :: Float64MultiArray. These messages include data regarding the ori-entation of Sauron relative the AprilTag. The orientation includes the distance in x-coordinates, y-coordinates, height and the offset in yaw angle. These values are thenused by the four PID-controllers to calculate the correct control signals. The node thenpublishes these control signals on the topic /sauron/pid as messages of type std_msgs ::Float64MultiArray. A schematic overview of the pid node is represented in Figure 49.

Figure 49: Schematic overview of pid.

There are four controllers in the pid.py script. One for throttle (to keep a constantheight), one for roll, one for pitch and one for yaw. The controller for roll, pitch and yaware implemented in a similar way except for that the parameter values for the individ-ual controllers differ. Each of the controllers are implemented in such way that they areputting out values in the same range as channel 1 through 4 on the remote controller. Inthis way, control signals can directly override the stick input from the remote controller.This is very beneficial as this enables easy switching between manual and automatic mode.

However, when the mode “Altitude Hold” is active (which it always is), the throttle stickhas a dead-band between 1400 and 1600 in PPM-signal. When flying in manual mode,this is neat in the way that it is easier to maintain a constant height as the reference heightonly is changed when the throttle stick is moved under 1400 or above 1600. The controllerfor throttle is thereby dead-band compensated, 100 is added to a positive output signaloch 100 is subtracted from a negative output signal. 1501 becomes 1601 and vice-versa,as seen in the example Figure 50.

Figure 50: Example of dead-band compensation


Minesweeper 46

Because of lag and delay, the frame-rate of which the images are transferred is varying fromframe to frame. To handle this, all controllers are implemented with a variable sampletime. The sample time is calculated as the difference between last frame and the currentframe in the sauron.py script and sent to the pid.py script through topic /sauron/output.In the worst case scenario when a single frame is delayed (because of processor occupancyor bad connection etc.) the controller will output a smooth control signal as the controlleroutput is scaled according to the time it has taken to receive the new frame.

When flying at a high altitude or far away from the tag, noisy output of the detectionalgorithm may occur which may transfer into noisy control signals. Therefore, all con-trollers have a filtered derivative of the raw data to reduce oscillating and noisy controlsignals. The derivative part also ensures a smoother behaviour of Sauron when movingat higher velocities. This was especially useful when Sauron approaches the Apriltag ata high velocity since the derivative part then ensures that Sauron does not overshoot bytoo much.

All outputs of the controllers are also limited to increase safety. Because of these limi-tations, the integral part is compensated, as the control signal is at its upper or lower limit.

The parameter values are manually set and manually tested for good performance. Thetests are only performed by visually determining what properties are preferred (by lookingat Sauron and controller output during flight). The parameter values are thereby adjustedto receive the desired behaviour. There are no guarantees that the values are perfectlyadjusted to apprehend best possible flying characteristics.

Changing operational modes must be done swiftly and smoothly. As described underSection 4.4.2, pid, the output of the controllers acts as if manual input is simulated.A PPM-value of 1500 represents centered throttle/yaw and roll/pitch sticks. However,Sauron will most likely not keep its position with all channels set to 1500. Sauron willdrift and when automatic mode is active, the integral action of each controller will takecare of this drifting. The outputs of all channels will therefore deviate from 1500 duringautomatic flight above a stationary tag.

From flight to flight, the value obtained in the integral will differ as sensors are re-calibrated every startup and the battery is mounted in slightly different positions everytime. However, all integral values are set to 1500 as the script is initiated. When Sauronenters automatic mode, the integral values will change, never to return to the initiatedvalue of 1500 unless the integral ends up on that value.

When switching from automatic mode to manual mode, the integral value will be madestatic until automatic mode is once again active. This disables the integral value to windup as Sauron may detect tags and continue to output control signals which later on areoverride by the manual input from the remote controller.

When automatic mode is active and there is a sudden loss in tag detection, the last setcontrol signal is smoothly transitioned into the last calculated integral value. This is use-ful when the detection algorithm fails to detect a tag of a single frame. The control signalwill not abruptly jump from one value to another, ensuring smooth and safe flight.


Minesweeper 47

pilot

The node pilot is operated on the Raspberry Pi mounted on Sauron and running thescript pilot_comm.py. The node is subscribing to the topic /sauron/pid from where itreceives control signals as messages of type std_msgs :: Float64MultiArray. The pilotnode does not publish any data, it is “the end of the line” in communication. The purposeof this node is to provide these control signals to Sauron, just as if they were outputs fromthe hand controller. A schematic overview of the pilot node is represented in Figure 51.

Figure 51: Schematic overview of pilot.

When running the pilot node functionality to override the hand controller is active! Whenauto mode is switched on the stick input on the hand controller will no longer controlSauron, but is instead generated by pid and passed on by pilot.

In addition to providing control signals the pilot node also has several safety featureswhere control is given back to the user and Sauron enters landing mode. The currentsaftey modes are described further below.

If the connection is not fast enough there is a risk of receiving control data based on toold information. This could cause unwanted behaviour and is therefor considered a safetyrisk. To prevent this from happening pilot_comm.py checks the time it takes for a newmessage to be delivered and should there not be a new message within 0.5 seconds alloverrides will be terminated and Sauron will be forced to land.

If pilot_comm.py lose connection to the ROS-master/network it will no longer be able toreceive any data and will therefor terminate all overrides and force Sauron to land.


Minesweeper 48

4.5 Assumptions

Some assumptions have been made to ease the integration of Sauron in to the minesweep-ing system. These are described below.

To ease the implementation of the controllers it was assumed that they are operatingindependently of each other without any major cross-coupling present. This could beinvestigated for further development of the controllers.

Also all of the controller parameters have been tuned in a very controlled environment.It has been done indoors, so that no major external disturbance (wind, etc.) has beenacting on the system. The floor has been of a light white color, greatly improving thespeed of the AprilTag package, compared to being outside with grass as background of thecamera. The theory behind this is that there is more work for the AprilTag package toscan the image when there are a lot more different colors surrounding the AprilTag. Theresult of this is that, in tracking mode, Sauron will probably not handle rapid maneuversfrom Balrog as well if the environment was to change.


Minesweeper 49

5 BaseStation

BaseStation is a computer running ROS which communicates with the Balrog and Sauronsubsystems by publishing and subscribing to different topics, and displays relevant sensordata in separate GUIs.

5.1 Balrog GUI

A simple GUI has been designed using rqt [23], as seen in Figure 52 below. The left halfof the GUI contains an rviz window that displays data from the LiDAR, different maps,the estimated 2D robot pose and a live video feed from the onboard camera, including avisualization of all detected AprilTags. The two plots in the upper right corner displaythe wheel encoder sensor data that is transmitted from RPi.

The center right part of the GUI is a message publisher, where the user can send basic com-mands to Balrog by publishing strings on two different topics. When put in autonomousmode, Balrog will not initialize the mission until “start” is published on start_topic. Thisis done by clicking the small square next to start_topic in the topic list. In this mode, theuser can also adjust the maximum speed of Balrog by publishing on the change_velocitytopic. The bottom right part contains a list of all active topics.

Figure 52: The Balrog GUI.


Minesweeper 50

5.2 Sauron GUI

Currently Saurons GUI, Figure 54, consists of a window showing a live feed from theRaspberry Pi camera and four gauges displaying ∆x, ∆y, ∆z and ∆ϕ. The node guidatasubscribes to the topic sauron/output and then publishes ∆x, ∆y, ∆z and ∆ϕ to thetopics /guidata/xdist, /guidata/ydist, /guidata/zdist and /guidata/yaw. The GUI hasbeen designed in rqt and subscribes to these topics and displays the data in the GUI. Inthe camera feed the AprilTag is marked by highlighting it when ever it is detected andthe gauges display a needle as a visualization of the offset to the reference as well as thenumerical value.

Figure 53: Schematic overview for the ROS nodes associated with Sauron GUI.

These gauges are an extension to rqt, found at [24], and have to be installed as any ROSpackage before they can be used (see the User Manual [2] for instructions on how to installpackages).

To run this GUI you first need to start the guidata node via the command:

rosrun guidata guidata.py

Then the GUI can be launched with the the command:

rosrun rqt_gauges gauges_script.py

This will open the GUI window and setup the gauges, the camera feed then has to beadded manually from the tab “plugins” by selecting the “image view” for the full GUI inFigure 54.


Minesweeper 51

Figure 54: Sauron GUI


Minesweeper 52

6 Further Development

In this section, a few different areas potentially suitable for further development by futureproject groups are presented and discussed.

6.1 Balrog

In this section areas related to Balrog which might be suitable for further developmentare presented.

Balrog shows at times an undesirable behavior when running autonomously. It occasion-ally rotates back and forth before heading for the next node. This does not interfere withany functionality of Balrog, however it does take longer to finish objectives. There isalso room for improvement with regards to efficiency. Currently there is no optimizationwith respect to mapping and covering tiles at the same time. One can easily envision analgorithm which during mapping mode takes into account where it has been in order toalso cover tiles while exploring.

Currently there is currently only one way of running the autonomous mode with mapping→ covering → disarming. It could be desirable to run only one of these modes, thusgaining more control of the autonomous mode. There is also an idea of creating a newmode clear pathway where the Balrog runs from point A to point B and clears a pathwayin between these points, ensuring they are free from mines.

6.1.1 Improve the controller

The controller being used in our project has been proven to work very well. Howeverthere are always ways to improve the functionality. Today the system is controlled withan open loop where no feedback is used for the linear and angular velocity. A closed loopsystem might improve the performance, especially when driving slowly the dead band canbe handled in a more elegant way. Feedback on velocities can be obtained by integratingthe data from the wheel encoders. A more relevant and simple improvement is to trim thescaling factor such that the desired velocities are closer to the actual velocity of Balrog.

A problem identified with the “turn first, then move” is that sometimes rather unnecessaryturn are made. If Balrog gets a goal position slightly behind it or drives past the goalposition, it will rotate heavily. The solution to this might be some kind of filter, whichalso takes into account that the current position is constantly updated from SLAM andmight move Balrog away from its goal position.

6.1.2 Tracking an AprilTag

An interesting addition to Balrog’s manual and autonomous mode would be a mode inwhich Balrog will physically track an AprilTag of specific ID, similar to how Sauron tracksAprilTags in the current system. Most necessary components should already be in placesince the detection node outputs an estimate of Balrog’s 3D position and orientation withrespect to the tag, and the controller takes a goal position as input.

6.1.3 Filtering of Mine Detections

As described in Section 3.5, no form of filtering is currently applied to the mine detections.This solution was found to provide good enough accuracy for the simple current use case,but it is definitely a reasonable candidate for further development. Potential improvements


Minesweeper 53

would be to apply a simple median filter to the detections, or to give less weight todetections occurring at a long distance from the tag.

6.1.4 Improving the Disarming Mode

As described in Section 3.6.5, the detected mines are currently visited in an arbitraryorder during disarming. An obvious improvement would thus be to try and compute amore clever order. Ideally, one would want to visit the mines in the order that minimizesthe total travelled distance.

6.2 Sauron

In this section areas related to Sauron which might be suitable for further developmentare presented.

6.2.1 Processing and communication

The initial approach was to let the Raspberry Pi handle the processing of the images.Due to technical problems during installation of OpenCV on the Raspberry Pi (which isrequired by the apriltag node), the decision to transfer the images to the base station andprocess them there was made. There is uncertainty if the Raspberry Pi has the requiredCPU performance to be able to handle the processing of the images.

The advantages of processing the images onboard Sauron is that the delay and eventuallag that appears in the communication over the network will be eliminated. So eventhough the fact that the images will take longer time processing on the Raspberry Pi,the lag and delay will be smaller. The FPS may be lower, but less delay and low FPS ispreferred over high FPS and high delay when the data will later on be used in a controller.

A lot of time was spent debugging the delay in the transport of the images. Some majorsources causing delay and lag were identified and eliminated but there are more work to bedone to further improve the connection. The hand controller is sending over 2.4 GHz andinterfered with the network. Therefore, a 5 GHz network was set up and the delay andlag was greatly reduced and the communication is not disturbed as the hand controller isswitched on.

Another major source of lag and delay was the network card within the provided laptop.By connecting the laptop directly to the router using an Ethernet cable, the communica-tion was greatly improved.

When all nodes are up and running on the network, images are transferred and processedsmoothly and the frame-rate is rather constant. However, occasionally, the frame-ratedrops significantly and the delay is temporary increased. Some man hours have beenput in trying to identify where this comes from and why. No solution has been found.“Renicing” (changing the priority) the processes that are related to all nodes have notaffected the lag and delay occasionally occurring.


Minesweeper 54

6.2.2 Controller

There are scenarios when Sauron decreases in height as a large roll and/or pitch anglesare present. There would be interesting to implement some sort of cross-coupling of thecontrollers to see if this behaviour could be eliminated.

Now four PID-controllers are implemented. The benefit of PID-controllers is that nomodel of the system is required. A future improvement would be to compile a model overSauron and implement some other sort of controller. It would also be possible to use theinformation of future movements of Balrog in a MPC-controller. By doing that, Sauroncould be able to follow Balrog with no delay.

6.2.3 GUI

Currently the fully working GUI only consists of the camera feed though the AprilTagpackage, showing the camera feed with a marking on the AprilTag if it’s detected. Anyadditional information for the user is currently received via the terminal.

The implementation of a better GUI was initialized and does in addition to the camerafeed also show the relevant distances to the AprilTag seen from Sauron. This GUI isimplemented using rqt with an additional package called /gauges.

It was early in the project thought that a similar GUI but with some more functionalityand better looks/design could be created but it was deemed to be too time consuming.Wanted functionality that does not exist in the current configuration would be:

• Options to choose mission for Sauron (if different missions would be implemented)

• Possibility to land from GUI with options to land instantly or return to startingposition then land (same functionality as on the hand controller)

• Options to tune controller parameters directly from GUI

With further development on Sauron a fully custom GUI could prove to be useful.

6.2.4 Detection and AprilTags

The AprilTag configuration is currently only looking for tag number 12 and would discardall other tag information if a different AprilTag was found. This is the intended imple-mentation as of now, to avoid Sauron from doing unwanted maneuvers if other tags aredetected.

A problem with using only one tag is that Sauron will sometimes lose detection due tothe tag flickering from the wind caused by Sauron, dirt on the tag or just simply becauseSauron drifted a little too far off. It might be worth looking into if a different AprilTagconfiguration would be worth using, 2 or maybe 4 AprilTags on the corners of Balrog tocounter the loss of detection. Taking a mean value of the relative position from each tagmay improve the quality of the data recieved.

In that way, if one of the tags is not detected, there are other tags that may be detectedinstead. The downside of this may be that each tag have to be smaller to fit on Balrog,


Minesweeper 55

making them harder to detect.

The higher the resolution of the images, the better the AprilTag detection module is todetect the tags and extract the relative position. However, the higher the resolution, thelonger time it takes for he Apriltags module to process the images. If the processingwas done onboard Sauron with an enhanced hardware, a higher resolution could be used.Even though the processing time would increase, the delay in the network would havebeen eliminating. The detection data would be better and smaller tags could be detected.

6.2.5 Different detection system

In the long run AprilTags should not have to be used at all, so some other detection systemwill have to be implemented at some point. One alternative is to use computer vision todetect Balrog without a tag and the controller would strive after centering Balrog in theimages. This implementation would rely on the IMU of Sauron as each image would haveto be adjusted according to the angle of Sauron.


Minesweeper 56

7 List of ROS Topics

All ROS topics utilized in the Balrog and Sauron subsystems are listed in Table 6 andTable 7, respectively. Each topic is listed together with its message type, all publishingand subscribing ROS nodes, and a description of the message content.

Table 6: All ROS topics utilized in the Balrog subsystem.

Message type Topic Publishers Subscribers Contentsensor_msgs:LaserScan

/scan sweep_node mapper Single scan readfrom the 360◦LiDAR.

std_msgs:Float64MultiArray

/encoder_data communicator slam_odom Wheel encodermeasurements.

nav_msgs:OccupancyGrid

/map mapper slam_visitedmain

Occupancy mapoutputted by theSLAM module.

visualization_msgs:Marker

/Mapper/vertices mapper slam_visited Sampled points onthe estimated posetrajectory.


/estimated_pose slam_pose controllermain

Estimated robotpose outputted bythe SLAM module.


/odom_pose slam_odom - Estimated robotpose, based solelyon the encoderdata.


/map_visited slam_visited nav_covering_map Map of all areasthat have been vis-ited by Balrog.


/goal_pos coordinator controller Current goal posi-tion (x, y).

std_msgs:String

/node_status main controller If anything is pub-lished on this topicthe controller willstop Balrog.

std_msgs:String

/start_topic balrog_gui controller A string “Start”needs to be pub-lished to start thecontroller.

std_msgs:String

/change_velocity balrog_gui controller A topic to changethe maximum lin-ear velocity.


/control_signals controller communicator The control signalto the left and rightmotor.

std_msgs:String

/controller_status controller coordinator String indicatingthat Balrog hasreached a goalposition.


Minesweeper 57


/map_covering nav_covering_map main The processed mapwith a 30 cm resolu-tion and expandedobstacles


/map_covering_large

nav_covering_map - The processed mapwith a 15 cm resolu-tion and expandedobstacles.


/path main coordinator Path that Balrogshould follow.

std_msgs:String

/coordinator_status coordinator main String indicatingthat Balrog hasreached the end ofa path.

std_msgs:String

/balrog_info main - String containinginfo about Balrog’sautonomous mode.

sensor_msgs:Image

/balrog_camera/image_raw

stream_to_ROS republish Raw frames fromthe video feed.

sensor_msgs:CameraInfo

/balrog_camera/camera_info

stream_to_ROS apriltag_detector_node

Camera info andcalibration parame-ters.

sensor_msgs:CompressedImage

/balrog_camera/image_raw/compressed

republish apriltag_detector_node

Compressed framesfrom the video feed.

sensor_msgs:CompressedImage

/tag_detections_image/compressed

apriltag_detector_node

- Compressed framesfrom the videofeed, includingvisualization ofdetected tags.


Minesweeper 58

Table 7: All ROS topics utilized in the Sauron subsystem.

Message type Topic Publishers Subscribers Contentsensor_msgs:CompressedImage

/image/compressed raspicam_node republish Compressed framesfrom the video feed.

sensor_msgs:CameraInfo

/camera_info raspicam_node apriltags Camera info. Timestamp, frame id etc.

image_transport:ImageTransport

/usb_cam/image_raw

republish apriltags Transforms thecompressed imagesto raw images.

visualization_msg:MarkerArray

/apriltags/marker_array

apriltags sauron AprilTag informa-tion.


/sauron/output sauron pidguidata

Transformed April-Tag information.


/sauron/pid pid pilot Control signals.


/guidata/xdist guidata rqt Data displayed inthe gauges GUI.


/guidata/ydist guidata rqt Data displayed inthe gauges GUI.


/guidata/zdist guidata rqt Data displayed inthe gauges GUI.


/guidata/yaw guidata rqt Data displayed inthe gauges GUI.


Minesweeper 59

References[1] D. Gossow D. Hershberger and J. Faust. rviz - ros wiki, 2017. URL http://wiki.

ros.org/rviz.

[2] E. Näsholm F. Gustafsson F. Tormod H. Andersson J. Jerner M. Andreasson A. Häg-glund, A. Lundgren. User manual minesweeper. Unpublished technical document,2017.

[3] CDIO 2016 TIGER. Technical documentation, 2016.

[4] Sebastian Kasperski. nav2d_karto - ros wiki, 2017. URL http://wiki.ros.org/nav2d_karto.

[5] H. Carrillo Y. Latif D. Scaramuzza J. Neira I. Reid C. Cadena, L. Carlone andJ. J. Leonard. Past, present, and future of simultaneous localization and mapping:Towards the robust-perception age. IEEE Transactions on Robotics, 32(6), pages1309–1332, 2016.

[6] E. B. Olson. Real-time correlative scan matching. Proceedings of the IEEE Interna-tional Conference on Robotics and Automation (ICRA), 2009.

[7] R. Kummerle W. Burgard B. Limketkai K. Konolige, G. Grisetti and R. Vincent.Efficient sparse pose adjustment for 2d mapping. Proceedings of the IEEE/RSJ In-ternational Conference on Intelligent Robots and Systems (IROS), 2010.

[8] C. Stachniss G. Grisetti and W. Burgard. Improved techniques for grid mappingwith rao- blackwellized particle filters. IEEE Transactions on Robotics 23(1), pages24–46, 2007.

[9] Scanse. Github - scanse/sweep-ros: Scanse sweep ros driver and node, 2017. URLhttps://github.com/scanse/sweep-ros.

[10] Turki Y. Abdalla Ziyad T. Allawi. An accurate dead reckoning method based ongeometry principles for mobile robot localization. International Journal of ComputerApplications (0975 – 8887) Volume 95– No. 13, 2014.

[11] Edwin Olson. AprilTag: A robust and flexible visual fiducial system. In Proceedingsof the IEEE International Conference on Robotics and Automation (ICRA), pages3400–3407. IEEE, May 2011.

[12] John Wang and Edwin Olson. AprilTag 2: Efficient and robust fiducial detection.In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots andSystems (IROS), October 2016.

[13] P. Mihelich. image_transport - ros wiki, 2017. URL http://wiki.ros.org/image_transport.

[14] RIVeR-Lab. Github - river-lab/apriltags_ros, 2017. URL https://github.com/RIVeR-Lab/apriltags_ros.

[15] Brian Yamauchi. Planning paths of complete coverage of an unstructured environ-ment by a mobile robot, 1997.

[16] J.C. Byrne A. Zelinsky, R.A. Jarvis and S. Yuta. A frontier-based approach forautonomous exploration, 1993.

[17] N. Koenig and A. Howard. Gazebo - ros wiki, 2017. URL http://wiki.ros.org/gazebo.


http://wiki.ros.org/rviz

http://wiki.ros.org/rviz

http://wiki.ros.org/nav2d_karto

http://wiki.ros.org/nav2d_karto

https://github.com/scanse/sweep-ros

http://wiki.ros.org/image_transport

http://wiki.ros.org/image_transport

https://github.com/RIVeR-Lab/apriltags_ros

https://github.com/RIVeR-Lab/apriltags_ros

http://wiki.ros.org/gazebo

http://wiki.ros.org/gazebo

Minesweeper 60

[18] Open Source Robotics Foundation Inc. Turtlebot2, 2017. URL http://www.turtlebot.com/turtlebot2/.

[19] Andrew Richardson, Johannes Strom, and Edwin Olson. AprilCal: Assisted andrepeatable camera calibration. In Proceedings of the IEEE/RSJ International Con-ference on Intelligent Robots and Systems (IROS), November 2013.

[20] P. Velagapudi A. Walsman. Ros wrapper for the swatbotics c++ port of the april-tags visual fiducial tracker, 2016. URL https://github.com/personalrobotics/apriltags.

[21] R. Agrawal and W. Gramlich. Package to access the raspberry pi camera from ros,2017. URL https://github.com/UbiquityRobotics/raspicam_node.

[22] M. Enqvist et al. Industriell reglerteknik kompendium, 2014.

[23] D. Scholz D. Thomas and A. Blasdel. rqt - ros wiki, 2017. URL http://wiki.ros.org/rqt.

[24] alexvs. gauges - ros wiki, 2017. URL http://wiki.ros.org/gauges.


http://www.turtlebot.com/turtlebot2/

http://www.turtlebot.com/turtlebot2/

https://github.com/personalrobotics/apriltags

https://github.com/personalrobotics/apriltags

https://github.com/UbiquityRobotics/raspicam_node

http://wiki.ros.org/rqt

http://wiki.ros.org/rqt

http://wiki.ros.org/gauges

technical documentation minesweeper · technical documentation minesweeper version 0.4 editor: elin...

Documents