design specification - linköping university · the mine sweeping functionality and improved the...

35
Autonomous Mine Sweeper LiTH 2015-10-05 Design Specification Editor: Sofie Griph Version 1.0 Status Reviewed Axel Halldin 2015-10-05 Approved Hanna Nyqvist 2015-xx-xx TSRT10 Oscar Hörberg Ross Haj [email protected] LIPs Page 1

Upload: others

Post on 29-Mar-2020

4 views

Category:

Documents


0 download

TRANSCRIPT

Page 1: Design Specification - Linköping University · the mine sweeping functionality and improved the crawlers positioning. The main goal of the project in 2012 was to verify that the

Autonomous Mine SweeperLiTH

2015-10-05

Design SpecificationEditor: Sofie Griph

Version 1.0

StatusReviewed Axel Halldin 2015-10-05Approved Hanna Nyqvist 2015-xx-xx

TSRT10Oscar Hörberg

Ross [email protected]

LIPsPage 1

Page 2: Design Specification - Linköping University · the mine sweeping functionality and improved the crawlers positioning. The main goal of the project in 2012 was to verify that the

Autonomous Mine SweeperLiTH

2015-10-05

PROJECT IDENTITY2015/HT, Ross Haj

Linköping University, Dept. of Electrical Engineering (ISY)

Group membersName Responsibility Phone EmailAxel Halldin Project Leader (PL) 070-3035052 [email protected] Hörberg Head of Documentation (DOC) 076-1308547 [email protected] Griph Head of Design 070-9413964 [email protected] Svensk Head of Software 070-6343977 [email protected] Örn Head of Hardware 070-3724874 [email protected] Fåhraeus Head of Tests 073-7270416 [email protected] Hyllengren Head of Sensor Fusion 073-0858131 [email protected]

Email list for the whole group: [email protected] site: not specified yet

Customer: Saab Bofors Dynamics, LinköpingCustomer contact: Torbjörn Crona, [email protected]

Course leader: Daniel Axehill, 013-284042, [email protected]: Hanna Nyqvist, 013-281 353, [email protected]

Tutors: Martin Lindfors, 013-281365, [email protected]örn Johansson, [email protected]

Carl Nordheim, [email protected]

TSRT10Oscar Hörberg

Ross [email protected]

LIPsPage I

Page 3: Design Specification - Linköping University · the mine sweeping functionality and improved the crawlers positioning. The main goal of the project in 2012 was to verify that the

Autonomous Mine SweeperLiTH

2015-10-05

Contents

Document history IV

1 Introduction 1

1.1 Project history . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1

2 System Overview 2

2.1 System flow . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

2.1.1 Sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

2.1.2 Sensor Diagnosis and Signal Processing (SDSP) . . . . . . . . . . . . 3

2.1.3 Positioning and Mapping . . . . . . . . . . . . . . . . . . . . . . . . . 4

2.1.4 Base Station . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

2.1.5 Hand Controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

3 Base station 6

3.1 Graphical User Interface (GUI) . . . . . . . . . . . . . . . . . . . . . . . . . . 6

3.1.1 Status bar . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

3.1.2 Settings section . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

3.1.3 Map section . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

3.1.4 Way point section . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

3.1.5 New functions in the GUI . . . . . . . . . . . . . . . . . . . . . . . . 7

4 Hand control 10

5 Balrog 11

5.1 Sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

5.1.1 IMU . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

5.1.1.1 Sensor interface . . . . . . . . . . . . . . . . . . . . . . . . 12

5.1.1.2 Communication . . . . . . . . . . . . . . . . . . . . . . . . 12

5.1.1.3 On board processing . . . . . . . . . . . . . . . . . . . . . . 12

5.1.1.4 Placement on Balrog . . . . . . . . . . . . . . . . . . . . . . 13

5.1.1.5 Output . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

5.1.2 Ultrasonic sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15

5.1.2.1 Sensor interface . . . . . . . . . . . . . . . . . . . . . . . . 15

5.1.2.2 Communication protocol . . . . . . . . . . . . . . . . . . . 16

5.1.3 Implementation and flow in subsystem . . . . . . . . . . . . . . . . . . 16

TSRT10Oscar Hörberg

Ross [email protected]

LIPsPage II

Page 4: Design Specification - Linköping University · the mine sweeping functionality and improved the crawlers positioning. The main goal of the project in 2012 was to verify that the

Autonomous Mine SweeperLiTH

2015-10-05

5.1.4 Dependencies to other systems . . . . . . . . . . . . . . . . . . . . . . 17

5.2 Sensor Diagnosis and Signal Processing (SDSP) . . . . . . . . . . . . . . . . . 18

5.2.1 Sensor modelling and signal pre-processing . . . . . . . . . . . . . . . 18

5.2.2 Sensor Diagnosis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19

5.2.2.1 Outlier rejection . . . . . . . . . . . . . . . . . . . . . . . . 19

5.2.3 Data available from SDSP . . . . . . . . . . . . . . . . . . . . . . . . 20

5.2.4 Implementation and flow in subsystem . . . . . . . . . . . . . . . . . . 20

5.3 Positioning and Mapping . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23

5.3.1 Pre-filter Processing . . . . . . . . . . . . . . . . . . . . . . . . . . . 23

5.3.2 Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23

5.3.2.1 Motion Model . . . . . . . . . . . . . . . . . . . . . . . . . 24

5.3.2.2 Measurement Model . . . . . . . . . . . . . . . . . . . . . . 25

5.3.3 Probability Mapping . . . . . . . . . . . . . . . . . . . . . . . . . . . 26

5.3.4 Mine Detection and Mapping . . . . . . . . . . . . . . . . . . . . . . 26

5.3.4.1 Known Issues . . . . . . . . . . . . . . . . . . . . . . . . . 26

5.3.5 Implementation and flow in subsystem . . . . . . . . . . . . . . . . . . 27

5.3.6 Dependencies to other subsystems . . . . . . . . . . . . . . . . . . . . 27

5.4 Implementation Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28

5.4.1 Shared data structure . . . . . . . . . . . . . . . . . . . . . . . . . . . 29

5.4.1.1 New data structures . . . . . . . . . . . . . . . . . . . . . . 29

5.4.2 Subsystems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29

5.4.2.1 New subsystem . . . . . . . . . . . . . . . . . . . . . . . . 29

A Appendix 30

References 30

TSRT10Oscar Hörberg

Ross [email protected]

LIPsPage III

Page 5: Design Specification - Linköping University · the mine sweeping functionality and improved the crawlers positioning. The main goal of the project in 2012 was to verify that the

Autonomous Mine SweeperLiTH

2015-10-05

Document history

Version Date Changes Sign Reviewed0.1 2015-10-05 First draft all SG, RS, SÖ, OH0.2 2015-10-10 Second draft all all0.3 2015-10-14 Third draft SÖ, RS, SG, HF, JH SG, HF

TSRT10Oscar Hörberg

Ross [email protected]

LIPsPage IV

Page 6: Design Specification - Linköping University · the mine sweeping functionality and improved the crawlers positioning. The main goal of the project in 2012 was to verify that the

Autonomous Mine SweeperLiTH

2015-10-05

1 Introduction

The Autonomous Mine Sweeper project has been running since the spring of 2009. The goal isto construct a prototype of an autonomous mine sweeping crawler. It is a joint project betweenSaab Bofors Dynamics and Linköping University.

The purpose of this document is to give a detailed description of the subsystems that are goingto be changed or added during this year’s project. For a thorough description of previouslyimplemented subsystems, see the previous years’ "Design Specification" [5] and "TechnicalDocumentation" [6]. A brief project history will be given in the following section.

1.1 Project history

The following description of the project history is grabbed from last year’s Design Specification[5].

In autumn 2009 the group Carpe Locus continued where O’hara’s had left and developed remotecontrol of the crawler and automatic control for the propulsion. The crawler was also equippedwith GPS.

The next autumn (2010) the group 8Yare continued by mounting an industrial computer on thecrawler and ported the old code to the new computer. The crawler was also mounted with stereocameras (model Bumblebee 2).

In 2011 group iMAP focused on using the camera and the crawlers sensors to create a 3D mapof the operating area.

In 2012 Minenmarker choose not to continue the work with the 3D map. Instead they developedthe mine sweeping functionality and improved the crawlers positioning. The main goal of theproject in 2012 was to verify that the entire area had been searched and that all the mines werefound. Minenmarker also named the robot Balrog.

In 2013 the group Ostende Abscondita continued to improve the positioning and search algo-rithm. They also integrated a wireless hand controller for manual control of Balrog.

In 2014, the project group Invenire Periculosa’s main goals were to continue to develop Bal-rog’s positioning. Balrog was equipped with a 360°laser sensor and a more powerful industrialcomputer.

This year, 2015, the main goal is to improve the positioning of Balrog. To help with this task abetter IMU-unit, also including a barometer, will be mounted on Balrog.

TSRT10Oscar Hörberg

Ross [email protected]

LIPsPage 1

Page 7: Design Specification - Linköping University · the mine sweeping functionality and improved the crawlers positioning. The main goal of the project in 2012 was to verify that the

Autonomous Mine SweeperLiTH

2015-10-05

2 System Overview

The mine sweeping system is divided in three main subsystems, the Base Station, the HandController and Balrog. Balrog is a small crawler that consists of different subsystems. Eachsubsystem performs specific tasks and includes hardware, software or both. For a schematicoverview of the system see figure 1. The arrows tell how the communication flows in thesystem.

Balrog

Exte

rnal

Com

mun

icat

ion

API

Hand Controller

Base Station

Wireless link

SensorsIMU

Odometer

Laser scanner

GPS

Ultrasonic

Sensor Diagnosis and Signal Processing

Positioning and MappingMine detection

Signal pre-processingPropulsion

Electric Motor

Control

Route planning

Description

Hardware

Software

Component

ARM processor

GUI

Wireless link

Information Flow

Barometer (in IMU)

Obstacle detection

Sensor diagnosis

Navigation filter

Map/Mapping

Figure 1: Illustration of the system and its subsystems. The communication flow in the systemis illustrated by arrows.

This year’s project will focus on the subsystems "Sensors", "Sensor Diagnosis and Signal Pro-cessing" and "Positioning and Mapping". The design goal is to keep as much as possible fromlast year, and therefore the other subsystems will not be changed unless it is necessary for thesystem to work well. The Base Station will be modified to meet the requirements in the "Re-quirement Specification" [4].

TSRT10Oscar Hörberg

Ross [email protected]

LIPsPage 2

Page 8: Design Specification - Linköping University · the mine sweeping functionality and improved the crawlers positioning. The main goal of the project in 2012 was to verify that the

Autonomous Mine SweeperLiTH

2015-10-05

2.1 System flow

In this section the system flow will be described and the focus will be according to the de-sign goal mentioned above. In other words, the data that is passed between the subsystemsRoute Planning, Control and Propulsion or between them and other subsystems will be kept thesame as in last year’s project if possible. Therefore, only the subsystems where the data flowwill be changed considerably are described below. To understand the flow between the othersubsystems, see the "Technical Documentation" from last year [6].

2.1.1 Sensors

In the Sensors subsystem raw sensor data will be stored and time stamped. This data will be sentto the Sensor Diagnosis and Signal Processing subsystem. Most of this is already implemented,but the code that log IMU data will be modified to fit the new IMU sensor that also includes abarometer. The raw sensor data will also be sent to the Base Station for debug purposes.

2.1.2 Sensor Diagnosis and Signal Processing (SDSP)

This subsystem will receive raw and timestamped sensor data and pre-process it. Its purposeis to provide the navigation filter with high quality data. The system will run marginally fasterthan the filter, meaning fresh data will always be available for the filter. The pre-processeddata from all sensors will be put together and stored in a shared data structure on Balrog. Eachprocessed sensor sample is stored together with a time stamp, a flag that tells if the sensor isreliable/unreliable/not available and a flag that marks if the data is an outlier or not, see figure2. If a new sensor value cannot be updated due to that the filter executes faster than the sensorcan be sampled, the sensor value will be flagged as not available. The package of pre-processedsensor data will be used by the Positioning and Mapping subsystem and sent to the Base Station.

From Positioning and Mapping the SDSP subsystem will receive information needed in thesensor diagnosis. The SDSP subsystem can also receive commands from the Base Stationtelling the SDSP subsystem to treat a sensor as reliable/unreliable.

Processed sample

Flag: reliable/unreliable/unavailableTimestamp Flag:

outlier/inlierSensor 1

Processed sample

Flag: reliable/unreliable/unavailableTimestamp Flag:

outlier/inlierSensor N

Figure 2: Schematic of a package of processed sensor data written by SDSP. Packages will bestored in a shared data structure. The filter will use the latest package as input.

TSRT10Oscar Hörberg

Ross [email protected]

LIPsPage 3

Page 9: Design Specification - Linköping University · the mine sweeping functionality and improved the crawlers positioning. The main goal of the project in 2012 was to verify that the

Autonomous Mine SweeperLiTH

2015-10-05

2.1.3 Positioning and Mapping

The Positioning and Mapping subsystem will use the pre-processed data from the Sensor Diag-nosis and Signal Processing subsystem as input to the navigation filter, obstacle detection, minedetection and mapping (if there is no predefined map). In this year’s project the mine detectionwill not be covered and no changes will be made to that algorithm unless it is necessary.

This subsystem will be in charge of all maps that are used in the mine sweeping system. Therewill be an obstacle map, a probability map and a mine map. All of these maps should beavailable for the Route Planning subsystem and sent to the Base Station. The estimated statesdelivered by the filter will also be sent to these subsystems.

2.1.4 Base Station

The Base Station and Balrog will be modified so that the Base Station can receive the followingdata from Balrog:

• Raw sensor data with time stamp (from Sensors)

• Pre-processed sensor data with time stamp and flag (from Sensor Diagnosis and SignalProcessing)

• Estimated states (from Positioning and Mapping)

• Maps: obstacle map, probability map and mine map (from Positioning and Mapping)

• Obstacle detection info (from the algorithm Obstacle detection)

• Motor voltage (from Propulsion)

The Base Station and Balrog will be modified so that the Base Station can send the followingdata to Balrog:

• Control commands to make it move (to Control)

• Set sensors unreliable/reliable (to Sensor Diagnosis and Signal Processing)

• Predefined obstacle map (to SDSP)

All data sent between the base station and Balrog will be encapsulated as shown in Table 1according to existing implementation [6, sect. 4.1]. There are some new messages being passedbetween the base station and Balrog, the id of those messages can be seen in Table 2 andTable 3. The payloads of the new sensor status messages sent from the base station to Bal-rog will be one of three defined modes with id; SENSOR_STATUS_ALWAYS_USE, SEN-SOR_STATUS_AUTOMATIC_USE or SENSOR_STATUS_NEVER_USE.

2.1.5 Hand Controller

The hand controller will be the first way of controlling Balrog in manual mode. It sends com-mands to Balrog (to Control) making it move.

TSRT10Oscar Hörberg

Ross [email protected]

LIPsPage 4

Page 10: Design Specification - Linköping University · the mine sweeping functionality and improved the crawlers positioning. The main goal of the project in 2012 was to verify that the

Autonomous Mine SweeperLiTH

2015-10-05

ID Payload length Payloaduint16_t id uint16_t len char payload[len]

Table 1: Data format for a message between Balrog and the base station

ID DescriptionNETWORK_MESSAGE_PROCESSED_DATA Sends the buffer of processed data

from SDSP periodically

Table 2: New messages sent from Balrog to base station

ID DescriptionNETWORK_MESSAGE_OBSTACLE_MAP Sends the predifinied map structureNETWORK_MESSAGE_IMU_STATUS Sends a command to set the IMU in

a specific stateNETWORK_MESSAGE_ODOMETER_STATUS Sends a command to set the odome-

ter in a specific stateNETWORK_MESSAGE_LASER_STATUS Sends a command to set the laser in

a specific state

Table 3: New messages sent from the base station to Balrog

TSRT10Oscar Hörberg

Ross [email protected]

LIPsPage 5

Page 11: Design Specification - Linköping University · the mine sweeping functionality and improved the crawlers positioning. The main goal of the project in 2012 was to verify that the

Autonomous Mine SweeperLiTH

2015-10-05

3 Base station

The base station consists of a laptop communicating with Balrog via WiFi. The purpose of thebase station is to receive data from Balrog and send predefined commands to Balrog. The datareceived should be presented in a way easily understood by the person operating Balrog.

If needed the operator shall be able to manually operate Balrog with simple commands fromthe base station. Since only manual operation will be considered this year the manual operationof Balrog via the base station is crucial as a backup to the hand control discussed in Section 4.

The functions on the base station that handles connection to the Balrog and manual operationis already implemented, [6, sect. 4], so these functions will be tested and minor fixes applied ifnecessary.

The main focus will be to develop a way to easily present all the data passed between modulesinside the Balrog system. This will be very helpfull when designing other parts of the systembut also good for evaluating missions done by Balrog.

The development will be based on earlier years work and some functions will be reimplemen-tation of existing code to work with the new Balrog system initialized last year (2014).

Another crucial feature needed this year is the possibility to load a predefined map into Balrogfrom the base station.

3.1 Graphical User Interface (GUI)

The main view in the current GUI is divided into four parts; The status bar at the top, the settingsat the left, the map in the center and way points at the right. See figure 3. There are somemore functions of the base station available in the ’File menu’ in the upper left corner of theapplet. These functions are Connect, Disconnect, XBoxConfiguration, ControllerConfigurationand FilterConfiguration.

Figure 3: Current layout of the GUI

TSRT10Oscar Hörberg

Ross [email protected]

LIPsPage 6

Page 12: Design Specification - Linköping University · the mine sweeping functionality and improved the crawlers positioning. The main goal of the project in 2012 was to verify that the

Autonomous Mine SweeperLiTH

2015-10-05

The current layout of the GUI will be kept as it is for the most part with some minor changesto make place for new functionality. Changes to the GUI are explained further in 3.1.5. Theunderlying source code will be overlooked and redesigned to make future implementation easierto integrate, as recommended by last year’s team.

3.1.1 Status bar

The status bar contains mostly status information sent from Balrog. The only exception is thebutton to switch mode between automatic and manual control which can be found on the far leftside of the bar. Expansions to the status bar will be status fields for each sensor on Balrog tellingthe user how the SDSP currently uses the sensor values from each sensor; always use values,automaticly discard bad values, never use values. A sketch of the layout with expansions canbe seen in figure 4.

3.1.2 Settings section

In the settings section of the GUI, in addition to previously implemented functionality [6],the operator will be able to open the window containing the newly implemented sensor datapresentation. There will be a button named ’Sensor Data’, a layout sketch can be seen in figure4.

3.1.3 Map section

Since the focus this year is navigation/localization this part of the GUI will be made operableagain and used for evaluating the algorithms developed by the team. A map created by a com-bination of the maps sent from Balrog and the predefined map will be displayed along withBalrog’s current position on it.

The design of the created map will be based on earlier decisions and some new aspects. Thepredefined map will be displayed as is in the background and the obstacle map from Balrog willbe drawn over it. They will be drawn with different colors to make it easy for the operator tomake out which is which. The probability map will be displayed as a grid of visited sectionsof the obstacle map. The probability map will be presented by displaying the probability thatBalrog has been in a certain grid of the map by colouring the specific grid in different shadesbetween white and green. Each grid is white from the beginning and becomes more green asthe probability grows. A sketch of how this will look can be seen in figure 4.

Since mines will not be considered this year, the GUI will not present a mine map.

3.1.4 Way point section

This section of the GUI will be left as it is since automatic operation is not a part of this year’sfocus.

3.1.5 New functions in the GUI

There are three new functions that are going to be integrated to the current GUI. These threefunctions are disable/enable sensors, input a predefined map, and display current sensor datapassed between the modules on Balrog.

TSRT10Oscar Hörberg

Ross [email protected]

LIPsPage 7

Page 13: Design Specification - Linköping University · the mine sweeping functionality and improved the crawlers positioning. The main goal of the project in 2012 was to verify that the

Autonomous Mine SweeperLiTH

2015-10-05

,

Figure 4: Sketch of the new layout of the GUI

Figure 5: Structure of the file containing the predifined map

Disable/enable Sensors

How the disable/enable sensors function will be implemented is decribed above in the sections3.1.1 and below in section ’Sensor Data Presentation’.

Input map

To input a map the user will be able to choose ’Input map’ in the ’File menu’ and input thelocation of a file defining the map. The map will be defined as start- and end-points of eachline of an object. Each point will be represented in cartesian coordinates and connected toeach-other to form a line as explained by the structure in figure 5.

Sensor Data Presentation

To present sensor data passed between the modules on Balrog the operator can push the buttonin the ’Settings section’ called ’Sensor Data’ which will open a new window containing graphsand values presenting the data as well as the functionallity to toggle the mode of each sensor.How data is displayed depends on the type of data.

The sensor toggle will be solved by having three buttons similar to the existing one for selectingmanual/automating mode for each sensor. To toggle a sensor the operator simply pushes the

TSRT10Oscar Hörberg

Ross [email protected]

LIPsPage 8

Page 14: Design Specification - Linköping University · the mine sweeping functionality and improved the crawlers positioning. The main goal of the project in 2012 was to verify that the

Autonomous Mine SweeperLiTH

2015-10-05

Figure 6: Sketch of the ’Sensor Data’ GUI

button corresponding to the desired mode. Selecting a new mode will automatically deselectthe previous one. A graphical sketch of how it will look like in the GUI can be seen in figure 6.

The data will be displayed using graphs drawing sensor value over time, with exception for thelaser sensor and the GPS. This is true for both raw sensor data and sensor data processed by theSDSP. The processed sensor data graphs will also show where the data is seen as unreliable bydrawing the drawing the line in a different color or another fashion so that it is obvious for theoperator. The processed data and corresponding raw data will be plotted in the same graph foreasy comparison.

The current position from the GPS will be printed as values and draw in a scatter plot markingeach position with a time stamp.

The data array from the laser sensor will be translated to a scatter plot plotting the currentmeasurements relative to Balrog.

A rough sketch of the layout of these graphs and values can be seen in Figure 6.

TSRT10Oscar Hörberg

Ross [email protected]

LIPsPage 9

Page 15: Design Specification - Linköping University · the mine sweeping functionality and improved the crawlers positioning. The main goal of the project in 2012 was to verify that the

Autonomous Mine SweeperLiTH

2015-10-05

4 Hand control

The hand controller is used to operate Balrog in manual mode. The controller is a Xbox 360control communication over the 2.4 GHz band with a dongle connected to and located on Bal-rog.

Since most of the functionality to manually operate Balrog already exist it will be tested andonly changed if needed. Some of the functions recommended by last years team may be lookedat. Probable examples of functions that will be implemented are:

• ’Set Balrog to manual mode’ - Switch between manual and automatic operation using thehand controller

• ’Stop Balrog’ - Stop all actions of Balrog

TSRT10Oscar Hörberg

Ross [email protected]

LIPsPage 10

Page 16: Design Specification - Linköping University · the mine sweeping functionality and improved the crawlers positioning. The main goal of the project in 2012 was to verify that the

Autonomous Mine SweeperLiTH

2015-10-05

5 Balrog

This section will describe the subsystems on which this year’s project will focus on.

5.1 Sensors

Balrog is equipped with a number of sensors, which are used to collect data about Balrog andits environment. All sensors but the odometer are directly connected to the main processingunit. The odometer is connected via an ARM-processor which handles sampling and settings.

This section will focus on explaining the new sensors for this year. See previous year’s DesignSpecification for more details on previously mounted sensors [5, sec. 5.2, p. 215]

In Table 4 all sensors, including the new ones, which will be mounted on Balrog are listed.

Sensor Measured physi-cal quantity

Output Max frequency Standardrange

Gyroscopes Angle velocity,heading

deg/s or delta an-gle

2000 Hz, 400 Hz 0-450deg/s

Accelerometers Velocity, acceler-ation

m/s2 or delta ve-locity

2000 Hz, 400 Hz 0-50 m/s2

Magnetometer Heading, minedetection

a u (arbitraryunits, normalizedto earth fieldstrength)

100 Hz 0-80 T

Barometer Altitude Pa 50 Hz 300-1100hPa

Temperature sen-sors

- °C 1 Hz -

Laser sensor Distance (to ob-ject)

Point cloud([mm, degrees]measured fromthe sensor’scenter)

2000 Hz 0-6 m

Ultrasonic sen-sors

Distance (to ob-ject)

cm 15 Hz and up 0-6 m (11m max)

GPS Position WGS84 position,number of satel-lites used, accu-racy

<4 Hz -

Odometer Distance (trav-elled), speed

mm, mm/s Event-based 1.07mm(maxresolution)

Table 4: Sensors that will be mounted on Balrog

TSRT10Oscar Hörberg

Ross [email protected]

LIPsPage 11

Page 17: Design Specification - Linköping University · the mine sweeping functionality and improved the crawlers positioning. The main goal of the project in 2012 was to verify that the

Autonomous Mine SweeperLiTH

2015-10-05

5.1.1 IMU

An IMU-unit, which also includes a barometer and a magnetometer, will be mounted on Balrog.The sensor is of type MTi 100 and is manufactured by Xsens. This is an upgrade from last year’sBalrog. The MTi 100 contains the following sensors:

1. 3D accelerometer

2. 3D gyroscope

3. 3D magnetometer

4. Barometer

5. Temperature sensor

All facts about the sensor in this section are taken from The MTi User Manual [2], unless statedotherwise.

See Figure 7 for a picture of the sensor.

5.1.1.1 Sensor interface

The IMU is connected to the main processing unit via USB. The USB cable also provides powerto the IMU.

5.1.1.2 Communication

An SDK package is provided by Xsens, this contains the Xsens Device API which can be usedfor communication with the MSi 100. The API is written in C and compatible with Windowsand Linux systems, but a C++ wrapper is also available and will be used in this project. Impor-tant to note is that the API works for all of Xsens’ sensors and not just MTi 100. This meanssome of the functions in the API will not provide a meaningful return value, because they targeta feature that does not exist in the MTi 100.

Xsens also offers a complete specification of the low level communication [3].

Furthermore, Xsens provides a set of graphical user interface applications which can be used todisplay measurements and to perform calibration. However, these applications are only avail-able in Microsoft Windows, meaning they will only be used for testing purposes in this project.

5.1.1.3 On board processing

The MTi 100 has an on board processor which handles sampling of the sensors and has builtin signal processing software. The unit contains an internal 16 bit ADC to convert the analogsensor values to digital. There is an option to get the raw, unprocessed (but digitalized) sensorvalues as output. See section 5.1.1.5 for specifications of what output the MTi 100 can provide.

TSRT10Oscar Hörberg

Ross [email protected]

LIPsPage 12

Page 18: Design Specification - Linköping University · the mine sweeping functionality and improved the crawlers positioning. The main goal of the project in 2012 was to verify that the

Autonomous Mine SweeperLiTH

2015-10-05

5.1.1.4 Placement on Balrog

To get the best performance from the gyroscopes and accelerometers the IMU should be placedto minimize transient accelerations. Thus it should be placed as close to the rotational axisof Balrog as possible. Since the rotation around the Z-axis (see Figure 7) is of most interest,placement on Balrog which minimizes transient acceleration around this axis takes priority.

The IMU should, if possible, be placed in an environment free of magnetic disturbances tominimize noise in the magnetometer. This condition will most likely prove hard to fulfill, sinceBalrog’s electrical motors will generate a varying magnetic field depending on how much poweris fed to them by the control unit. Smaller magnetic disturbances from all other electronics willalso be present.

The IMU-unit should be firmly mounted, so that it always follows the movement of Balrog. Tohave some damping material to absorb vibrations (which will give noise in accelerometer data)might be considered.

Coordinate system

The sensor fixed coordinate system is a right handed system, see Figure 7. The sensor shouldbe either mounted or rotated (software calibration) so its coordinate system is aligned withBalrog’s, see Figure 8.

X

Z

Y

Figure 7: The MTi 100 and its default coordinate system

5.1.1.5 Output

In the following section the different outputs of the MTi 100 are listed and explained.

Raw uncalibrated output

The MTi 100 can output raw sensor data, meaning it is unprocessed by the internal software.Table 5 shows the raw output and sampling frequency of the sensors in MTi 100.

TSRT10Oscar Hörberg

Ross [email protected]

LIPsPage 13

Page 19: Design Specification - Linköping University · the mine sweeping functionality and improved the crawlers positioning. The main goal of the project in 2012 was to verify that the

Autonomous Mine SweeperLiTH

2015-10-05

Y

XZ

Front

Back

Figure 8: Balrog’s coordinate system, seen from above

Sensor Type Unit Max frequency Standardrange

Gyroscopes Analog sensor, 16bit ADC

[1] 2000 Hz 450 deg/s

Accelerometers Analog sensor, 16bit ADC

[1] 2000 Hz 50 m/s2

Magnetometer Digital sensor a u (arbitraryunit)

100 Hz 80 T

Barometer Digital sensor Pa 50 Hz 300-1100hpa

Temperature sen-sors

Analog sensor, 12bit ADC

°C 1 Hz -

Table 5: Sensors in MTi 100, their raw output and sampling frequency

Calibrated delta velocity and delta angle

Calibrated output, which shows the change of velocity (delta velocity) and change of angle(delta angle) since last sampling is available from the IMU. The output is obtained by doinga culling- and coning compensated strap down integration in a sensor fixed coordinate system.Note that the output depends on output frequency, since integration is carried out over a specifictime interval. The default time interval is 2.5 ms (400 Hz) [2, sec. 4.2.2, p. 26]

Angle change over multiple sample periods can be obtained by multiplying Delta_q. Note thatthis will most likely introduce a drift. Table 6 shows delta velocity and delta angle. [2, sec.4.9.3, p. 48]

TSRT10Oscar Hörberg

Ross [email protected]

LIPsPage 14

Page 20: Design Specification - Linköping University · the mine sweeping functionality and improved the crawlers positioning. The main goal of the project in 2012 was to verify that the

Autonomous Mine SweeperLiTH

2015-10-05

Output UnitDelta_q (DataID 0x8030) [1] (a.u., quaternion values)Delta_v (DataID 0x4010) m/s

Table 6: Delta velocity and delta angle. a.u. stands for arbitrary units, normalized to earth fieldstrength

Calibrated inertial and magnetic data outputs

The MTi 100 can output preprocessed 3D linear acceleration, 3D rate of turn (gyro) and 3Dmagnetic field data. All values are in a sensor-fixed coordinate system. The calibrated datahas been going through Strapdown Integration and Inverse Strapdown Integration. Table 7illustrates the calibrated inertial and magnetic data output.

Vector UnitAcceleration m/s2

Angular velocity (rate of turn) rad/sMagnetic field a.u.(arbitrary units, normal-

ized to earth field strength)

Table 7: Calibrated inertial and magnetic data output

Time stamps and package counter

The MTi-100 can send a time stamp and/or package counter with each package.

Status message

The MTi 100 can put out a status message which contains information about configuration,status of individual sensors etc. See MTi User Manual [2, sec. 4.13, p. 56] for details.

5.1.2 Ultrasonic sensors

Balrog will be equipped with four ultrasonic sensors to help with obstacle detection. These areof type SRF10 and the specifications for the sensors can be found in [7]. The sensors shall bemounted on Balrog as shown in figure 9 below. The will be two sensors at the front to get goodmeasurements for obstacle detection, where the two sensor lobes overlaps.

5.1.2.1 Sensor interface

The ultrasonic sensors are connected to the main control system by a I2C (Inter-IntegratedCircuit) bus. Through the I2C bus multiple sensor units are able to communicate on the samephysical wire. Each ultrasonic sensor are given a unique address that specifies which sensor isto be read or written to at the moment, see Table 8. The main control system at the Balrog doesnot have any interface for communication through I2C. Thus it use a USB-I2C converter unit.

TSRT10Oscar Hörberg

Ross [email protected]

LIPsPage 15

Page 21: Design Specification - Linköping University · the mine sweeping functionality and improved the crawlers positioning. The main goal of the project in 2012 was to verify that the

Autonomous Mine SweeperLiTH

2015-10-05

β

Figure 9: Layout of the positions of the ultrasonic sensors

Direction Position Address

ForwardLeft

Right0xE20xE6

Left 0xEARight 0xEE

Table 8: I2C address for each ultrasonic sensor

5.1.2.2 Communication protocol

The ultrasonic sensors can be set to different addresses in order to communicate using the I2Cbus. The I2C-USB converter are controlled by a simple text based protocol which is specifiedin [8]. The ultrasonic sensors have four different internal register which that can be written toand read from through the I2C bus.

A distance measurement is started by sending a command to the communication register ofa sensor. The result will be read from the result register of the sensor in a predeterminedfrequency. The results of the measurement are given in centimetres.

5.1.3 Implementation and flow in subsystem

Each sensor subsystem includes the following:

1. The hardware (including eventual on board processing provided by the manufacturer)

2. An API which handles communication with the sensor

3. A log where all sensor data is stored

The task for the API is to handle all interaction with the sensor. This includes fetching sensordata and setting parameters in the sensor’s on board processing unit. All measurements that isfetched by the API will be timestamped and stored in a shared data structure available to othersubsystems within Balrog.

See Figure 10 for a schematic of the flow.

TSRT10Oscar Hörberg

Ross [email protected]

LIPsPage 16

Page 22: Design Specification - Linköping University · the mine sweeping functionality and improved the crawlers positioning. The main goal of the project in 2012 was to verify that the

Autonomous Mine SweeperLiTH

2015-10-05

Sensor

API

User

Data log

Hardware

Software

Globally accessible data structure

On board processing

Figure 10: Flow of a sensor subsystem. The arrows shows the communication.

5.1.4 Dependencies to other systems

Each sensor subsystem is independent. Thus every sensor subsystem can be run in its ownthread without any considerations.

TSRT10Oscar Hörberg

Ross [email protected]

LIPsPage 17

Page 23: Design Specification - Linköping University · the mine sweeping functionality and improved the crawlers positioning. The main goal of the project in 2012 was to verify that the

Autonomous Mine SweeperLiTH

2015-10-05

5.2 Sensor Diagnosis and Signal Processing (SDSP)

The SDSP subsystem will be added by this year’s project in order to perform sensor modellingand sensor diagnosis. All pre-processing and synchronization of sensor data will take placehere. The sections below describe how the SDSP subsystem will work.

5.2.1 Sensor modelling and signal pre-processing

The purpose of the sensor modelling and signal pre-processing is to process the sensor data andmodel the noise. If the noise is non-Gaussian, an inverse filter will be modelled in order to geta signal with white Gaussian noise (WGN). The navigation filter in Positioning and Mappingwill work better if the noise is WGN.

Multiple data sets will be collected before modelling, so that the resulting models describereality as well as possible.

The data sets from each sensor will be examined using periodograms in order to see whetherrespective noise is considered as white or non-white. To be able to tell if the noise is Gaus-sian, different plot tools in MATLAB will be used (ndist, normplot etc.). The variances andcovariances of the WGN will be estimated so that the navigation filter can use that info.

If the noise is non-Gaussian, an inverse filter will be found by using system identification. Thiswill be done by examining the noise using linear models in MATLAB’s System IdentificationToolbox [9]. The inverse filter that will be used if the noise is non-Gaussian will most likely beestimated using an ARMA or AR model. In this case, the data sets will be divided into modeldata and validation data.

The data sets will also be analysed in order to find an eventual bias in the sensor. It will only bepossible to know if there is a bias for the sensors that are measuring something with a knownground truth, for example a distance sensor. The sensor data will be compensated for the biasif it is necessary. Dependent on how the sensor works, the bias will be estimated off-line oron-line. If the bias seems to be constant both when Balrog is moving and when it is stationary,the bias can be set off-line. Otherwise it will be set on-line.

Because of major difficulties in determining an exact input signal - which would be the groundtruth of what the sensor is measuring - especially for a dynamic input signal, the sensor mod-elling will be of a constant-but-unknown-input kind (the ground truth is not known). The modelwill therefore not examine the dynamics of the sensors and will only examine its stationaryproperties.

In cases where measurable noise contributors (disturbances) can be identified, the noise willbe modelled based on these contributors. One example of such a contributor is the electricpropulsion motors that are likely to create disturbances in the magnetometer measurements.

Experiments

Constant-but-unknown-input experiments will be conducted by collecting sequences of sensordata while the position of Balrog is constant.

Experiments with disturbance-source input will instead be conducted when the disturbance ispresent in order to estimate the effect of it on the signal. For example, to model the signal noisewhen the electrical propulsion motors are on, Balrog will be driven with a constant speed.

Table 9 shows which experiments that will be conducted for each sensor.

TSRT10Oscar Hörberg

Ross [email protected]

LIPsPage 18

Page 24: Design Specification - Linköping University · the mine sweeping functionality and improved the crawlers positioning. The main goal of the project in 2012 was to verify that the

Autonomous Mine SweeperLiTH

2015-10-05

Sensor Constant-but-unknown-input experiment

Disturbance-source experi-ment

Laser scanner Yes NoUltrasonic sensors Yes NoAccelerometer Yes NoGyroscope Yes NoMagnetometer Yes Constant reference signal to

the electrical propulsion mo-tors

Barometer Yes NoOdometer Yes No

Table 9: Experiments for each sensor

5.2.2 Sensor Diagnosis

The main task of the sensor diagnosis part of the SDSP subsystem is to detect when a sensoris unreliable/reliable. Data from unreliable sensors will not be utilized in the Positioning andMapping subsystem. Neither will outliers be available for that subsystem. To decide if a sensorworks well or not, outlier rejection will be used. If a sensor gives too many outliers, based ona design parameter for that specific sensor, it will be marked as unreliable. A sensor that hasbeen marked unreliable can be reset to reliable if it seems to work well again. Another designparameter will be needed for that decision.

There will be some cases when sensor data must be flagged not available. This happens whenthe filter in Positioning and Mapping executes faster than some of the sensors sample. This flagwill also be set by SDSP.

5.2.2.1 Outlier rejection

The outlier rejection will be based on a design parameter (threshold) L for each sensor and theprobability density function p(yk+1|y1:k) for each sensor. The same main idea for outlier rejec-tion will be used for all sensors except from the odometer, accelerometer and magnetometer.The idea is presented below.

The filter used in Positioning and Mapping is a marginalized particle filter (see section 5.3).Each particle given by that filter will contain the nonlinear states xn,i, the linear states xl,i, thecovariance P l,i and the weight wi. This information together with the general model structureof the marginalized particle filter [10] gives that a measurement y is distributed as a Gaussianmixture:

y ∼ p(yk+1|y1:k) ≈N∑i=1

wik+1|kN (hk(x

n,ik+1|k)+Hk(x

n,ik+1|k)x̂

l,ik+1|k, Hk(x

n,ik+1|k)P

l,ik+1|kHk(x

n,ik+1|k)

T+R)

(1)Where R is the covariance of the measurement noise.

If (1) holds, the following test could be made to detect outliers.

p(yk+1|y1:k) < L (2)

TSRT10Oscar Hörberg

Ross [email protected]

LIPsPage 19

Page 25: Design Specification - Linköping University · the mine sweeping functionality and improved the crawlers positioning. The main goal of the project in 2012 was to verify that the

Autonomous Mine SweeperLiTH

2015-10-05

All sensor values that do not fullfill the test will be treated as outliers. By changing the value ofL, it will be possible to tune the test. Not all sensors will be analysed according to (2) and theyare described in the following sections.

Odometer and accelerometer

The odometer is unlikely to suffer from big outliers and the test is then unnecessary. Theodometer have larger variance when driving due to slip but will be highly reliable when standingstill. This can be used to correct the accelerometer measurements when standing still.

Magnetometer

This year’s project will not focus on mine detection and therefore no sensor diagnosis willbe performed for the magnetometer. Code skeletons will be written to facilitate for next year’sproject group and it will be possible to get magnetometer data and set it as unreliable/reliable/notavailable. It will also be possible to flag the sensor as unreliable/reliable.

Barometer

If a 3D map is used for positioning, the position of Balrog and the uncertainty of it will be usedin the outlier test. The height on the map at the position given by the filter will be comparedwith the height given by the barometer measurement.

Basic outlier rejection

Besides the outlier rejection mentioned above some sort of basic outlier rejection will be madefor some of the sensors (when it is considered to be necessary). This means that an upper and/ora lower threshold will be set for the sensor values to be able to eliminate outliers. For example,it will be made for the distance sensors. A lower threshold will be set to remove all values thatcorresponds to the scenario when the sensor is detecting Balrog and not other obstacles.

5.2.3 Data available from SDSP

Data that will be available from the SDSP subsystem will be processed sensor data in the samephysical quantity that is given by the sensor but might be scaled to other units for example fromm/s to mm/s. If the filter in Positioning and Mapping wants a different physical quantity, it hasto convert the data itself. One example is to calculate balrog’s vertical speed from barometerdata.

5.2.4 Implementation and flow in subsystem

The SDSP subsystem includes the following:

1. Filtering of non-Gaussian noise

2. Sensor diagnosis

TSRT10Oscar Hörberg

Ross [email protected]

LIPsPage 20

Page 26: Design Specification - Linköping University · the mine sweeping functionality and improved the crawlers positioning. The main goal of the project in 2012 was to verify that the

Autonomous Mine SweeperLiTH

2015-10-05

3. Data from sensor subsystem

4. Data to filter

5. Data from filter

When online, the SDSP will if necessary filter the incoming signal with a noise filter in orderto get a signal with Gaussian noise. After that, SDSP will store the output in the same dataformat as the unfiltered sensor data. This data will be used by the sensor diagnosis and outlierrejection.

The SDSP is also a synchronizing barrier between the sensors and the filter, thus ensuring thatthe filter obtain data from the same time period for all available sensors. This means that theSDSP will only provide fresh data to the filter. In practice, this means that the reliable/unreliable/unavailable-flag will be set to unavailable for old sensor data. The filter can handle any numberof available sensor data, however, more fresh (available) data will likely make the filter outputmore accurate. The data synchronization will set the reliable/unreliable/unavailable-flag tounavailable when data is read and set the flag to reliable or unreliable when new data is written,this is done for every sensor data respectively. When the filter access the data, it can checkthe flag whether data has been updated since the last read and can therefore ignore data withthe unavailable flag. The filter will access all sensor data regardless of the condition of thereliable/unreliable/unavailable-flag.The base station has the possibility to set the reliable/unreliable/unavailable-flag for any sensorto a fixed state through public methods. This means that the sensor diagnosis will have thepermission to update the flag. The flag can also be unset so that the sensor diagnosis will beable to update the flag. This will be implemented through a private flag which will or will notgive the sensor diagnosis updating authority.

See Figure 11 for a schematic of the flow.

Dependencies to other systems

The different inverse noise filtering, the outlier rejection and the sensor diagnosis can be exe-cuted independently from each other and can be run on the same thread as its correspondingsensor. This means that every sensor filtering and diagnosis will independently fetch the latestfilter output available.

TSRT10Oscar Hörberg

Ross [email protected]

LIPsPage 21

Page 27: Design Specification - Linköping University · the mine sweeping functionality and improved the crawlers positioning. The main goal of the project in 2012 was to verify that the

Autonomous Mine SweeperLiTH

2015-10-05

Inverse noise model filteringLaser scanner

data

Ultrasonic sensor data

IMU data

Barometer data

GPS Data

Odometer data

Magnetometer data

Inverse noise model filtering

Inverse noise model filtering

Inverse noise model filtering

Inverse noise model filtering

Inverse noise model filtering

Inverse noise model filtering

Propulsion reference

signal data

Sensor diagnosis

Sensor diagnosis

Sensor diagnosis

Sensor diagnosis

Sensor diagnosis

Sensor diagnosis

Sensor diagnosis

Data from filter

Data synchronization Data to filter

Figure 11: Flow of the SDSP subsystem. The arrows shows the communication.

TSRT10Oscar Hörberg

Ross [email protected]

LIPsPage 22

Page 28: Design Specification - Linköping University · the mine sweeping functionality and improved the crawlers positioning. The main goal of the project in 2012 was to verify that the

Autonomous Mine SweeperLiTH

2015-10-05

5.3 Positioning and Mapping

The purpose of the positioning and mapping subsystem is to estimate Balrog’s position andheading and to create three maps: a map of detected mines, a map of the probabilities thata points in the search area has been visited and a map of obstacles in the search area (if noobstacle map has been provided). A sketch of the positioning and mapping system can be seenin Figure 12 (arrows denote information flow). The positioning and mapping subsystem wascalled the SLAM subsystem in the 2014 "Technical Documentation" [6].

Processed Sensor Data

Filter

Mine Detection Mine MappingMagnetometer

Data

Predefiend Obstacle Map Balrog States

Mine Map

Probability MapProbability Mapping

Figure 12: A sketch of the positioning and mapping system (arrows denote information flow)

The positioning and mapping system consists of:

• Pre-filter processing

• Filter

• Probability mapping

• Mine detection and mapping

5.3.1 Pre-filter Processing

Not all sensors provide data that is suitable for direct use in the filter. Values from these sensorsare pre-processed to make them usable.

The pressure as measured with the barometer is used to calculate Balrog’s vertical velocity.This is done by first taking the slope of a first order linear regression on the barometer readingstaken since last filter iteration. This gives the change in pressure during the time since last filteriteration, which is converted to a velocity.

The GPS data is processed to the x and y coordinates of Balrog in its obstacle map. This is doneby comparing the GPS coordinates to the GPS coordinates of a given point in the obstacle mapand calculating the x and y difference in meters.

5.3.2 Filter

The filter is a marginalized particle filter [10]. The development of the filter was started in 2014.This years project will continue to develop the filter. New sensor models and motion models

TSRT10Oscar Hörberg

Ross [email protected]

LIPsPage 23

Page 29: Design Specification - Linköping University · the mine sweeping functionality and improved the crawlers positioning. The main goal of the project in 2012 was to verify that the

Autonomous Mine SweeperLiTH

2015-10-05

will be added, and numerical issues from last year will be fixed. For a full description of thefilter algorithm and its implementation (including the loop parallelisation and the numericalissues), see the technical documentation from the 2014 project [6].

To estimate Balrog’s states, the filter uses a motion model and a sensor model for each sensortype. Parts of the accelerometer and gyroscope data will be regarded as input in the motionmodel, and all other data will be regarded as outputs from the sensor models. It will be possibleto predefine an obstacle map in Balrog. If this is done, Balrog will compare the range sensorreadings with this map to estimate its states. The given map will be regarded as absolute andwill not be changed by Balrog. If no map is predefined, Balrog will do simultaneous localizationand mapping with a dynamic map that will be updated as new data from the obstacle detectionsystem becomes available

5.3.2.1 Motion Model

A motion model will be used by Balrog. The model will be based on a coordinated turn 2Dmotion model with polar velocity. The model will also contain the roll and pitch of Balrog, asthese states are useful in modelling the accelerometer. The states at time k, xk are given below:

xk =

xkykvkφk

θkψk

ωk

(3)

where x is Balrog’s x coordinate, y is Balrog’s y coordinate, v its velocity, φ its roll, θ its pitch,ψ is its yaw and ω its yaw angle velocity. The time-discrete dynamics of the model is given by:

xk+1 =

xk + Tvk cos(ψk)yk + Tvk sin(ψk)

vkφk

θkψk + Tωk

ωk

+

T 2

2cos(ψ) 0 0

T 2

2sin(ψ) 0 0T 0 00 T 00 0 T0 0 00 0 0

uk+

T 2

2cos(ψ) 0 0 0

T 2

2sin(ψ) 0 0 0T 0 0 00 0 T 00 0 0 T

0 T 2

20 0

0 T 0 0

wk (4)

The vector uk contains the system inputs. u1k is given by the equation u1k = ak − θkg (g is thegravitational constant), Balrog’s acceleration (measured by the IMU and adjusted for gravity)linearised with the sinus small angle approximation. u2k is the roll angle speed (measured bythe gyroscope) and u3k is the pitch angle speed (measured by the gyroscope). The vector wk

contains the process noise. w1k is the IMU acceleration noise, w2

k is the yaw angle acceleration(modelled as noise), w3

k is the noise in the roll speed measurement and w4k is the noise in the

pitch speed measurement.

TSRT10Oscar Hörberg

Ross [email protected]

LIPsPage 24

Page 30: Design Specification - Linköping University · the mine sweeping functionality and improved the crawlers positioning. The main goal of the project in 2012 was to verify that the

Autonomous Mine SweeperLiTH

2015-10-05

5.3.2.2 Measurement Model

As the SDSP subsystem will handle the more complicated sensor modelling (subtracting biasesetc.), only simple sensor models will be needed. The sensor inputs are:

yk =

XGPS

YGPS

ωgyroscopevR+vL

2vR−vL

dv

vh,barometer

rlaser,1rlaser,2

...rlaser,360rultrasonic,1rultrasonic,2rultrasonic,3rultrasonic,4

(5)

XGPS and YGPS are the GPS position measurements, ωgyroscope is the IMU angle velocity mea-surements, vR and vL are the odometer measurements, dv is the virtual length of Balrog definedby the group, vh,barometer is the vertical speed of Balrog calculate from the barometer data bySDSP, rlaser,j is the LIDAR range measurement for angle j and rultrasonic,k is the ultrasonicrange measurement for sensor k. Only the sensor readings that SDSP deems accurate will beused in the filter, which means that yk might not contain all of the readings stated above. Theyare connected to the states according to:

h(xk) =

xkykωkvkωk

θkvkllaser(1, xj, yj, ψk)llaser(2, xj, yj, ψk)

...llaser(360, xj, yj, ψk)lultrasonic(1, xk, yk, ψk)lultrasonic(2, xk, yk, ψk)lultrasonic(3, xk, yk, ψk)lultrasonic(4, xk, yk, ψk)

(6)

θkvk is Balrog’s vertical velocity linearised with the sinus small angle approximation. llaser(j, xk, yk, ψk)and lultrasonic(k, xk, yk, ψk) are functions that given Balrog’s position, yaw and obstacle map(predefined or dynamic) calculates the distance from Balrog’s position (as given by the statevector) to the nearest wall in the direction of the measurement. For the laser scanner, eachvalue of j is the number of degrees at which the measurement was taken, and for the ultrasonicsensors each k is associated with an angle at which the sensor k is mounted.

TSRT10Oscar Hörberg

Ross [email protected]

LIPsPage 25

Page 31: Design Specification - Linköping University · the mine sweeping functionality and improved the crawlers positioning. The main goal of the project in 2012 was to verify that the

Autonomous Mine SweeperLiTH

2015-10-05

The llaser,j(x) and lultrasonic,k(x) algorithms run in three steps:

• Shift the obstacle map to make Balrog’s position (x, y) = (0, 0)

• Rotate the obstacle map with the measurement angle, so that the measurement directionbecomes (1, 0).

• Discard all lines whose endpoints has negative x-coordinates (lines behind Balrog).

• Discard all lines whose endpoints has non-zero y-coordinates with the same sign (linesabove/below the measurement line).

• Calculate the intersection between the measurement line (0, 0) + (1, 0)t, t > 0 and allremaining line segments. The answer will be on the form (R, 0), where R is the distanceto the intersection. This distance will be saved. For some lines no intersection will befound, the algorithm will skip to the next line segment in that case.

• Return the smallest distance R found. If no R (e.g. no intersections) was found, return aspecific distance larger then the range of the laser/ultrasonic sensor to indicate this.

5.3.3 Probability Mapping

This subsystem is responsible for creating a map where the probability that each point has beenvisited is stored. New estimates of Balrog’s positions and their uncertainties will be used toupdate the map. Old probabilities must be updated when new information arises. The map willhave a resolution of 10 cm. The probability map will be implemented according to last yearstechnical documentation [6].

5.3.4 Mine Detection and Mapping

The Mine Detection and Mapping subsystem uses data from the magnetometer to detect mines.The period during which Balrog detects mines (high magnetometer output) will be recorded.The position of a mine will be estimated from Balrog’s position, compared to the position ofprevious detected mines. This will not be changed from last year’s implementation.

5.3.4.1 Known Issues

For a better position estimate, mine mapping could be integrated into the filter in a mannersimilar to obstacle detection. As the improvement from this is expected to be small, it will notbe done. But it could be worth doing as treating all sensor data the same way would makethe system more modular and easier to understand. Also, it would enable mapping using themines as landmarks, which would be especially helpful in harsh outdoor conditions with noGPS coverage.

TSRT10Oscar Hörberg

Ross [email protected]

LIPsPage 26

Page 32: Design Specification - Linköping University · the mine sweeping functionality and improved the crawlers positioning. The main goal of the project in 2012 was to verify that the

Autonomous Mine SweeperLiTH

2015-10-05

5.3.5 Implementation and flow in subsystem

Information flows in to the positioning and mapping subsystem from the SDSP subsystem andfrom the predefined map, if available. The data is sent to the particle filter. It estimates Balrog’sstates and an obstacle map (if no predefined is available). The states are used by the probabilitymapping system to estimate the probability that Balrog has visited each part of the map. It isalso used by the mine mapping system, together with the magnetometer readings, to map thedetected mines. An overview of the system was given in Figure 12 earlier.

The positioning and mapping system will be implemented stepwise. The aim will be to havethe subsystem upp and running as quickly as possible and then expand it by adding more sensormodels.

• The motion model will be implemented, and the filter will be designed to use the IMUdata. Balrog’s position will be estimated from only this.

• A sensor model of the odometer readings will be added. Odometer and IMU data will befused for state estimates.

• A sensor model for the gyroscope will be added.

• A sensor model for the GPS will be added.

• A sensor model for the laser scanner will be added. This model will require that thepredefined map is implemented as well.

• A sensor model for ultrasonic sensors will be added. It will also use the predefined map.

• Support for obstacle detection with no predefined map will be added.

The probability mapping system will be implemented in parallel with the filter. The mine de-tection and mapping subsystem is beyond the scope for this years project and will therefore notbe modified.

5.3.6 Dependencies to other subsystems

The Positioning and Mapping Subsystem is dependent on data from the SDSP subsystem. Ifa predefined map is provided, the subsystem is dependent on that his map is correct, and maperrors should be as small as possible.

TSRT10Oscar Hörberg

Ross [email protected]

LIPsPage 27

Page 33: Design Specification - Linköping University · the mine sweeping functionality and improved the crawlers positioning. The main goal of the project in 2012 was to verify that the

Autonomous Mine SweeperLiTH

2015-10-05

5.4 Implementation Overview

This section presents the implementation of Balrog. In figure 13 all hardware, subsystems, dataclasses and external systems are presented. Figure 13 shows which parts that communicate andwhich shared data the subsystems need.

Figure 13: Overview of the system and how it communicates

This year’s project’s key concepts for the implementation will be the same as last year’s project’skey concepts:

• Each subsystem must be independent of the other subsystems and must be able to run asa standalone application. All dependencies must be passed in at instantiation. This allowsa high degree of decoupling between the different subsystems and they can be developedin parallel and tested independently of each other.

• Each subsystem can either run periodically or once when they are called.

• Shared data structures can be shared between subsystems. All operations in the shareddata structures must be implemented as if they were atomic to avoid concurrency prob-lems.

TSRT10Oscar Hörberg

Ross [email protected]

LIPsPage 28

Page 34: Design Specification - Linköping University · the mine sweeping functionality and improved the crawlers positioning. The main goal of the project in 2012 was to verify that the

Autonomous Mine SweeperLiTH

2015-10-05

5.4.1 Shared data structure

This year’s project will keep the design of shared data from last year [5]. The data is sharedbetween different subsystems by passing in a reference to the data when the subsystem is in-stantiated. All data manipulation (such as add and get) must be implemented to be safe forconcurrency, race condition etc. In practice this might mean mutually exclusive locks etc.,but the objects using the shared data structures can be ignorant of this and use them as if allmanipulating tasks were atomic.

All shared data structures inherits from ASerializable, meaning it is possible to serialize anddeserialize the object to and from a stream, making it possible to send them via the data link.

5.4.1.1 New data structures

The new subsystem SDSP saves processed sensor data to a new data structure called Processed-Sensor. The new data structure inherits from ASerializable and DataCollection and they will besimilar to the data structure SensorSamples that saves raw sensor data. The ProcessedSensorhas, in addition to everything SensorSamples has, a new data member that holds informationwhether the processed sensor values are accurate or inaccurate. The data member is set by theSDSP and is described in section 5.2.2. The new structure will be able to send and receive datafrom the base station. Other subsystems like Positioning and Mapping will if instantiated witha reference to ProcessedSensor be able to get values from the data structure.

5.4.2 Subsystems

Implementation of subsystems will follow the design from last year [5]. Subsystems inheritfrom the class ARunnable and are all completely isolated subsystems. As long as they areinstantiated with the correct parameters (such as references to shared data structures and othersubsystems), they must be able to run independently of the other subsystems. This allows ahigh degree of decoupling between subsystems and they can easily be tested and developed inparallel.

Each subsystem runs in its own thread. The thread instantiation is taken care of automaticallyin the ARunnable class and is nothing we need to consider when using the objects. Usuallysome of the shared data structures are passed as arguments to the constructor. A subsystemmight need to be able to read the shared data or even to manipulate it.

5.4.2.1 New subsystem

The SDSP subsystem is a completely new subsystem and will be implemented from scratch thisyear. The purpose of SDSP is to supply the Positioning and Mapping with reliable sensor data.SDSP should process and analyse sensor data and label the data as outlier/not outlier. It shouldalso be able to tell whether the sensors are reliable/unreliable/not available.

TSRT10Oscar Hörberg

Ross [email protected]

LIPsPage 29

Page 35: Design Specification - Linköping University · the mine sweeping functionality and improved the crawlers positioning. The main goal of the project in 2012 was to verify that the

Autonomous Mine SweeperLiTH

2015-10-05

A Appendix

References

[1] LIPS – nivå 1. Version 1.0. Tomas Svensson och Christian Krysander. Compendium,LiTH, 2002.

[2] MTi User Manual, Xsens Technologies B.V., Netherlands, Document MT0605P, RevisionF, 27 February 2015.

[3] MT Low Level Communication Documentation, Xsens Technologies B.V., Netherlands,Document MT0101P, Revision T, 27 February 2015

[4] Requirement Specification, Marcus Bäck, http://www.isy.liu.se/edu/projekt/tsrt10/2014/bandvagn/Documents/Requirement_specification_v1.0.pdf , Revision 1.0, 19 September 2014

[5] Design Specification, Johan Källström, http://www.isy.liu.se/edu/projekt/tsrt10/2014/bandvagn/Documents/TSRT10___Designspec.pdf , Revision 1.0, 27 September 2014

[6] Technical Documentation, Marcus Bäck, http://www.isy.liu.se/edu/projekt/tsrt10/2014/bandvagn/Documents/Documents/TechnicalDocumentation.pdf , Revision 1.0, 11 December 2014

[7] SRF10 Ultrasonic range finder Technical Specification, Robot Electronics, http://www.robot-electronics.co.uk/htm/srf10tech.htm

[8] BL233 Datasheet, I2Cchip http://www.i2cchip.com/pdfs/bl233_b.pdf

[9] System Identification Toolbox, MATLAB http://se.mathworks.com/help/ident/

[10] Statistical Sensor Fusion, Fredrik Gustafsson, Studentlitteratur AB, 2012

TSRT10Oscar Hörberg

Ross [email protected]

LIPsPage 30