Design Specification
Autonomous Mine Sweeper
LiTH
2015-10-05
Design Specification
Editor: Sofie Griph
Version 1.0
Status
Reviewed
Approved
TSRT10
Oscar Hörberg
Axel Halldin
Hanna Nyqvist
2015-10-05
2015-xx-xx
Ross Haj
[email protected]
L IPs
Page 1
LiTH
2015-10-05
Autonomous Mine Sweeper
PROJECT IDENTITY
2015/HT, Ross Haj
Linköping University, Dept. of Electrical Engineering (ISY)
Group members
Name
Axel Halldin
Oscar Hörberg
Sofie Griph
Robert Svensk
Samuel Örn
Henrik Fåhraeus
Jonas Hyllengren
Responsibility
Project Leader (PL)
Head of Documentation (DOC)
Head of Design
Head of Software
Head of Hardware
Head of Tests
Head of Sensor Fusion
Phone
070-3035052
076-1308547
070-9413964
070-6343977
070-3724874
073-7270416
073-0858131
Email
[email protected]
[email protected]
[email protected]
[email protected]
[email protected]
[email protected]
[email protected]
Email list for the whole group: [email protected]
Web site: not specified yet
Customer: Saab Bofors Dynamics, Linköping
Customer contact: Torbjörn Crona, [email protected]
Course leader: Daniel Axehill, 013-284042, [email protected]
Client: Hanna Nyqvist, 013-281 353, [email protected]
Tutors: Martin Lindfors, 013-281365, [email protected]
Björn Johansson, [email protected]
Carl Nordheim, [email protected]
TSRT10
Oscar Hörberg
Ross Haj
[email protected]
L IPs
Page I
Autonomous Mine Sweeper
LiTH
2015-10-05
Contents
Document history
1
2
3
IV
Introduction
1
1.1
1
Project history . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
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
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
Ultrasonic sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . .
15
5.1.2.1
Sensor interface . . . . . . . . . . . . . . . . . . . . . . . .
15
5.1.2.2
Communication protocol . . . . . . . . . . . . . . . . . . .
16
Implementation and flow in subsystem . . . . . . . . . . . . . . . . . .
16
5.1.2
5.1.3
TSRT10
Oscar Hörberg
Ross Haj
[email protected]
L IPs
Page II
Autonomous Mine Sweeper
5.1.4
5.2
5.3
5.4
LiTH
2015-10-05
Dependencies to other systems . . . . . . . . . . . . . . . . . . . . . .
17
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
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
Implementation Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . .
28
5.4.1
Shared data structure . . . . . . . . . . . . . . . . . . . . . . . . . . .
29
5.4.1.1
New data structures . . . . . . . . . . . . . . . . . . . . . .
29
Subsystems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
29
5.4.2.1
29
5.4.2
New subsystem . . . . . . . . . . . . . . . . . . . . . . . .
A Appendix
30
References
30
TSRT10
Oscar Hörberg
Ross Haj
[email protected]
L IPs
Page III
LiTH
2015-10-05
Autonomous Mine Sweeper
Document history
Version
0.1
0.2
0.3
Date
2015-10-05
2015-10-10
2015-10-14
TSRT10
Oscar Hörberg
Changes
First draft
Second draft
Third draft
Sign
all
all
SÖ, RS, SG, HF, JH
Ross Haj
[email protected]
Reviewed
SG, RS, SÖ, OH
all
SG, HF
L IPs
Page IV
Autonomous Mine Sweeper
1
LiTH
2015-10-05
Introduction
The Autonomous Mine Sweeper project has been running since the spring of 2009. The goal is
to construct a prototype of an autonomous mine sweeping crawler. It is a joint project between
Saab Bofors Dynamics and Linköping University.
The purpose of this document is to give a detailed description of the subsystems that are going
to be changed or added during this year’s project. For a thorough description of previously
implemented subsystems, see the previous years’ "Design Specification" [5] and "Technical
Documentation" [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 remote
control of the crawler and automatic control for the propulsion. The crawler was also equipped
with GPS.
The next autumn (2010) the group 8Yare continued by mounting an industrial computer on the
crawler and ported the old code to the new computer. The crawler was also mounted with stereo
cameras (model Bumblebee 2).
In 2011 group iMAP focused on using the camera and the crawlers sensors to create a 3D map
of the operating area.
In 2012 Minenmarker choose not to continue the work with the 3D map. Instead they developed
the mine sweeping functionality and improved the crawlers positioning. The main goal of the
project in 2012 was to verify that the entire area had been searched and that all the mines were
found. Minenmarker also named the robot Balrog.
In 2013 the group Ostende Abscondita continued to improve the positioning and search algorithm. 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 Balrog’s positioning. Balrog was equipped with a 360°laser sensor and a more powerful industrial
computer.
This year, 2015, the main goal is to improve the positioning of Balrog. To help with this task a
better IMU-unit, also including a barometer, will be mounted on Balrog.
TSRT10
Oscar Hörberg
Ross Haj
[email protected]
L IPs
Page 1
LiTH
2015-10-05
Autonomous Mine Sweeper
2
System Overview
The mine sweeping system is divided in three main subsystems, the Base Station, the Hand
Controller and Balrog. Balrog is a small crawler that consists of different subsystems. Each
subsystem performs specific tasks and includes hardware, software or both. For a schematic
overview of the system see figure 1. The arrows tell how the communication flows in the
system.
Sensors
Balrog
Hand Controller
IMU
Wireless link
Laser scanner
GPS
Ultrasonic
Barometer (in IMU)
Sensor Diagnosis and
Signal Processing
Signal pre-processing
Propulsion
Sensor diagnosis
ARM processor
Electric Motor
Positioning and
Mapping
Mine detection
Obstacle detection
External Communication API
Odometer
Control
GUI
Wireless link
Description
Information Flow
Hardware
Software
Component
Navigation filter
Map/Mapping
Base Station
Route planning
Figure 1: Illustration of the system and its subsystems. The communication flow in the system
is illustrated by arrows.
This year’s project will focus on the subsystems "Sensors", "Sensor Diagnosis and Signal Processing" and "Positioning and Mapping". The design goal is to keep as much as possible from
last year, and therefore the other subsystems will not be changed unless it is necessary for the
system to work well. The Base Station will be modified to meet the requirements in the "Requirement Specification" [4].
TSRT10
Oscar Hörberg
Ross Haj
[email protected]
L IPs
Page 2
Autonomous Mine Sweeper
2.1
LiTH
2015-10-05
System flow
In this section the system flow will be described and the focus will be according to the design goal mentioned above. In other words, the data that is passed between the subsystems
Route Planning, Control and Propulsion or between them and other subsystems will be kept the
same as in last year’s project if possible. Therefore, only the subsystems where the data flow
will be changed considerably are described below. To understand the flow between the other
subsystems, 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 sent
to 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 a
barometer. 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 purpose
is to provide the navigation filter with high quality data. The system will run marginally faster
than the filter, meaning fresh data will always be available for the filter. The pre-processed
data from all sensors will be put together and stored in a shared data structure on Balrog. Each
processed sensor sample is stored together with a time stamp, a flag that tells if the sensor is
reliable/unreliable/not available and a flag that marks if the data is an outlier or not, see figure
2. If a new sensor value cannot be updated due to that the filter executes faster than the sensor
can be sampled, the sensor value will be flagged as not available. The package of pre-processed
sensor 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 the
sensor diagnosis. The SDSP subsystem can also receive commands from the Base Station
telling the SDSP subsystem to treat a sensor as reliable/unreliable.
Sensor 1
Processed Timestamp
sample
Flag:
outlier/inlier
Flag:
reliable/unreliable/unavailable
Sensor N
Processed Timestamp
sample
Flag:
outlier/inlier
Flag:
reliable/unreliable/unavailable
Figure 2: Schematic of a package of processed sensor data written by SDSP. Packages will be
stored in a shared data structure. The filter will use the latest package as input.
TSRT10
Oscar Hörberg
Ross Haj
[email protected]
L IPs
Page 3
Autonomous Mine Sweeper
2.1.3
LiTH
2015-10-05
Positioning and Mapping
The Positioning and Mapping subsystem will use the pre-processed data from the Sensor Diagnosis and Signal Processing subsystem as input to the navigation filter, obstacle detection, mine
detection and mapping (if there is no predefined map). In this year’s project the mine detection
will 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. There
will be an obstacle map, a probability map and a mine map. All of these maps should be
available for the Route Planning subsystem and sent to the Base Station. The estimated states
delivered 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 following
data from Balrog:
• Raw sensor data with time stamp (from Sensors)
• Pre-processed sensor data with time stamp and flag (from Sensor Diagnosis and Signal
Processing)
• 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 following
data 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 1
according to existing implementation [6, sect. 4.1]. There are some new messages being passed
between the base station and Balrog, the id of those messages can be seen in Table 2 and
Table 3. The payloads of the new sensor status messages sent from the base station to Balrog will be one of three defined modes with id; SENSOR_STATUS_ALWAYS_USE, SENSOR_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 commands to Balrog (to Control) making it move.
TSRT10
Oscar Hörberg
Ross Haj
[email protected]
L IPs
Page 4
Autonomous Mine Sweeper
ID
uint16_t id
Payload length
uint16_t len
LiTH
2015-10-05
Payload
char payload[len]
Table 1: Data format for a message between Balrog and the base station
ID
NETWORK_MESSAGE_PROCESSED_DATA
Description
Sends the buffer of processed data
from SDSP periodically
Table 2: New messages sent from Balrog to base station
ID
NETWORK_MESSAGE_OBSTACLE_MAP
NETWORK_MESSAGE_IMU_STATUS
NETWORK_MESSAGE_ODOMETER_STATUS
NETWORK_MESSAGE_LASER_STATUS
Description
Sends the predifinied map structure
Sends a command to set the IMU in
a specific state
Sends a command to set the odometer in a specific state
Sends a command to set the laser in
a specific state
Table 3: New messages sent from the base station to Balrog
TSRT10
Oscar Hörberg
Ross Haj
[email protected]
L IPs
Page 5
Autonomous Mine Sweeper
3
LiTH
2015-10-05
Base station
The base station consists of a laptop communicating with Balrog via WiFi. The purpose of the
base station is to receive data from Balrog and send predefined commands to Balrog. The data
received 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 from
the base station. Since only manual operation will be considered this year the manual operation
of 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 operation
is already implemented, [6, sect. 4], so these functions will be tested and minor fixes applied if
necessary.
The main focus will be to develop a way to easily present all the data passed between modules
inside the Balrog system. This will be very helpfull when designing other parts of the system
but also good for evaluating missions done by Balrog.
The development will be based on earlier years work and some functions will be reimplementation 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 Balrog
from 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 settings
at the left, the map in the center and way points at the right. See figure 3. There are some
more functions of the base station available in the ’File menu’ in the upper left corner of the
applet. These functions are Connect, Disconnect, XBoxConfiguration, ControllerConfiguration
and FilterConfiguration.
Figure 3: Current layout of the GUI
TSRT10
Oscar Hörberg
Ross Haj
[email protected]
L IPs
Page 6
Autonomous Mine Sweeper
LiTH
2015-10-05
The current layout of the GUI will be kept as it is for the most part with some minor changes
to make place for new functionality. Changes to the GUI are explained further in 3.1.5. The
underlying source code will be overlooked and redesigned to make future implementation easier
to 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 the
button to switch mode between automatic and manual control which can be found on the far left
side of the bar. Expansions to the status bar will be status fields for each sensor on Balrog telling
the 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 can
be 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 data
presentation. There will be a button named ’Sensor Data’, a layout sketch can be seen in figure
4.
3.1.3
Map section
Since the focus this year is navigation/localization this part of the GUI will be made operable
again and used for evaluating the algorithms developed by the team. A map created by a combination of the maps sent from Balrog and the predefined map will be displayed along with
Balrog’s current position on it.
The design of the created map will be based on earlier decisions and some new aspects. The
predefined map will be displayed as is in the background and the obstacle map from Balrog will
be drawn over it. They will be drawn with different colors to make it easy for the operator to
make out which is which. The probability map will be displayed as a grid of visited sections
of the obstacle map. The probability map will be presented by displaying the probability that
Balrog has been in a certain grid of the map by colouring the specific grid in different shades
between white and green. Each grid is white from the beginning and becomes more green as
the 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’s
focus.
3.1.5
New functions in the GUI
There are three new functions that are going to be integrated to the current GUI. These three
functions are disable/enable sensors, input a predefined map, and display current sensor data
passed between the modules on Balrog.
TSRT10
Oscar Hörberg
Ross Haj
[email protected]
L IPs
Page 7
Autonomous Mine Sweeper
LiTH
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 sections
3.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 the
location of a file defining the map. The map will be defined as start- and end-points of each
line of an object. Each point will be represented in cartesian coordinates and connected to
each-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 button
in the ’Settings section’ called ’Sensor Data’ which will open a new window containing graphs
and 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 selecting
manual/automating mode for each sensor. To toggle a sensor the operator simply pushes the
TSRT10
Oscar Hörberg
Ross Haj
[email protected]
L IPs
Page 8
Autonomous Mine Sweeper
LiTH
2015-10-05
Figure 6: Sketch of the ’Sensor Data’ GUI
button corresponding to the desired mode. Selecting a new mode will automatically deselect
the 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 the
laser sensor and the GPS. This is true for both raw sensor data and sensor data processed by the
SDSP. The processed sensor data graphs will also show where the data is seen as unreliable by
drawing the drawing the line in a different color or another fashion so that it is obvious for the
operator. The processed data and corresponding raw data will be plotted in the same graph for
easy comparison.
The current position from the GPS will be printed as values and draw in a scatter plot marking
each position with a time stamp.
The data array from the laser sensor will be translated to a scatter plot plotting the current
measurements relative to Balrog.
A rough sketch of the layout of these graphs and values can be seen in Figure 6.
TSRT10
Oscar Hörberg
Ross Haj
[email protected]
L IPs
Page 9
Autonomous Mine Sweeper
4
LiTH
2015-10-05
Hand control
The hand controller is used to operate Balrog in manual mode. The controller is a Xbox 360
control communication over the 2.4 GHz band with a dongle connected to and located on Balrog.
Since most of the functionality to manually operate Balrog already exist it will be tested and
only changed if needed. Some of the functions recommended by last years team may be looked
at. Probable examples of functions that will be implemented are:
• ’Set Balrog to manual mode’ - Switch between manual and automatic operation using the
hand controller
• ’Stop Balrog’ - Stop all actions of Balrog
TSRT10
Oscar Hörberg
Ross Haj
[email protected]
L IPs
Page 10
LiTH
2015-10-05
Autonomous Mine Sweeper
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 and
its environment. All sensors but the odometer are directly connected to the main processing
unit. 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 Design
Specification 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 physical quantity
Angle velocity,
heading
Velocity, acceleration
Heading, mine
detection
Output
Max frequency
2000 Hz, 400 Hz
Standard
range
0-450
deg/s
0-50 m/s2
2000 Hz, 400 Hz
100 Hz
0-80 T
50 Hz
Barometer
Altitude
deg/s or delta angle
m/s2 or delta velocity
au
(arbitrary
units, normalized
to earth field
strength)
Pa
Temperature sensors
Laser sensor
-
°C
1 Hz
300-1100
hPa
-
Distance (to object)
2000 Hz
0-6 m
Ultrasonic
sors
GPS
Distance (to object)
Position
Point
cloud
([mm, degrees]
measured from
the
sensor’s
center)
cm
15 Hz and up
WGS84 position,
number of satellites used, accuracy
mm, mm/s
<4 Hz
0-6 m (11
m max)
-
Gyroscopes
Accelerometers
Magnetometer
sen-
Odometer
Distance (travelled), speed
Event-based
1.07mm
(max
resolution)
Table 4: Sensors that will be mounted on Balrog
TSRT10
Oscar Hörberg
Ross Haj
[email protected]
L IPs
Page 11
Autonomous Mine Sweeper
5.1.1
LiTH
2015-10-05
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’s
Balrog. 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 stated
otherwise.
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 power
to the IMU.
5.1.1.2
Communication
An SDK package is provided by Xsens, this contains the Xsens Device API which can be used
for communication with the MSi 100. The API is written in C and compatible with Windows
and Linux systems, but a C++ wrapper is also available and will be used in this project. Important to note is that the API works for all of Xsens’ sensors and not just MTi 100. This means
some of the functions in the API will not provide a meaningful return value, because they target
a 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 to
display measurements and to perform calibration. However, these applications are only available 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 built
in signal processing software. The unit contains an internal 16 bit ADC to convert the analog
sensor values to digital. There is an option to get the raw, unprocessed (but digitalized) sensor
values as output. See section 5.1.1.5 for specifications of what output the MTi 100 can provide.
TSRT10
Oscar Hörberg
Ross Haj
[email protected]
L IPs
Page 12
LiTH
2015-10-05
Autonomous Mine Sweeper
5.1.1.4
Placement on Balrog
To get the best performance from the gyroscopes and accelerometers the IMU should be placed
to minimize transient accelerations. Thus it should be placed as close to the rotational axis
of 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 to
minimize noise in the magnetometer. This condition will most likely prove hard to fulfill, since
Balrog’s electrical motors will generate a varying magnetic field depending on how much power
is fed to them by the control unit. Smaller magnetic disturbances from all other electronics will
also be present.
The IMU-unit should be firmly mounted, so that it always follows the movement of Balrog. To
have 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 should
be either mounted or rotated (software calibration) so its coordinate system is aligned with
Balrog’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.
TSRT10
Oscar Hörberg
Ross Haj
[email protected]
L IPs
Page 13
LiTH
2015-10-05
Autonomous Mine Sweeper
Y
Front
X
Z
Back
Figure 8: Balrog’s coordinate system, seen from above
Sensor
Type
Unit
Max frequency
Gyroscopes
[1]
2000 Hz
[1]
2000 Hz
50 m/s2
Magnetometer
Analog sensor, 16
bit ADC
Analog sensor, 16
bit ADC
Digital sensor
Standard
range
450 deg/s
100 Hz
80 T
Barometer
Digital sensor
50 Hz
Temperature sensors
Analog sensor, 12
bit ADC
300-1100
hpa
-
Accelerometers
au
unit)
Pa
°C
(arbitrary
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 doing
a 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 specific
time 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 that
this will most likely introduce a drift. Table 6 shows delta velocity and delta angle. [2, sec.
4.9.3, p. 48]
TSRT10
Oscar Hörberg
Ross Haj
[email protected]
L IPs
Page 14
Autonomous Mine Sweeper
Output
Delta_q (DataID 0x8030)
Delta_v (DataID 0x4010)
LiTH
2015-10-05
Unit
[1] (a.u., quaternion values)
m/s
Table 6: Delta velocity and delta angle. a.u. stands for arbitrary units, normalized to earth field
strength
Calibrated inertial and magnetic data outputs
The MTi 100 can output preprocessed 3D linear acceleration, 3D rate of turn (gyro) and 3D
magnetic field data. All values are in a sensor-fixed coordinate system. The calibrated data
has been going through Strapdown Integration and Inverse Strapdown Integration. Table 7
illustrates the calibrated inertial and magnetic data output.
Vector
Acceleration
Angular velocity (rate of turn)
Magnetic field
Unit
m/s2
rad/s
a.u.(arbitrary units, normalized 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 are
of type SRF10 and the specifications for the sensors can be found in [7]. The sensors shall be
mounted on Balrog as shown in figure 9 below. The will be two sensors at the front to get good
measurements 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-Integrated
Circuit) bus. Through the I2C bus multiple sensor units are able to communicate on the same
physical wire. Each ultrasonic sensor are given a unique address that specifies which sensor is
to be read or written to at the moment, see Table 8. The main control system at the Balrog does
not have any interface for communication through I2C. Thus it use a USB-I2C converter unit.
TSRT10
Oscar Hörberg
Ross Haj
[email protected]
L IPs
Page 15
Autonomous Mine Sweeper
LiTH
2015-10-05
β
Figure 9: Layout of the positions of the ultrasonic sensors
Direction
Forward
Position
Left
Right
Left
Right
Address
0xE2
0xE6
0xEA
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 I2C
bus. The I2C-USB converter are controlled by a simple text based protocol which is specified
in [8]. The ultrasonic sensors have four different internal register which that can be written to
and read from through the I2C bus.
A distance measurement is started by sending a command to the communication register of
a sensor. The result will be read from the result register of the sensor in a predetermined
frequency. 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 sensor
data and setting parameters in the sensor’s on board processing unit. All measurements that is
fetched by the API will be timestamped and stored in a shared data structure available to other
subsystems within Balrog.
See Figure 10 for a schematic of the flow.
TSRT10
Oscar Hörberg
Ross Haj
[email protected]
L IPs
Page 16
LiTH
2015-10-05
Autonomous Mine Sweeper
Hardware
Sensor
Software
On board processing
Globally accessible
data structure
API
Data log
User
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 own
thread without any considerations.
TSRT10
Oscar Hörberg
Ross Haj
[email protected]
L IPs
Page 17
Autonomous Mine Sweeper
5.2
LiTH
2015-10-05
Sensor Diagnosis and Signal Processing (SDSP)
The SDSP subsystem will be added by this year’s project in order to perform sensor modelling
and sensor diagnosis. All pre-processing and synchronization of sensor data will take place
here. 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 and
model the noise. If the noise is non-Gaussian, an inverse filter will be modelled in order to get
a signal with white Gaussian noise (WGN). The navigation filter in Positioning and Mapping
will work better if the noise is WGN.
Multiple data sets will be collected before modelling, so that the resulting models describe
reality as well as possible.
The data sets from each sensor will be examined using periodograms in order to see whether
respective noise is considered as white or non-white. To be able to tell if the noise is Gaussian, different plot tools in MATLAB will be used (ndist, normplot etc.). The variances and
covariances 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. This
will be done by examining the noise using linear models in MATLAB’s System Identification
Toolbox [9]. The inverse filter that will be used if the noise is non-Gaussian will most likely be
estimated using an ARMA or AR model. In this case, the data sets will be divided into model
data and validation data.
The data sets will also be analysed in order to find an eventual bias in the sensor. It will only be
possible to know if there is a bias for the sensors that are measuring something with a known
ground truth, for example a distance sensor. The sensor data will be compensated for the bias
if it is necessary. Dependent on how the sensor works, the bias will be estimated off-line or
on-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 ground
truth of what the sensor is measuring - especially for a dynamic input signal, the sensor modelling will be of a constant-but-unknown-input kind (the ground truth is not known). The model
will therefore not examine the dynamics of the sensors and will only examine its stationary
properties.
In cases where measurable noise contributors (disturbances) can be identified, the noise will
be modelled based on these contributors. One example of such a contributor is the electric
propulsion motors that are likely to create disturbances in the magnetometer measurements.
Experiments
Constant-but-unknown-input experiments will be conducted by collecting sequences of sensor
data while the position of Balrog is constant.
Experiments with disturbance-source input will instead be conducted when the disturbance is
present in order to estimate the effect of it on the signal. For example, to model the signal noise
when 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.
TSRT10
Oscar Hörberg
Ross Haj
[email protected]
L IPs
Page 18
LiTH
2015-10-05
Autonomous Mine Sweeper
Sensor
Laser scanner
Ultrasonic sensors
Accelerometer
Gyroscope
Magnetometer
Constant-but-unknowninput experiment
Yes
Yes
Yes
Yes
Yes
Barometer
Odometer
Yes
Yes
Disturbance-source experiment
No
No
No
No
Constant reference signal to
the electrical propulsion motors
No
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 sensor
is unreliable/reliable. Data from unreliable sensors will not be utilized in the Positioning and
Mapping subsystem. Neither will outliers be available for that subsystem. To decide if a sensor
works well or not, outlier rejection will be used. If a sensor gives too many outliers, based on
a design parameter for that specific sensor, it will be marked as unreliable. A sensor that has
been marked unreliable can be reset to reliable if it seems to work well again. Another design
parameter will be needed for that decision.
There will be some cases when sensor data must be flagged not available. This happens when
the filter in Positioning and Mapping executes faster than some of the sensors sample. This flag
will 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 the
probability density function p(yk+1 |y1:k ) for each sensor. The same main idea for outlier rejection 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 , the
covariance P l,i and the weight wi . This information together with the general model structure
of the marginalized particle filter [10] gives that a measurement y is distributed as a Gaussian
mixture:
y ∼ p(yk+1 |y1:k ) ≈
N
X
n,i
l,i
n,i
l,i
n,i
T
i
N (hk (xn,i
wk+1|k
k+1|k )+Hk (xk+1|k )x̂k+1|k , Hk (xk+1|k )Pk+1|k Hk (xk+1|k ) +R)
i=1
(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
TSRT10
Oscar Hörberg
Ross Haj
[email protected]
(2)
L IPs
Page 19
Autonomous Mine Sweeper
LiTH
2015-10-05
All sensor values that do not fullfill the test will be treated as outliers. By changing the value of
L, it will be possible to tune the test. Not all sensors will be analysed according to (2) and they
are described in the following sections.
Odometer and accelerometer
The odometer is unlikely to suffer from big outliers and the test is then unnecessary. The
odometer have larger variance when driving due to slip but will be highly reliable when standing
still. 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 will
be performed for the magnetometer. Code skeletons will be written to facilitate for next year’s
project group and it will be possible to get magnetometer data and set it as unreliable/reliable/not
available. 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 used
in the outlier test. The height on the map at the position given by the filter will be compared
with the height given by the barometer measurement.
Basic outlier rejection
Besides the outlier rejection mentioned above some sort of basic outlier rejection will be made
for some of the sensors (when it is considered to be necessary). This means that an upper and/or
a 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 that
corresponds 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 same
physical quantity that is given by the sensor but might be scaled to other units for example from
m/s to mm/s. If the filter in Positioning and Mapping wants a different physical quantity, it has
to convert the data itself. One example is to calculate balrog’s vertical speed from barometer
data.
5.2.4
Implementation and flow in subsystem
The SDSP subsystem includes the following:
1. Filtering of non-Gaussian noise
2. Sensor diagnosis
TSRT10
Oscar Hörberg
Ross Haj
[email protected]
L IPs
Page 20
Autonomous Mine Sweeper
LiTH
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 order
to get a signal with Gaussian noise. After that, SDSP will store the output in the same data
format as the unfiltered sensor data. This data will be used by the sensor diagnosis and outlier
rejection.
The SDSP is also a synchronizing barrier between the sensors and the filter, thus ensuring that
the filter obtain data from the same time period for all available sensors. This means that the
SDSP 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 number
of available sensor data, however, more fresh (available) data will likely make the filter output
more accurate. The data synchronization will set the reliable/unreliable/unavailable-flag to
unavailable 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 check
the flag whether data has been updated since the last read and can therefore ignore data with
the unavailable flag. The filter will access all sensor data regardless of the condition of the
reliable/unreliable/unavailable-flag.
The base station has the possibility to set the reliable/unreliable/unavailable-flag for any sensor
to a fixed state through public methods. This means that the sensor diagnosis will have the
permission to update the flag. The flag can also be unset so that the sensor diagnosis will be
able to update the flag. This will be implemented through a private flag which will or will not
give 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 executed independently from each other and can be run on the same thread as its corresponding
sensor. This means that every sensor filtering and diagnosis will independently fetch the latest
filter output available.
TSRT10
Oscar Hörberg
Ross Haj
[email protected]
L IPs
Page 21
LiTH
2015-10-05
Autonomous Mine Sweeper
Data from filter
Inverse noise
model filtering
Sensor diagnosis
Inverse noise
model filtering
Sensor diagnosis
IMU data
Inverse noise
model filtering
Sensor diagnosis
Barometer data
Inverse noise
model filtering
Sensor diagnosis
GPS Data
Inverse noise
model filtering
Sensor diagnosis
Odometer data
Inverse noise
model filtering
Sensor diagnosis
Inverse noise
model filtering
Sensor diagnosis
Laser scanner
data
Ultrasonic
sensor data
Magnetometer
data
Data
synchronization
Data to filter
Propulsion
reference
signal data
Figure 11: Flow of the SDSP subsystem. The arrows shows the communication.
TSRT10
Oscar Hörberg
Ross Haj
[email protected]
L IPs
Page 22
LiTH
2015-10-05
Autonomous Mine Sweeper
5.3
Positioning and Mapping
The purpose of the positioning and mapping subsystem is to estimate Balrog’s position and
heading and to create three maps: a map of detected mines, a map of the probabilities that
a points in the search area has been visited and a map of obstacles in the search area (if no
obstacle map has been provided). A sketch of the positioning and mapping system can be seen
in Figure 12 (arrows denote information flow). The positioning and mapping subsystem was
called the SLAM subsystem in the 2014 "Technical Documentation" [6].
Filter
Predefiend
Obstacle Map
Processed
Sensor Data
Mine Detection
Balrog States
Mine Mapping
Magnetometer
Data
Probability
Mapping
Probability Map
Mine Map
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 sensors
are 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 readings
taken since last filter iteration. This gives the change in pressure during the time since last filter
iteration, 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 done
by comparing the GPS coordinates to the GPS coordinates of a given point in the obstacle map
and 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
TSRT10
Oscar Hörberg
Ross Haj
[email protected]
L IPs
Page 23
LiTH
2015-10-05
Autonomous Mine Sweeper
will be added, and numerical issues from last year will be fixed. For a full description of the
filter algorithm and its implementation (including the loop parallelisation and the numerical
issues), 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 sensor
type. Parts of the accelerometer and gyroscope data will be regarded as input in the motion
model, and all other data will be regarded as outputs from the sensor models. It will be possible
to predefine an obstacle map in Balrog. If this is done, Balrog will compare the range sensor
readings with this map to estimate its states. The given map will be regarded as absolute and
will not be changed by Balrog. If no map is predefined, Balrog will do simultaneous localization
and mapping with a dynamic map that will be updated as new data from the obstacle detection
system 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 2D
motion model with polar velocity. The model will also contain the roll and pitch of Balrog, as
these states are useful in modelling the accelerometer. The states at time k, xk are given below:


xk
 yk 
 
 vk 
 

xk = 
φ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:
 T2
cos(ψ)
xk + T vk cos(ψk )
2
 yk + T vk sin(ψk )   T 2 sin(ψ)

  2

 
v
T
k

 

+
φk
=
0
 



θk
0

 

 
ψk + T ωk
0
ωk
0

xk+1

T2
cos(ψ)
0 0
2
T2


0 0
 2 sin(ψ)


T
0 0


0
T 0  uk + 



0
0 T



0 0
0
0 0
0
0
0
0
0
0
T2
2
T

0 0
0 0

0 0

T 0
 wk (4)
0 T

0 0
0 0
The vector uk contains the system inputs. u1k is given by the equation u1k = ak − θk g (g is the
gravitational 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 by
the gyroscope) and u3k is the pitch angle speed (measured by the gyroscope). The vector wk
contains the process noise. wk1 is the IMU acceleration noise, wk2 is the yaw angle acceleration
(modelled as noise), wk3 is the noise in the roll speed measurement and wk4 is the noise in the
pitch speed measurement.
TSRT10
Oscar Hörberg
Ross Haj
[email protected]
L IPs
Page 24
LiTH
2015-10-05
Autonomous Mine Sweeper
5.3.2.2
Measurement Model
As the SDSP subsystem will handle the more complicated sensor modelling (subtracting biases
etc.), only simple sensor models will be needed. The sensor inputs are:

XGP S
 YGP S 


 ωgyroscope 

 v +v
 R L 

 v −v
2
 R L 


dv

v
 h,barometer 

 r
 laser,1 
yk =  r

 laser,2 


..


.


 rlaser,360 


 rultrasonic,1 


 rultrasonic,2 


 rultrasonic,3 
rultrasonic,4

(5)
XGP S and YGP S are the GPS position measurements, ωgyroscope is the IMU angle velocity measurements, vR and vL are the odometer measurements, dv is the virtual length of Balrog defined
by the group, vh,barometer is the vertical speed of Balrog calculate from the barometer data by
SDSP, rlaser,j is the LIDAR range measurement for angle j and rultrasonic,k is the ultrasonic
range measurement for sensor k. Only the sensor readings that SDSP deems accurate will be
used in the filter, which means that yk might not contain all of the readings stated above. They
are connected to the states according to:

xk
yk
ωk vk
ωk
θk vk
llaser (1, xj , yj , ψk )
llaser (2, xj , yj , ψk )
..
.























h(xk ) = 







 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)
θk vk 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 state
vector) to the nearest wall in the direction of the measurement. For the laser scanner, each
value of j is the number of degrees at which the measurement was taken, and for the ultrasonic
sensors each k is associated with an angle at which the sensor k is mounted.
TSRT10
Oscar Hörberg
Ross Haj
[email protected]
L IPs
Page 25
Autonomous Mine Sweeper
LiTH
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 direction
becomes (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 (lines
above/below the measurement line).
• Calculate the intersection between the measurement line (0, 0) + (1, 0)t, t > 0 and all
remaining line segments. The answer will be on the form (R, 0), where R is the distance
to the intersection. This distance will be saved. For some lines no intersection will be
found, 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 a
specific 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 been
visited is stored. New estimates of Balrog’s positions and their uncertainties will be used to
update the map. Old probabilities must be updated when new information arises. The map will
have a resolution of 10 cm. The probability map will be implemented according to last years
technical 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 of
previous 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 manner
similar to obstacle detection. As the improvement from this is expected to be small, it will not
be done. But it could be worth doing as treating all sensor data the same way would make
the system more modular and easier to understand. Also, it would enable mapping using the
mines as landmarks, which would be especially helpful in harsh outdoor conditions with no
GPS coverage.
TSRT10
Oscar Hörberg
Ross Haj
[email protected]
L IPs
Page 26
Autonomous Mine Sweeper
5.3.5
LiTH
2015-10-05
Implementation and flow in subsystem
Information flows in to the positioning and mapping subsystem from the SDSP subsystem and
from the predefined map, if available. The data is sent to the particle filter. It estimates Balrog’s
states and an obstacle map (if no predefined is available). The states are used by the probability
mapping system to estimate the probability that Balrog has visited each part of the map. It is
also used by the mine mapping system, together with the magnetometer readings, to map the
detected 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 have
the subsystem upp and running as quickly as possible and then expand it by adding more sensor
models.
• The motion model will be implemented, and the filter will be designed to use the IMU
data. Balrog’s position will be estimated from only this.
• A sensor model of the odometer readings will be added. Odometer and IMU data will be
fused 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 the
predefined 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 detection and mapping subsystem is beyond the scope for this years project and will therefore not
be modified.
5.3.6
Dependencies to other subsystems
The Positioning and Mapping Subsystem is dependent on data from the SDSP subsystem. If
a predefined map is provided, the subsystem is dependent on that his map is correct, and map
errors should be as small as possible.
TSRT10
Oscar Hörberg
Ross Haj
[email protected]
L IPs
Page 27
Autonomous Mine Sweeper
5.4
LiTH
2015-10-05
Implementation Overview
This section presents the implementation of Balrog. In figure 13 all hardware, subsystems, data
classes and external systems are presented. Figure 13 shows which parts that communicate and
which 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’s
key concepts:
• Each subsystem must be independent of the other subsystems and must be able to run as
a standalone application. All dependencies must be passed in at instantiation. This allows
a high degree of decoupling between the different subsystems and they can be developed
in 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 shared
data structures must be implemented as if they were atomic to avoid concurrency problems.
TSRT10
Oscar Hörberg
Ross Haj
[email protected]
L IPs
Page 28
Autonomous Mine Sweeper
5.4.1
LiTH
2015-10-05
Shared data structure
This year’s project will keep the design of shared data from last year [5]. The data is shared
between different subsystems by passing in a reference to the data when the subsystem is instantiated. All data manipulation (such as add and get) must be implemented to be safe for
concurrency, 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 all
manipulating tasks were atomic.
All shared data structures inherits from ASerializable, meaning it is possible to serialize and
deserialize 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 ProcessedSensor. The new data structure inherits from ASerializable and DataCollection and they will be
similar to the data structure SensorSamples that saves raw sensor data. The ProcessedSensor
has, in addition to everything SensorSamples has, a new data member that holds information
whether the processed sensor values are accurate or inaccurate. The data member is set by the
SDSP and is described in section 5.2.2. The new structure will be able to send and receive data
from the base station. Other subsystems like Positioning and Mapping will if instantiated with
a 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 inherit
from the class ARunnable and are all completely isolated subsystems. As long as they are
instantiated with the correct parameters (such as references to shared data structures and other
subsystems), they must be able to run independently of the other subsystems. This allows a
high degree of decoupling between subsystems and they can easily be tested and developed in
parallel.
Each subsystem runs in its own thread. The thread instantiation is taken care of automatically
in the ARunnable class and is nothing we need to consider when using the objects. Usually
some of the shared data structures are passed as arguments to the constructor. A subsystem
might 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 this
year. 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 should
also be able to tell whether the sensors are reliable/unreliable/not available.
TSRT10
Oscar Hörberg
Ross Haj
[email protected]
L IPs
Page 29
Autonomous Mine Sweeper
A
LiTH
2015-10-05
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, Revision
F, 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
TSRT10
Oscar Hörberg
Ross Haj
[email protected]
L IPs
Page 30
Was this manual useful for you? yes no
Thank you for your participation!

* Your assessment is very important for improving the work of artificial intelligence, which forms the content of this project

Download PDF

advertisement