Implementation of SLAM Algorithms in a Small-Scale Vehicle Using Model-Based Development

(1)

Master of Science Thesis in Datorteknik, Fordonssystem

Department of Electrical Engineering, Linköping University, 2017

Implementation of SLAM

algorithms in a small-scale

vehicle using model-based

development

(2)

Master of Science Thesis in Datorteknik, Fordonssystem

Implementation of SLAM algorithms in a small-scale vehicle using model-based development

Johan Alexandersson och Olle Nordin LiTH-ISY-EX–17/5101–SE Supervisor: Peter A Johansson

isy, Linköpings universitet

Simon Tegelid

ÅF Consulting

Examiner: Mattias Krysander

isy_{, Linköpings universitet}

Department of Electrical Engineering Department of Electrical Engineering

Linköping University SE-581 83 Linköping, Sweden

(3)

Abstract

As autonomous driving is rapidly becoming the next major challenge in the auto-motive industry, the problem of Simultaneous Localization And Mapping (SLAM) has never been more relevant than it is today. This thesis presents the idea of examining SLAM algorithms by implementing such an algorithm on a radio con-trolled car which has been fitted with sensors and microcontrollers. The software architecture of this small-scale vehicle is based on the Robot Operating System (ROS), an open-source framework designed to be used in robotic applications.

This thesis covers Extended Kalman Filter (EKF)-based SLAM, FastSLAM, and GraphSLAM, examining these algorithms in both theoretical investigations, simulations, and real-world experiments. The method used in this thesis is model-based development, meaning that a model of the vehicle is first implemented in order to be able to perform simulations using each algorithm. A decision of which algorithm to be implemented on the physical vehicle is then made backed up by these simulation results, as well as a theoretical investigation of each algorithm.

This thesis has resulted in a dynamic model of a small-scale vehicle which can be used for simulation of any ROS-compliant SLAM-algorithm, and this model has been simulated extensively in order to provide empirical evidence to define which SLAM algorithm is most suitable for this application. Out of the algo-rithms examined, FastSLAM was proven to the best candidate, and was in the final stage, through usage of the ROS package gMapping, successfully imple-mented on the small-scale vehicle.

(4)

(5)

Acknowledgments

First and foremost, we would like to thank Simon Tegelid and Magnus Carlsson at ÅF, for endless support whenever needed. Additional accolades go out to our supervisor Peter Johansson at the Department of Electrical Engineering who was very helpful with technical support. We would also like to extend our gratitude to our examiner Mattias Krysander, also at the aforementioned department, who provided an extensive amount of good insight about the subject and also lots of useful hints regarding academic writing in general.

Linköping, October 2017 Johan Alexandersson and Olle Nordin

(6)

(7)

viii Contents 3.2.3 Data association . . . 19 3.3 FastSLAM . . . 20 3.3.1 Particle structure . . . 20 3.3.2 Algorithm . . . 20 3.3.3 FastSLAM 2.0 . . . 21 3.3.4 Computational complexity . . . 21 3.3.5 Data Association . . . 22 3.4 GraphSLAM . . . 23 3.4.1 Algorithm . . . 23 3.4.2 Computational complexity . . . 25 3.4.3 Data association . . . 25

3.5 Theoretical evaluation of SLAM . . . 26

4 Vehicle Dynamics 29 4.1 Longitudinal dynamics . . . 29 4.1.1 Gear ratio . . . 29 4.1.2 Wheel torque . . . 29 4.1.3 Forward velocity . . . 30 4.2 Lateral dynamics . . . 30 4.2.1 Ackermann angle . . . 31 4.2.2 Heading angle . . . 31 4.3 Coordinates . . . 32 5 Sensor theory 33 5.1 Gyroscope . . . 33 5.2 Wheel encoders . . . 34

5.3 Laser range finder . . . 34

6 Simulink Model Implementation 37 6.1 Input . . . 38

6.2 Kinematic Vehicle . . . 38

6.2.1 Velocity . . . 38

6.2.2 Servo . . . 39

6.2.3 Angle & Angular Velocity . . . 39

6.2.4 Position . . . 39 6.3 Map . . . 39 6.3.1 Parameters . . . 39 6.3.2 Implementation . . . 39 6.4 Odometry Sensors . . . 41 6.4.1 Gyroscope . . . 41 6.4.2 Wheel encoders . . . 44 6.5 LIDAR . . . 47 6.5.1 Description . . . 47 6.5.2 Operation . . . 48 6.5.3 Probability of error . . . 50 6.6 ROS Interface . . . 53

(9)

Contents ix 6.6.1 /sensor_data . . . 55 6.6.2 /scan . . . 55 6.6.3 /car_position . . . 55 6.6.4 /clock . . . 55 7 ROS Implementation 57 7.1 CAN Reader . . . 58 7.2 Odometry . . . 58 7.3 SLAM . . . 58 7.4 rViz . . . 59 7.5 Position . . . 60 7.6 simulinkPosition . . . 60

7.7 ROS Control App . . . 61

7.8 Joystick Input . . . 61 7.9 CAN Writer . . . 61 7.10 CPU Recoder . . . 61 8 Simulations 63 8.1 Method . . . 63 8.1.1 Overview . . . 63 8.1.2 Maps . . . 64 8.1.3 Tests . . . 66 8.2 Results . . . 69 8.2.1 Mapping accuracy . . . 69 8.2.2 Path accuracy . . . 72

8.2.3 Average position error . . . 76

8.2.4 Average CPU load . . . 77

8.3 Issues . . . 78 8.4 Conclusion . . . 79 9 Real-world experiment 81 9.1 Overview . . . 81 9.2 Results . . . 83 9.3 Conclusion . . . 86 10 Discussion 87 10.1 Achievements . . . 87 10.2 Method . . . 88 10.3 Complications . . . 88 10.4 Future work . . . 89 Bibliography 91

(10)

(11)

Notation

Abbreviations

Abbreviation Description

ROS Robot Operating System

LI DAR Light detection and ranging

SLAM Simultaneous localization and mapping

I MU Inertial measurement unit

GU I Graphical user interface

AI Artificial intelligence

P DF Probability Density Function

MEMS micro-electro-mechanical systems

LU T Lookup table

CP U Central Processing Unit

P C Personal Computer

CAN Controller Area Network

(12)

(13)

1

Introduction

1.1 The Company

This thesis has been conducted at ÅF in Linköping. ÅF is a Swedish consulting company with over 8000 employees worldwide [6]. ÅF as a company focuses heavily on technology areas such as industry, infrastructure and energy [6]. The thesis work was conducted at ÅF’s technology department.

1.2 Background

In recent times, many companies in the automotive industry have spent a lot of time and funds on researching the possibility of producing autonomous cars. The research has moved forward to the point that there are actually semi-autonomous cars available in the market today, and it is widely believed that at some point in time, almost all vehicles will be autonomous. In order to achieve this, there is obviously a need for advanced sensors and functions. One of the major issues is the fact that the vehicle needs to be aware of its current position within an unknown environment. This is known as the Simultaneous Localization And Mapping problem, commonly abbreviated as SLAM.

As ÅF is a company that works a lot with autonomous driving, a competence project concerning this has been started. The goal with this project is to assemble a small-scale vehicle with embedded sensors and microprocessors, and ment functions which will enable this vehicle to drive autonomously. As imple-menting a SLAM algorithm is a vital part of achieving this, this thesis will focus on finding the best SLAM algorithm for this purpose and implement it in the vehicle.

(14)

2 1 Introduction

1.3 Thesis Scope

The purpose of this thesis is to implement a Simultaneous-Location-And-Mapping (SLAM) algorithm in a small-scale vehicle running the Robot Operating System (ROS). Which SLAM algorithm to be chosen will be supported by a theoretical investigation. Three different algorithms will be investigated, Extended Kalman Filter based SLAM (EKF)[12], particle filter-based SLAM (FastSLAM) [29], and graph-based SLAM (GraphSlam) [11]. FastSlam is implemented in the ROS ages gmapping [2] and coreslam [1], and Graphslam is implemented in the pack-age slam_karto [3]. In addition, simulations will be done on a model created in Simulink representing the actual car and further information regarding the SLAM algorithm implemented in the model will be presented.

The SLAM algorithm is supposed to simultaneously create a map of the vehicle’s environment as well as calculating the position of the vehicle within this map. This data is to be broadcast to another ROS node on a PC, which will show the map and the vehicle’s position in real-time in a GUI.

There will be two different levels of priority in this thesis. The first priority is to implement passive SLAM, and the second priority is to implement active SLAM. Another second priority goal is to make the algorithm consider dynamic objects. In order to reach the first priority goals, the following needs to be investigated:

• A theoretical investigation comparing different SLAM-algorithms in order to make a valid decision on which one to use

• How to run a SLAM algorithm using limited resources (since it will be run on an embedded processor). This means first and foremost to pick a SLAM algorithm which uses few resources. However, if this is not sufficient, mod-ifications to the algorithm (or the implementation of one) may be needed. • Development of a model in Simulink and evaluating the model by

compar-ing the output of the model to a theoretical position/direction within a map In order to reach the second priority goals, the following needs to be investigated: • A theoretical investigation comparing different pathfinding algorithms for

active SLAM, as well as simulating different algorithms in Simulink. • Theoretical studies and simulations where dynamic objects are considered.

1.4 Limitations

The following limitations are set for this project:

• Ethical aspects of any kind will not be discussed in this thesis.

• There are no requirements of the environment of which the vehicle will operate in.

(15)

1.5 Risk analysis 3

1.5 Risk analysis

There are several risks that certain parts of this thesis may be delayed or fail. These are explained in the following sections.

1.5.1 General risks

A major part of this thesis will be conducted during the summer, when super-visors may be on vacation leave. This will limit the amount of support that is available.

1.5.2 Kinematic vehicle model

There are several risks regarding the kinematic vehicle model:

• The sensors in the kinematic vehicle model should be able to represent the actual sensors in the car as well as possible. The sensors being modelled should be validated by some measurements on the real sensors as well as theoretical values. However, that is not a guarantee that they actually rep-resent the actual sensors.

• The kinematic vehicle model should be validated using a measuring system. Currently, the measuring system is under development at Linköpings Uni-versitet. This basically implies that the testing phase of the model could be delayed. Alternative solutions for measuring could be by plain eye sight comparing the difference in position within the maps.

(16)

4 1 Introduction

1.6 Equipment

Fig. 1.1 displays an overview of the system architecture used in this thesis.

Figure 1.1:System architecture overview

The hardware equipment and software components are further explained be-low:

• ROS

– A framework and operating system developed for robots. This thesis will use the ROS Kinetic distribution

• Lubuntu 14.04

– Lightweight Ubuntu operating system, operating on the imx6 proces-sor on the Nitrogen6_MAX board.

(17)

1.7 Thesis methodology 5

• STM32 board

– A PCB containing a stm32f4 microcontroller, CAN transceivers, as well as being connected to a RPLIDAR, magnet encoders and an IMU. • Nitrogen6_MAX

– A motherboard containing CAN transceivers, Ethernet, Wi-Fi, and an imx6 quad-core processor.

• Magnet encoders

– Hall effect sensors attached to the wheel axis of the physical vehicle, measuring each time a magnet mounted inside the wheel passes by. • IMU 9250

– Inertial measurement unit, connected to a magnetometer, a gyro and an accelerometer .

• RPLIDAR

– A 2D laser scanner capable of producing scans in 360 different angles.

1.7 Thesis methodology

Fig. 1.2 illustrates the method workflow of this thesis. The first phase is a pre-study, where the basic theory of SLAM algorithms will be studied and ROS/Simulink tutorials will be conducted. This is just to get a good initial understanding of the subject. The next phase of the thesis is to concurrently develop a model of the physical vehicle in Simulink and doing a theoretical investigation on the SLAM algorithms chosen for investigation in this thesis. Once the model is done, some ROS implementation is needed to be able to perform simulations with the vehi-cle model. When the simulations are done, the results will be used to choose an algorithm to be implemented on the physical vehicle.

(18)

6 1 Introduction

1.8 Related work

This thesis, as stated above in Section 1.3, includes conducting a theoretical in-vestigation about three SLAM algorithms. Given that the SLAM problem has been known for a long time, there exists papers related to each of the SLAM al-gorithms examined in this thesis. [4, p.6]. From the book Probabilistic robotics [28] by Sebastian Thrun, Wolfram Burgard and Dieter Fox, each SLAM algorithm have been described thoroughly and it is being widely cited in this thesis. How-ever, given the specified thesis scope described in Section 1.3 the theoretical in-vestigation is also to be backed up by simulation results. Various aspects of the GraphSLAM and the FastSLAM algorithms have been compared to each other in the paper "An evaluation of 2D SLAM Techniques available in Robot Operating System" [24, p.1-6] and the master thesis "SmokeNav - Simultaneous Localization and Mapping in Reduced Visibility Scenarios". [25, p.26-33]. Moreover, at pages 33-36 in the master thesis [25], the reader is provided with real world results from GraphSLAM and FastSLAM implementations. Furthermore, real world re-sults of a GraphSLAM and FastSLAM implementation has been provided in [24, p.5-6].

(19)

2

Recursive Bayesian Estimation

A key component of what enables a robot to successfully perform navigation is sensing. In order to navigate properly, the robot needs information about its en-vironment. This information is supplied by sensors. A major problem, however, is that the sensor data will always be corrupted by noise to some degree. That means that if the robot just uses the raw sensor data as it is, the navigation will produce erroneous results. To apprehend the fact that all measurements are in the presence of noise, filtering is needed. One approach of doing this is through Recursive Bayesian Estimation [28, p. 13], which uses probabilistic models in order to filter out the noise.

2.1 State-space representation

In order to define states of a system, state-space representation is used. In general, a continuous-time system is said to be in state-space form [20, p. 19] if it can be defined as

˙x(t) = f (x(t), u(t)) (2.1a)

y(t) = g(x(t), u(t)) (2.1b) where x(t) is the system’s state vector, and u(t) and y(t) is the input and the output of the system, respectively. The derivative of the state vector ˙x(t) is a function of the current state x(t) and the current input u(t). Similarly, the output of the system y(t) is a function of the current state and the current input. For discrete-time systems, state-space equations [20, p. 19] are defined as

xt+1= f (xt, ut) (2.2a)

yt= g(xt, ut) (2.2b)

(20)

8 2 Recursive Bayesian Estimation

For linear continuous-time systems, the state space model can be formulated as

˙x(t) = Ax(t) + Bu(t) (2.3a)

y(t) = Cx(t) + Du(t) (2.3b)

For linear discrete-time systems, the state space model can be formulated as

xt+1= Axt+ Bxt (2.4a)

yt= Cxt+ Bxt (2.4b)

2.2 Bayesian Filtering

The pose of a robot is due to its current state xt. When the system enters a new

state xt+1, which is due to some control command of the robot ut, the new state

will produce a measurement zt+1. The relationship between controls, states, and

measurements is shown in Fig. 2.1 below. A Bayes Filter is an algorithm which

(21)

2.3 Kalman filter 9

calculates theprobability density function (PDF) for the state vector xt. This is

done in two steps, the prediction step bel(xt) and the correction step bel(xt) as

bel(xt) =

Z

p(xt|ut, xt−1) bel(xt−1) dxt−1 (2.5a)

bel(xt) = η p(zt|xt) bel(xt) (2.5b)

where in the prediction step (given by Eq. 2.5a), a new state xtis predicted

us-ing only the control command ut and the previous state xt−1. This prediction

is altered in the correction step by using sensor data in order to get a better estimation[28, p.27]. A scaling factor η is introduced to scale the resulting PDF in order to make the integral sum 1 [28, p.17]. Pseudo-code for the algorithm is described in Algorithm 1.

Algorithm 1Recursive Bayes Filter

1: function bayes_filter(bel(xt−1), ut, zt) 2: forall xtdo 3: bel(xt) = R p(xt|u1, xt−1) bel(xt−1) dxt−1 4: bel(xt) = η p(zt|xt) bel(xt) 5: end for 6: returnbel(xt) 7: end function

2.3 Kalman filter

A technique which implements a Bayes filter is the Kalman filter, which is used in linear Gaussian systems. Basically, it computes a belief for a continuous state [28, p.40]. This state represents the information regarding the vehicle such as the odometry, and its surrounding environment for example landmarks[28, p.20]. The belief of the state at a specific time instance t bel(xt), can be expressed by its

mean and its covariance.[28, p.40]

The Kalman filter needs three specific properties as prerequisites [28, p. 41-42], and these are:

• The current state xt, needs to be represented by linear arguments, meaning

that

xt= At(xt−1) + Btut+ t (2.6)

where Atand Btare constants. Moreover, the added noise source should be

of Gaussian nature. The current state xtdepends on the previous state xt−1,

the control ut, and the noise t.

• The current measurement zt requires its arguments to be of linear nature

meaning that

(22)

where Ct is a constant and the noise source is Gaussian. The current

mea-surement ztdepends on the current state xtand the noise source δt.

• The last prerequisite is that the start belief bel(x0) has to be Gaussian

dis-tributed.

The Kalman filter algorithm itself, shown below in Algorithm 2 expresses the belief of the current state xt, at a specific time t. The bel(xt), is expressed by

the mean µtand the covariance Σt, which is also the output of the algorithm.[28,

p.42]

Algorithm 2Kalman filter algorithm

1: function Kalman_filter(µt−1, Σt−1, ut, zt) 2: µt = Atµt−1+ Btut 3: Σ_t= AtΣt−1ATt+Rt 4: Kt= ΣtCtT(CtΣtCtT+Qt) −₁ 5: µt = µt+ Kt(zt−Ctµt) 6: Σt= (I − KtCt)Σt 7: returnµt, Σt 8: end function

Basically, the algorithm calculates predictions of its mean µt and covariance

Σ_tat line 2 and 3. Whereas Atand Btare matrices which multiplied by the state

vector xt−1 and the control vector ut represents the state transition probability

with linear arguments, which is one of the requirements for the Kalman filter algorithm to be successful. These predictions are used in the correction step, lines 4-6. The first step of the correction is the calculation of the Kalman gain denoted

Kt. Moreover, the calculation of the mean µt is performed with regards to the

predicted mean µt the measurement zt - the predicted measurement (2.7) Ctµt

times the Kalman gain. At last, the new covariance Σtis calculated with regards

to the Kalman gain factor as well as the predicted covariance Σt[28, p.43].

2.4 Extended Kalman filter

The Extended Kalman Filter is an approach of dealing with nonlinear models, of which the traditional normal Kalman filter cannot handle. Basically, real world measurements and state transitions are often not linear, thus a normal Kalman filter’s assumption of linear data is not accurate. The approach of how EKF pro-cesses this is by describing the state transition probability and the measurement probability with nonlinear functions [28, p. 54]

x(t) = g(u(t), x(t − 1)) + (t) (2.8a)

z(t) = h(x(t)) + δ(t) (2.8b) As stated above the EKF uses nonlinear functions. The output of the algo-rithm is an approximation in the form of a Gaussian distribution, with an esti-mated mean and covariance. The approximation part of the algorithm is due to

(23)

2.4 Extended Kalman filter 11

Figure 2.2: Linearization of the nonlinear function g(x). Dashed line indi-cates the Taylor approximation of g(x)

the fact that it uses nonlinear functions. These functions need to go through a linearization process. This process linearizes the functions by for example using a first order Taylor expansion around a linearization point, this can be seen in Fig. 2.2. The resulting Gaussian distribution can then be calculated in closed form. This enables the extraction of interesting characteristics of the distribution such as the mean µt and the covariance Σt. Given the mean and the covariance,

the belief bel(xt) can be represented [28, p.56-58]. The EKF algorithm is shown

below in Algorithm 3 [28, p.59].

Algorithm 3Extended Kalman filter algorithm

1: function Extended_Kalman_filter(µt−1, Σt−1, ut, zt) 2: µt= g(ut, µt−1) 3: Σt = GtΣt−1GTt+Rt 4: Kt = ΣtHtT(HtΣtHtT+Qt)−1 5: µt= µt+ Kt(zt−h(µt)) 6: Σ_t = (I − KtHt)Σt 7: returnµt, Σt 8: end function

As can be seen from Algorithm 3 it is slightly different compared to the Kalman filter Algorithm 2. This is due to the fact that the EKF uses nonlinear functions to calculate the mean µtand the covariance Σt.

(24)

2.5 Particle filter

Another implementation of a Bayes filter is the particle filter. In contrast to the Extended Kalman filter, it does not use a parametric model for the probability distributions. The main idea of the particle filter is that it instead keeps a set of

M samples, where each sample xt[m](known as a "particle") is a set of potential

state variable values. The probability itself of these state variables is represented by how many particles contain similar state variable values, meaning that areas with high probability will have a higher density of particles than the areas where the probability is low [19, p. 27].

The particle filter is described in Algorithm 4[28, p. 98], where ˜Xt is the

predicted state vector, and Xtis the corrected state vector.

Algorithm 4Particle Filter

1: function particle_filter(Xt−1, ut, zt) 2: X˜t = Xt= ∅ 3: form = 1 to M do 4: sample x[m]t ∼p(xt|ut, x[m]t−1) 5: w[m]t = p(zt|x [m] t ) 6: X˜t= ˜Xt+ (x [m] t , w [m] t ) 7: end for 8: form = 1 to M do

9: draw i with probability ∝ w[i]_t

10: add x[i]_t to Xt

11: end for

12: returnXt

13: end function

2.5.1 Sampling

During sampling (lines 3-7 in Algorithm 4), new values are generated for each particle in the filter. These values are only temporary, and the particles them-selves will not be updated until the resampling stage. This, for each particle, new state is due to the control ut and the previous state of the particle x

[m]

t . After

this, theimportance factor w[m]_t is calculated. The importance factor is due to the measurement zt and is used as a likelihood measure of the particle used in the

forthcoming resampling of the particle filter. [28, p. 99]

2.5.2 Resampling

During resampling (lines 8-10 in Algorithm 4) the actual particles in the filter are updated. This is done by randomly selecting a particle generated in the sampling stage, where the probability of selection for each particle is due to its importance

(25)

2.5 Particle filter 13

factor. The resampling is done with replacement, so each particle can be drawn several times. [28, p.99]

2.5.3 Rao-Blackwellized Particle Filter

There’s also another version of the Particle Filter called theRao-Blackwellized par-ticle filter. This is an extension of the regular parpar-ticle filter where some state

(26)

(27)

3

Simultaneous localization and

mapping

This chapter describes the basic features of a generic SLAM algorithm, as well as introducing a few SLAM algorithms. The key features of these algorithms are then summarized in Section 3.5, where each algorithm is a also evaluated in terms of being appropriate for implementation in the physical vehicle of this thesis.

3.1 Introduction to SLAM

Simultaneous localization and mapping is a problem where a moving object needs to build a map of an unknown environment, while simultaneously calculating its position within this map. [12, p. 229]

There are several areas which could benefit from having autonomous vehicles with SLAM algorithms implemented. Examples would be the mining industry, underwater exploration, and planetary exploration.[12, p. 229] The SLAM prob-lem in general can be formulated using a probability density function denoted

p(xt, m|z1:t, u1:t), where xt is the position of the vehicle, m is the map, and z1:tis

a vector of all measurements. u1:t is a vector of the control signals of the vehicle,

which is either the control commands themselves or odometry, depending on the application.

3.1.1 Data association

Basically, the concept data association is to investigate the relationship between older data and new data gathered. In a SLAM context it is of necessity to relate older measurements to newer measurements. This enables the process of deter-mining the locations of landmarks in the environment, and thus this also gives information regarding the robots position within the map. [4, p. 26]

(28)

16 3 Simultaneous localization and mapping

3.1.2 Loop closure

The concept of loop closure in a SLAM context is the ability of a vehicle to rec-ognize that a location has already been visited. By applying a loop closure algo-rithm, the accuracy of both the map and the vehicles position within the map can be increased. [30, p.1] However, this is not an easy task to perform, due to the fact that the operating environment of the vehicle could contain similar structural circumstances as previously visited locations. In that case if the loop closure algorithm performs poorly, it could lead to a faulty loop closure, which could corrupt both the map as well as the pose of the vehicle within the map.[30, p.1]

3.1.3 Full SLAM and online SLAM

There are two different types of SLAM problems. The online SLAM problem of which only the current pose xt and the map m are expressed, given the control

input u1:t and measurements z1:t. As well as the full SLAM problem which

ex-presses the entire trajectory of the robot. The PDF of the full SLAM problem is denoted as p(x0:t, m|z1:t, u1:t), where all the poses of the robot are considered,

in-cluding its initial pose x0. [17, p. 21]

3.1.4 General models for SLAM problem

A motion model for the SLAM problem can be described by

xt = g(ut, xt−1) + δt (3.1)

where the current pose xtdepends on the possible nonlinear function g(ut, xt−1),

with ut being the control input, xt−1the previous pose and δt being a random

Gaussian variable with zero mean.[17, p. 19]

A measurement model for the SLAM problem can be described by:

zt = h(xt, m) + t (3.2)

where the current measurement zt, depends on the possible nonlinear

func-tion h(xt, m), where xtdenotes the current pose, m represents the map, ta

ran-dom Gaussian variable with zero mean.[17, p. 19]

3.2 EKF SLAM

The EKF SLAM (Extended Kalman Filter) approach of solving the SLAM prob-lem is by using sensor data gathered from the movement and rotation of the robot. This can be done by for example using wheel encoders and a gyroscope. Furthermore, it is also necessary to gather information of the environment by, for example, using a Laser Range Finder (LIDAR). With this data, the algorithm

(29)

3.2 EKF SLAM 17

keeps track of where the robot is likely positioned within a map, as well as keep-ing track of specific landmarks observed.[4]

3.2.1 Algorithm

When the robot is turned on, the sensors on the robot such as the wheel encoders and the gyroscope will gather information of the positioning of the robot. More-over, landmarks from the environment are also extracted based on new observa-tions from the LIDAR which is mounted on the robot. These new observaobserva-tions are associated with previous observations and updated in the EKF algorithm. However, if an observation of a landmark cannot be associated to a previous observation the observation itself is presented to the EKF algorithm as a new observation.[4, p. 29] This process is illustrated in Fig. 3.1.

(30)

Figure 3.1:EKF SLAM algorithm. [4, p. 10]

First and foremost, the robot localizes itself within the map being created using observed landmarks and information from the sensors. The landmarks should preferably be observable from multiple angles, as well as not being sepa-rated from other landmarks. These prerequisites gives the EKF algorithm a good possibility to distinguish between landmarks at a later time. Moreover, the land-marks being used should also be stationary. [4, p. 18]

To describe the EKF SLAM process in short terms, it consists of two stages. • Prediction step

– Predict the current state expressed by the predicted mean and the pre-dicted covariance. These are being calculated based on control input, matrices, the previous covariance and the previous mean. [27, p. 20-28]

(31)

3.2 EKF SLAM 19

– First and foremost, the idea behind the correction step is the data as-sociation. The main idea behind associating data in a SLAM context is to distinguish between an earlier observed landmark and a newly ob-served one. [27, p.15-16,30] When a new measurement has occurred the procedure is as following:

* Store the locations of newly observed landmarks. [27, p.15-16,30] * Associate previously observed landmarks with newly observed ones.

[27, p.15-16,30]

Furthermore, the correction gain, also known as the Kalman gain is computed. Basically, it is a correction gain factor necessary to up-date the current mean and covariance of the current state. The cur-rent mean and covariance is not only dependent on the Kalman gain though, but they are also taking the predicted mean and the predicted covariance and the measurement into account. [27, p.37-38]

3.2.2 Computational complexity

The computational cost of the EKF SLAM algorithm is quite costly in comparison to other SLAM algorithms. When new landmarks are detected, they are added to the state vector of the filter, and the map of with N identified landmarks, will increase linearily. [23, p.7765] In terms of numbers, the use of memory is O(N2). The computation cost of computing one step of the algorithm is approximately O_(N2_{) [27, p.57] [5, p. 792], the complete cost for computing the whole algorithm} is O(N3), heavily dependant on the size of the map[5, p.792].

3.2.3 Data association

A bottleneck when using the EKF algorithm is the computational cost which in-creases when new landmarks are observed. This leads to a prerequisite of that the environment should not contain too many landmarks. However, since the num-ber of landmarks in the environment are few, the localization within the map and the data association is getting increasingly more difficult.[28, p. 330-331] Moreover, the data association is made by computing the maximum likelihood and in case of a faulty association, the future results which are based on previous associations by the algorithm will also be faulty.[28, p.331-332]

(32)

3.3 FastSLAM

Another way of solving the SLAM problem is using the FastSLAM algorithm, which makes use of the Rao-Blackwellized particle filter. FastSLAM takes ad-vantage of something that most other SLAM algorithms do not, namely the fact that each observation only concerns a small amount of the state variables. The pose of the robot is probabilistically dependent on its previous pose, whereas the landmark locations are probabilistically dependent on the position of the robot. [19, p. 27] This is not taken into account by the EKF SLAM algorithm, for exam-ple, which with each update has to recalculate its covariance matrix, resulting in poor scaling in large maps. [19, p. 29] This is not a problem in FastSLAM, as it factors out the calculations to simpler subproblems, as shown by

p(st, θ | zt, ut, nt) = p(st|_zt_{, u}t_{, n}t₎

N

Y

n=1

p(θn|st, zt, ut, nt) (3.3)

which factors out the robot path stand each of the N landmarks θn. One major

difference here between FastSLAM and, for example EKF SLAM, is that the entire path st _{is calculated, as opposed to just the current pose x}

t. This means that

FastSLAM is a full SLAM algorithm and EKF SLAM is an online SLAM algorithm, as described in Section 3.1.3.

3.3.1 Particle structure

The particle filter used in FastSLAM is comprised of M particles, where each particle is defined by

S_t[m]= (st[m], µ1,t, Σ1,t, ..., µN ,t, ΣN ,t) (3.4)

Each particle contains N + 1 variables, one robot path st[m], and N landmark posteriors, where each landmark is represented by an EKF with a mean µn,t and

a variance Σn,t. During one iteration of the algorithm, the robot path for every

particle st[m]_{is updated once, as well as each landmark filter µ}

n,t, Σn,t.

3.3.2 Algorithm

Each iteration of the algorithm can be summarized in four steps. These are pre-sented below.

1. For each of the M particles, sample a new robot path stfrom p(st|s[m]t−1, ut)

2. With the new measurement, update the landmark filters 3. Give each particle an importance factor

4. Resample the particles, with particles with a higher importance factor hav-ing a higher draw probability.

(33)

3.3 FastSLAM 21

In step 1, a new robot path is sampled for each particle. This path is drawn from the motion model, with the robot control as input. After this, the posteriors of the landmarks are updated due to the new measurement. After this, each particle is given an importance factor, which is a measure on how realistic the variables in the particle are. Later, the particles are resampled in order to get rid of the ones which do not have high probability.

[19, p. 10]

3.3.3 FastSLAM 2.0

The FastSLAM algorithm (and particle filters in general) performs more accu-rately when there is more diversity among the particles. Particles will not be diverse if, for example, the odometry sensors are noisy and the laser range finder is accurate, as this will render many of the motion hypothesises unlikely and many particles will have a low importance factor. Because of this drawback of the FastSLAM algorithm, an updated one has been developed, named FastSLAM 2.0. FastSLAM 2.0 is quite similar to its predecessor, with the exception that when estimating the motion, it also considers the range sensors. This creates bet-ter guesses in general, and will result in more particles having higher importance factors. [19, p. 63]

3.3.4 Computational complexity

If the FastSLAM is directly implemented as described above, it has O(M ∗ N ) computational complexity, where M is the number of particles and N is the num-ber of landmarks. [19, p. 48] To implement an algorithm like this would not be a good idea, since it would scale linearly with the number of landmarks. This would render the algorithm unsuitable for large maps.

To apprehend this issue, FastSLAM is implemented using binary trees rather than the array structure shown in 3.4. The algorithm will then have O(MLog(N )) complexity [28, p. 465], which is illustrated in Fig. 3.2 below, which illustrates such a tree in a single particle with N = 8.

Using this binary tree approach also signifcantly reduces the amount of mem-ory used by the algorithm. [19, p. 58]

(34)

Figure 3.2:Binary tree data structure of FastSLAM particle

3.3.5 Data Association

Another advantage of the FastSLAM algorithm is the fact that its data associa-tion is on a per-particle basis. This means that each particle will have its own hypothesis on which landmark the measurement is associated with. If a measure-ment associates incorrectly with a landmark in a particle, this particle will have a low importance factor, thus making the impact of this wrong association very small. In other algorithms, such as EKF-based SLAM, once an incorrect data as-sociation has been made, it will cause the algorithm to produce incorrect results from that point. This is one of the main advantages of the FastSLAM algorithm in comparison to other algorithms, and contributes to the algorithm having a high robustness. [19, p. 41]

(35)

3.4 GraphSLAM 23

3.4 GraphSLAM

GraphSLAM is another approach of solving the SLAM problem. The idea be-hind GraphSLAM is to address the SLAM problem by using a graph. The graph contains nodes which represent the poses of the robot x0, ..., xt as well as

land-marks in the map which are denoted as m0, ..., mt. However, this is not enough to

address the SLAM problem, as there are also constraints between the positions

xt, xt−1, xt−2, ..., xt−nof the robot and the landmarks m0, ..., mt. [26, p.361] These

constraints represents the distance between adjacent locations such as xt−1, xt as

well as the distance between the locations to the landmarks m0, ...mt. These

con-straints can be constructed due to information from the odometry source ut.[26,

p. 361-362] The graph mentioned earlier can be converted to a sparse matrix. Which in contrast to dense matrices can be more efficiently used. [17, p. 1] Fig. 3.3 illustrates how the landmarks m0, ..., mt, the locations x0, ..., xt and the

con-straints are linked together.

Figure 3.3: Robot poses seen as circles, landmarks of the map seen as squares, dashed lines representing the motion of the robot based on control commands given to the robot as well as solid lines representing the distance to the landmarks given by measurements.

3.4.1 Algorithm

The PDF for the full SLAM problem is denoted p(x0:t, m|z1:t, u1:t). For simplicity,

(36)

in Fig. 3.4.

Figure 3.4:Statespace vector containing all poses and landmarks

Then in order to achieve the most likely pose of the robot within the map and the positions of the landmarks in the map, an optimization algorithm has to be computed. [17, p. 11]

Moreover, the PDF p(y|z1:t, u1:t) contains the possibly nonlinear functions

g(ut, xt−1) and h(xt, mi), whereas mi represents the i-th landmark observed at

time t. [17, p.4,11] The optimization algorithm expects linear functions. Thus, a necessary step is to linearize these. This could be done by for example executing a Taylor linearization algorithm. [17, p.11]

As specified earlier, the GraphSLAM approach of solving the SLAM problem is by introducing a graph. This graph can be converted to a sparse matrix. Basi-cally, this sparse matrix contains all the information acquired by the robot while moving in an environment. For example, the robots movement through the map and landmarks observed at different locations. Thus, as the robot moves through the environment the sparse matrix will increase in size and contain even more information.[17, p. 14]

In the following Figs. 3.5,3.6,3.7 below, it is illustrated of how the robot poses illustrated as blue squares and landmarks illustrated as red squares observed are added to the sparse matrix. Moreover, the blue squares are also adjacent to each other due to their relationship as the new pose is dependant on the old pose. The diagonal grey squares shows that the landmarks of each row m1, m2, m3, m4 and its corresponding column m1, m2, m3, m4 is the one and same landmark.

(37)

3.4 GraphSLAM 25

Figure 3.6:Robot poses and landmarks observed

Figure 3.7:Robot poses and landmarks observed

3.4.2 Computational complexity

Some interesting statistics when discussing the GraphSLAM algorithm is the com-putational load and the use of memory. In comparison with the EKF SLAM algo-rithm of which the use of memory grows by O(N2) when new landmarks N are in-troduced and the computational cost by O(N3), the GraphSLAMs use of memory grows linearly when new landmarks N are introduced. As for the computational cost of the GraphSLAM algorithm it is dependent on time, thus, if the path of the vehicle is very long time-wise the computational cost can be costly. However, in general the attributes of the GraphSLAM algorithm is very desirable when the environment contains a lot of landmarks, of which the EKF algorithm tends to fail.[26, p. 362]. [28, p. 330]

3.4.3 Data association

The data association part of the GraphSLAM algorithm is done using a vector containing correspondences between landmarks observed in the map. [28, p.362] The algorithm performs a correspondence test to check the probability that dif-ferent landmarks observed within the map actually is the same landmark. If the result of this correspondence test exceeds a certain value, the algorithm will de-termine the landmarks as the same landmark, otherwise it will dede-termine these

(38)

as two separate landmarks. [28, p. 363] In comparison with the EKF SLAM algorithm where the future data associations can be faulty due to earlier data associations, the GraphSLAM algorithm can reexamine older data associations, thus reducing the risk of introducing errors for future data associations. [28, p. 339,363] The GraphSLAM algorithm solves the full SLAM problem [17, p. 15], in comparison to the EKF slam algorithm which only solves the online SLAM problem.

3.5 Theoretical evaluation of SLAM

Given the specified thesis scope in Section 1.3, a theoretical investigation regard-ing three different SLAM algorithms, namely EKF, GraphSLAM and FastSLAM has been conducted.

There are several parameters of interest when discussing SLAM algorithms, some of which have been investigated are the following: robustness, efficiency as well as ROS compatibility. These parameters are evaluated and compared to each other below.

• EKF

– Robustness

* Given what has been stated in 3.2.3, a faulty data association will result in erroneous future results, thus the robustness of the EKF algorithm is low. [28, p.331-332]

– Efficiency

* In Section 3.2.2 the total computational cost of executing the al-gorithm is O(N3), to a great degree dependant on the size of the map[5, p.792]. In conclusion this makes the EKF SLAM algorithm more suitable for smaller maps containing fewer landmarks due to the heavy computational load.[27, p. 57] However, as stated in 3.2.3 the localization and data association part can be difficult when the landmarks in the environment are few. [28, p. 330-331] – ROS compatibility

* Implemented in the mrpt_ekf_slam_2d package. • FastSLAM

– Robustness

* Given what has been stated in 3.3.5, the FastSLAM algorithm is sig-nificantly more robust than the EKF SLAM algorithm due to the fact that the data association from measurements to landmarks is particle-based which reduces the impact of a wrong data associa-tion.

(39)

3.5 Theoretical evaluation of SLAM 27 * In Section 3.3.4 the total computational cost is stated as O(MLog(N)),

meaning that the complexity is due to the number of particles M and the number of landmarks N .

– ROS compatibility

* Implemented in the gMapping package. • GraphSLAM

– Robustness

* Given what has been stated in 3.4.3, the robustness of the Graph-SLAM algorithm is higher than the EKF algorithm, partly due to the fact that older data associations can be reexamined if a wrong data association has been performed. Basically, this reduces the risk of producing erroneous future data associations thus increas-ing the robustness.

– Efficiency

* In Section 3.4.2 it is stated that the use of memory is linearly de-pendent in comparison to the EKF algorithm which uses O(N2) based on number of landmarks N Moreover, the computational load is time dependent, thus if the path is long the algorithm can be quite costly as stated in 3.4.2.

– ROS compatibility

* Implemented in the slam_karto package.

From what can be read above, the EKF SLAM algorithm is not too robust, while both the GraphSLAM algorithm and the FastSLAM algorithm have a higher robustness. Moreover, in terms of efficiency the EKF SLAM algorithm is very costly. Given that all algorithms have been implemented in ROS packages, each of these are viable choices. However, as the EKF SLAM algorithm lacks in both robustness and effeciency, the conclusion of using other more effective algorithms such as the FastSLAM or GraphSLAM instead of the EKF algorithm can be reached. These algorithms are able to efficiently deal with maps containing more land-marks, as well as larger maps in terms of sheer size. With maps containing a cou-ple of hundred landmarks, for examcou-ple when trying to map the outside world, the computational complexity of the EKF algorithm could prove to be a huge problem to deal with. [18, p. 1], [28, p. 330]

(40)

(41)

4

Vehicle Dynamics

This chapter describes some of the vehicle dynamics theory which was used to develop the dynamic model of the small-scale vehicle. Some of the theory pre-sented in this chapter was ultimately not used in the final model, but is still included here since it describes the initial modeling approach. This chapter only concerns the movement of the vehicle, whereas the theory concerning the sensors is presented in Chapter 5.

4.1 Longitudinal dynamics

Longitudinal dynamics describe the movement of a vehicle along the longitudi-nal axis. Thus, it can be said that longitudilongitudi-nal dynamics describe the speed of the vehicle in the forward and backward direction, due to how different forces act upon the vehicle.

4.1.1 Gear ratio

In a vehicle the motor and the wheels rotate at different speeds. This is due to the fact that they are connected by different sized gears. The relationship between the angular velocity ω of the motor and gear is known as the gear ratio [15, p. 43] and is defined as

ngear =

ωmotor

ωwheels (4.1)

4.1.2 Wheel torque

The torque τ at the wheel of the vehicle can be computed with

(42)

30 4 Vehicle Dynamics

P = τ ∗ ω (4.2) where P is the power of torque and ω is the angular velocity.

If it is assumed that there are no losses, the power at the motor and the wheels will be equal, meaning that

Pmotor = Pwheels (4.3)

and together with with (4.2) this implies

τmotor∗ωmotor = τwheels∗ωwheels (4.4)

If (4.1) is substituted into (4.4) we obtain the torque of the wheels as a function of the gear ratio and torque of the motor

τwheels= ngear∗τmotor (4.5)

4.1.3 Forward velocity

To obtain the forward velocity of the vehicle, the longitudinal force of the driving wheels needs to be calculated. This is obtained by

F = τ

r (4.6)

and in the terms of the wheels it is denoted by

F = τwheels

rwheels (4.7)

Newton’s second law of motion can then be used to determine the acceleration of the vehicle as

a = Fwheels mvehicle

(4.8) where mvehicle denotes the mass of the vehicle. The rotational inertia of the

wheels and drive shaft can be neglected or included as mass equivalent. Finally, the velocity is obtained through integration as

v = T Z t adt (4.9)

4.2 Lateral dynamics

Lateral dynamics describe the movement of the vehicle along the lateral axis. Lateral movement is mainly determined by the heading angle, which needs to be calculated in order to determine the lateral velocity of the vehicle.

(43)

4.2 Lateral dynamics 31

4.2.1 Ackermann angle

The Ackermann steering angle is defined as the wheelbase angle

δ = l/R (4.10) which causes the vehicle to turn in a specific heading angle, or yaw. l is the length of the wheelbase and R is the radius of the heading angle, i.e. the distance between the center of gravity and the turn center, as Fig 4.1 shows. [16, p .129]

Figure 4.1:The Ackermann angle

4.2.2 Heading angle

If the Ackermann angle, the length of the wheelbase, and the forward velocity is known, the heading angle of the vehicle can be calculated. The forward velocity is defined as

v = R ∗ ˙ψ (4.11) Since the radius of the heading angle is related to the Ackermann angle, (4.10) can be substituted into (4.11) in order to get the yaw rate

˙

ψ = v

l ∗δ (4.12)

Finally, the heading angle can be obtained through integration as

ψ = T Z t ˙ ψdt (4.13)

(44)

32 4 Vehicle Dynamics

4.3 Coordinates

Coordinates of the vehicle can then be calculated by integrating the velocities in the x and y direction over time as

x = Z vxdt (4.14a) y = Z vydt (4.14b)

If the forward velocity v, vehicle length l, yaw rate ω = ˙ψ, and yaw ψ of the

ve-hicle are known, the velocities vxand vycan be calculated using a transformation

matrix [10] as "vx vy # = " v l 2 ∗ω # "cos(ψ) −sin(ψ) sin(ψ) cos(ψ) # (4.15)

(45)

5

Sensor theory

The sensors used in this thesis are a gyroscope, wheel encoders, and a LIDAR. The measurements from these sensors provide the necessary input to the SLAM algorithms. The SLAM algorithms will process this data in order to present a map of the surrounding environment, including the pose of the vehicle positioned within the map. The following sections will provide a description of each sensor mentioned.

5.1 Gyroscope

Gyroscopes have been widely used for military and civilian purposes the last cen-tury. Not only for aeronautical purposes but also by the marine. The traditional gyroscopes used to be mechanical. However, what is being used in many ap-plications nowadays are MEMS gyroscopes (micro-electro-mechanical systems), which are being used in cell phones, robotic projects, and for military purposes. [14, p. 171] Gyroscopes in general can be described by the the formula of Coriolis force

Fc= −2 ∗ m ∗ (ω ∗ v) (5.1)

As can be seen from (5.1) the Coriolis force is depending on the mass m of an object, the angular velocity ω as well as the velocity v of the object. This object is a part of the gyroscope and the mass of the object as well as the velocity of the object are known. Given that the object rotates, and by having information about the mass and velocity of the object the angular velocity can be detected.[14, p. 173] Given the angular velocity, the total angle that the vehicle has rotated can be calculated as

(46)

34 5 Sensor theory a = t Z 0 ψdt (5.2)

5.2 Wheel encoders

Wheel encoders are being used to provide the microcontroller with information about the velocity of the vehicle. There are several different types of wheel en-coders based on different technologies, such as, optical wheel enen-coders, mechan-ical as well as magnetic wheel encoders just to name a few. In this thesis a mag-netic wheel encoder is being used which consists of a Hall effect sensor and a number of magnets. The Hall effect sensor outputs a high voltage level when-ever the magnetic field surrounding it is sufficiently strong, (a magnet passing by)[22, p.3-4]. The pulse outputted by the Hall effect sensor is being registered by the microcontroller. By counting the number of pulses generated since the previous sampling instance, the microcontroller is able to estimate the speed of the vehicle.

Below is some pseudo code for calculating the vehicle velocity in Algorithm 5.

Algorithm 5Vehicle car speed

1: function car_speed(()P ulsesavg,T)

2: Tnew= T − Told

3: vcar = (P ulsesavg∗(wheelCircum/10))/Tnew

4: Tnew= Toldreturnvcar

5: end function

The algorithm calculates the Vcarin cm/s. The average number of high pulses

registered by the two Hall effect sensors is denoted P ulsesavg. Which is

multi-plied by (1/10) of the circumference of the wheels, thus giving a distance trav-eled. By dividing with the time period Tnew the vcar resembles the well known

formula for velocity v = s/t. The wheel circumference is divided by ten due to the fact that there are ten magnets attached on each wheel.

5.3 Laser range finder

Laser Range Finders (LIDAR) can be used to measure the distance to objects in the vicinity of the vehicle. A laser range finder consists of a laser emitter and receiver, and is sometimes mounted on a rotating motor in order to measure distances in every direction of the vehicle. The basic operation principle is that a short pulse of light is emitted, reflected on an object, and then received. The time twait

between the emitting and receiving can then be used to calculate the distance d to the object as

(47)

5.3 Laser range finder 35

d = 1

2ctwait (5.3)

(48)

(49)

6

Simulink Model Implementation

The vehicle, sensors, and the environment of the vehicle were modelled in Simulink. This chapter describes the implementation of all the subsystems of the model, which can be seen in Fig 6.1 below. The model is not able to be simulated in real-time, due to the computation time being too high.

Figure 6.1:Block Diagram of Simulink Model

(50)

38 6 Simulink Model Implementation

6.1 Input

The input subsystem provides the input stimuli to the system. This is done in two different modes, keyboard, and file. In keyboard mode, the steering angle and the velocity of the vehicle is controlled using the arrow keys on the keyboard. In file mode, the velocity and steering angled is defined by a .mat file. These .mat files are recorded when running the model in keyboard mode. A typical workflow while using this model would be to first record the instructions for the vehicle to a .mat file, and then use the same .mat file for repeated experiments.

6.2 Kinematic Vehicle

The kinematic vehicle, shown in Fig. 6.2 is comprised of five subsystems, each of which is described in the sections below.

Figure 6.2:Model of Kinematic Vehicle

6.2.1 Velocity

6.2.1.1 Initial model

The initial velocity model was modelled after how velocity is created in the ve-hicle itself, with the motor being a torque source which is converted to force, and ultimately velocity. This is described in Section 4.1. However this model was never completed, and scrapped. This is because conception of such a model proved to be too time-consuming, and might have required a master thesis on its own to do.

6.2.1.2 Final model

The final model is considerably simpler than the initial model. In this model, the input signal is the desired velocity of the vehicle. This is passed through a

(51)

first-6.3 Map 39

order Filter in order to model the acceleration and deceleration of the vehicle. The time it takes for the vehicle to go from a stationary state to be moving at full speed was measured in order to calculate the time constant τ of the first order filter.

6.2.2 Servo

A simple model was also used for the servo. The servo control signal is a value from -100 to 100, which is then passed through a look-up table which ranges from -45°to 45°, which is the maximum steering angle of the vehicle. A first-order filter is used to model the delay of the steering of the vehicle, and the time constant τ was calculated by measuring the time it takes for the wheel of the vehicle to turn from 0°to 45°.

6.2.3 Angle & Angular Velocity

This subsystem calculates the yaw rate and the yaw angle for the vehicle, which is due to the steering angle and the speed. This is calculated by using the Acker-mann angle, which is explained in Eq. 4.10 in Section 4.2

6.2.4 Position

The X-position and Y-position subsystems realize the transformation matrix de-scribed in Section 4.3 in order to determine the current position of the vehicle.

6.3 Map

6.3.1 Parameters

In order to emulate the environment of the vehicle, a map definition needed to be created. These maps are defined by two parameters;dimensions and walls. The

dimensions array is a 1-by-2 matrix where the first index is the width of the map and the second is the height of the map. The walls matrix is an n-by-4 matrix which describes the walls in the map, where each row contains the coordinates of a wall. These can be either diagonal, vertical, or horizontal. The map parameters are stored in a .mat file which can then be read by the Simulink model file.

6.3.2 Implementation

The map is implemented in two S-functions calledvehicleMap and drawMap. The

former calculates all necessary values to display the current position and yaw of the vehicle within this map, and the latter draws this is in a MATLAB plot. In this plot, the position and yaw of the vehicle is represented by a triangle. This is shown in Fig. 6.3 below. The inputs ofvehicleMap is shown in Table 6.1 below,

(52)

Figure 6.3:Plot of map generated by vehicleMap S-function

Table 6.1:Inputs of vehicleMap

Inputs Direction Size Definition

Coords In 2 Current coordinates of the vehicle. Yaw In 1 Current yaw of the vehicle.

Triangle Coords Out 6 Coordinates of where to draw the triangle Collision Out 1 Boolean indicating if the vehicle has hit a wall Init Out 1 Boolean indicating if map has been initialized

(53)

6.4 Odometry Sensors 41

Figure 6.4:Map subsystem

6.4 Odometry Sensors

6.4.1 Gyroscope

6.4.1.1 Description

The model receives the actual angular velocity produced from the car model, and outputs an estimation of what the sensor would have measured. These measuments are corrupted by noise and bias. The model has been designed with re-gards to actual measurements on the values from the gyroscope. By measuring the output when the physical vehicle is standing still, an average error can be cal-culated and modelled. The pseudo code for deriving the bias is presented below in algorithm 6

Algorithm 6Gyroscope bias

1: forGz in array do

2: count = count + int(Gz)

3: number = number + 1

4: end for

5: ψ = (count ∗ 250 ∗ 3.14/(number ∗ 32768))

First and foremost, what is being derived above in algorithm 6 is the angular velocity in rad/s. The bias output from the gyroscope’s z-axis has been measured

(54)

and an average bias output has been calculated. To transform this output to an angular velocity in rad/s, the average bias (count/number) is multiplied by DPS/ADC * pi/180. Where DPS = degrees per second of range specified, there are three possible alternatives: 250 °/s, 500 °/s and 2000 °/s. ADC = Analog to digital converter, resolution of 16 bits. This division gives the angular rate in °/s, by multiplying with pi/180 the angular rate can be expressed in radians/s.

Moreover, the noise from the gyroscope has also been modelled using Simulink. The variance and the standard deviation has been derived from 21000 samples of the gyroscope left stationary.

6.4.1.2 Implementation

Given the precondition that the vehicle is standing still, samples were obtained from the physical sensor. These samples were converted into angular velocities in rad/s by algorithm 6. Given these converted samples, figures were created in Matlab which display the Gaussian nature of the values from the gyroscope. Fig. 6.5 shows the angular velocities from the gyroscope, and how the curve resembles a Gaussian distribution. This hypothesis is also in line with conclusions of [13, p.7-8,22] when measuring sensor errors.

Figure 6.5:Bar chart displaying the noise of the gyroscope, curve showing a Gaussian distribution

The Gaussian noise as well as the bias offset was implemented into the Simulink model. This can be seen in Fig. 6.6. This model outputs an angular rate expressed in rad/s corrupted by noise and bias with the aim of resembling the actual gyro-scope implemented in the physical car.

(55)

Figure 6.6:Gyroscope implementation in Simulink

6.4.1.3 Test phase

Evaluation of the model has been done by implementing a test environment in Simulink. Basically, a ROS subscriber block has been implemented in Simulink which receives the actual values from the physical car. The values of the z-axis of the gyroscope are being measured and computed as an angular rate in rad/s as well as an angle in rad. The comparison between the Simulink model of the sen-sor and the physical sensen-sor values can then be done in the Simulink environment. Fig. 6.7 below shows the test environment.

Figure 6.7:Gyroscope test environment in Simulink

Moreover, Fig. 6.8 shows the angular difference between the gyroscope model and real time measurements on the physical gyroscope.

(56)

Figure 6.8: Comparison between gyroscope model and real time measure-ments on the physical gyroscope. Top display shows the model output after 129.2 seconds, and bottom display shows the total angle measured from the physical gyroscope. (Notice, both are initialized to 1.507 rad, 90 degrees)

6.4.2 Wheel encoders

6.4.2.1 Description

A sensor model of the wheel encoders has been implemented into the model of the car in Simulink. They have been modelled with regards to measurements of the output from the wheel encoders attached on the physical car. However, the data being received is actually an estimate of the speed of the physical car and not a number of magnets detected since the last timestamp. With that being said, the sensor model implemented is using these measurements to estimate and mimic the transmitted car speed from the physical wheel encoders attached on the car. These car speed measurements are not that precise though, which could be explained by a bias offset and noise. The bias consists of truncation errors and the algorithm which converts magnet pulses to car speed in cm/s. The truncation error occurs when the algorithm truncates the floating number to an u_int8 which is to be transmitted on the CAN bus. The noise can be expressed in terms of the variance around the mean velocity.

(57)

6.4.2.2 Test phase

The test phase of the wheel encoders was executed at our office on the floor. More-over, we used equipment such as a yardstick, rulers, as well as a mobile phone for measuring time of which the vehicle traveled a certain distance. Each mea-surement conducted consisted of about 40 measured samples. Table 6.2 below shows the measurements done from the test environment. The results on each row differs quite a bit, this is mostly due to the fact that the engine on our car restarted randomly during runtime.

Table 6.2:Results of measurements on the wheel encoders MI µ σ σ2 [s] [cm] [tq] [vq] 5 4.1 0.98 0.96 6.59 58 0.83 0.96 5 4.1 1.1 1.2 4.3 33 0.83 0.95 5 4.4 0.79 0.62 4.6 40 0.83 0.94 5 4.1 0.31 0.1 4.15 34 0.44 0.73 10 10.9 0.34 0.11 5.13 94 0.73 0.89 10 10.7 0.94 0.89 6.4 122 0.76 0.87 10 11 0.91 0.81 5.3 103 0.83 0.87 10 10.7 0.47 0.22 3.55 50 0.38 0.65 15 15.9 0.94 0.89 6.1 177 0.8 0.93 15 15.7 1.24 1.56 3.6 103 0.74 0.85 15 15.9 1.51 2.26 6.35 196 0.75 0.88 15 15.9 1.1 1.21 5.2 144 0.72 0.88 20 22.1 1.32 1.75 4.8 190 0.72 0.84 20 21.5 1.02 1.05 4 142 0.56 0.73 20 20.2 1.99 3.99 3.4 122 0.58 0.80 20 19.8 0.95 0.9 4.65 167 0.61 0.79

Basically, what can be seen from Table 6.2 is the sent instruction MI to the motor, the average measured velocity µ, the standard deviation σ , the variance

σ2, the physical time [s] for the car to travel a distance expressed in seconds, the physical distance [cm] the car travelled during the measurement. Moreover, the factor [tq] denotes the portion of time of which the speed of the vehicle is above a certain threshold, thus disregarding the portion of time of which the vehicle is accelerating. Furthermore, the factor [vq] corrects the actual distance the car travelled during the performed test, by disregarding the distance travelled during acceleration to a more steady velocity.

The Fig. 6.9 shows the speed received from the physical wheel encoders.

6.4.2.3 Implementation

The model was implemented based on the values in Table 6.2. The bias has been modelled through numerous measurements on the wheel encoder values and

(58)

Figure 6.9:Plot of the measured speed from the wheel encoders

physical measurements on the actual car driving a predefined distance of 2 m. From Table 6.3 below, the measured velocity from physical tests are significantly higher than the car speed which the sensors believe the car is driving in. The way this is implemented in Simulink is by introducing a look up table, with an actual velocity input scaled to the sensed value.

Table 6.3:Measured velocity compared to the sensed velocity from the phys-ical wheel encoders

Motor instruction 0 5 10 15 20 Sensed velocity 0 3.9 10 15.7 21.3 Measured velocity 0 9.82 23.3 36 49

In Fig. 6.10 below, the car speed from the sensor implemented in Simulink is plotted. The plot is showing the car speed in cm/s plotted over time. Moreover, the car speed is depending on user input in the model which can be seen as the steps where a user has increased the car speed. In addition, the sensor model implemented includes a noise source which depends on the current speed of the car model, this noise can be seen in the plot below.

Fig. 6.11 below shows the model implementation in Simulink. Basically, what can be seen is the conversion from m/s to cm/s, a LUT converting the velocity input to what the sensor detects, as well as a Matlab function which adds noise to the velocity depending on the current velocity.