• No results found

A Localisation and Navigation System for an Autonomous Wheel Loader

N/A
N/A
Protected

Academic year: 2021

Share "A Localisation and Navigation System for an Autonomous Wheel Loader"

Copied!
92
0
0

Loading.... (view fulltext now)

Full text

(1)

A Localisation and Navigation System for an

Autonomous Wheel Loader

Robin Lilja

Master’s Thesis, January 2011

Mälardalen University

(2)

A Localisation and Navigation System for an Autonomous Wheel Loader

Written by Robin Lilja

Supervisors

Torbjörn Martinsson (Volvo Construction Equipment) Doctor (Ph.D) Giacomo Spampinato (Mälardalen University)

Examiner

Professor Lars Asplund

Mälardalen University

School of Innovation, Design and Engineering Box 883, 721 23 Västerås

SWEDEN

http://www.mdh.se/idt

Release Date: 2011-01-17 Edition: First

Comments: This Master’s Thesis report is submitted as partial fulfilment of the requirements for the degree of Master of Science in Robotic Engineering. The report represents 30 ECTS points.

Images: Front logotype is a property of Mälardalen University, all others are produced by the author or obtained from Volvo CE.

(3)
(4)

Abstract

Autonomous vehicles are an emerging trend in robotics, seen in a vast range of applications and environments. Consequently, Volvo Construction Equipment endeavour to apply the concept of autonomous vehicles onto one of their main products. In the company’s Autonomous Machine project an autonomous wheel loader is being developed. As an objective given by the company; a demonstration proving the possibility of conducting a fully autonomous load and haul cycle should be performed.

Conducting such cycle requires the vehicle to be able to localise itself in its task space and navigate accordingly. In this Master’s Thesis, methods of solving those requirements are proposed and eval-uated on a real wheel loader. The approach taken regarding localisation, is to apply sensor fusion, by extended Kalman filtering, to the available sensors mounted on the vehicle, including; odometric sensors, a Global Positioning System receiver and an Inertial Measurement Unit.

Navigational control is provided through an interface developed, allowing high level software to command the vehicle by specifying drive paths. A path following controller is implemented and evaluated.

The main objective was successfully accomplished by integrating the developed localisation and navigational system with the existing system prior this thesis. A discussion of how to continue the development concludes the report; the addition of a continuous vision feedback is proposed as the next logical advancement.

(5)

Acknowledgements

First of all I would like to thank my nearest colleagues Niclas Evestedt and Jonathan Blom for all the hilarious discussions and moments we had together for the past months, making the long hours spent in the wheel loader endurable. Their technical and analytical feedback is appreciated as well. A special gratitude is directed to my friend and colleague Magnus Saaw for proof-reading this thesis. Special thanks to my supervisor at Mälardalen University, Dr. Giacomo Spampinato, for his competent and insightful discussion on Kalman filters. Dr. Martin Magnusson at Örebro University deserves a special recognition for his work and assistance on the vision system. A thank to Staffan Backen and Ulf Andersson at Luleå University of Technology for lending the DGPS equipment. Finally, a thank to my supervisor at Volvo CE, Torbjörn Martinsson, for his visionary inspiration and enthusiasm shown for the Autonomous Machine project.

(6)

Contents

List of Figures vii

List of Tables x

1 Introduction 1

1.1 Background . . . 1

1.1.1 Autonomous Machine Project . . . 1

1.1.2 Volvo Construction Equipment . . . 1

1.1.3 Wheel Loaders . . . 1 1.2 Problem Specification . . . 2 1.3 Objectives . . . 3 1.4 Safety . . . 3 1.5 Delimitations . . . 3 1.6 Thesis Outline . . . 4 1.7 Related Work . . . 4

1.7.1 Autonomous Ground Vehicles . . . 5

1.7.2 Autonomous Mining Equipment . . . 6

1.8 Platform . . . 6

2 Coordinate Systems 8 2.1 Local Planar Frame . . . 8

2.2 Vehicle Body Fixed Frame . . . 9

3 Sensors 10 3.1 Sensor Measurement Model . . . 10

(7)

Contents

3.2 Odometry Sensors . . . 10

3.2.1 Articulation Angle Sensor . . . 11

3.2.2 Rotary Encoder . . . 11

3.3 Inertial Measurement Unit . . . 12

3.4 Global Positioning System . . . 13

3.4.1 Dilution of Precision . . . 14 3.4.2 Differential GPS . . . 15 4 Vehicle Modelling 16 4.1 Related Work . . . 16 4.2 Wheel Slip . . . 18 4.2.1 Longitudinal Slip . . . 18 4.2.2 Lateral Slip . . . 19 4.3 Kinematic Model . . . 20 4.3.1 Model Verification . . . 22 5 Sensor Fusion 24 5.1 Multisensor Systems . . . 24 5.2 Kalman Filters . . . 24 5.2.1 Historical Background . . . 24

5.2.2 Linear Dynamic System Model . . . 25

5.2.3 Linear Kalman Filter . . . 26

5.2.4 Kalman Filter Extensions . . . 28

5.2.5 Implementation Methods . . . 30

6 Localisation 33 6.1 Full Model Kalman Filter . . . 33

6.1.1 Model . . . 33

6.1.2 Covariance Matrices . . . 35

6.1.3 Evaluation and Performance . . . 37

6.2 Parameter Estimating Kalman Filter . . . 38

(8)

Contents

6.2.2 Covariance Matrices . . . 40

6.2.3 Evaluation and Performance . . . 41

6.3 Slip Estimating Kalman Filter . . . 46

6.3.1 Model . . . 46

6.3.2 Covariance Matrices . . . 47

6.3.3 Evaluation and Performance . . . 48

7 Control 49 7.1 Vehicle Control . . . 49

7.1.1 Hydraulic Functions . . . 49

7.1.2 Speed and Brake . . . 49

7.2 Navigation Control . . . 51

7.2.1 Path Representation . . . 51

7.2.2 Path Following . . . 51

8 System Design and Integration 57 8.1 Communication . . . 57

8.1.1 Original Solution . . . 57

8.1.2 Revised Solution . . . 58

8.2 Interface . . . 59

8.3 Realtime System Design . . . 59

8.4 Integration . . . 60

9 Conclusion 61 9.1 Results . . . 61

9.1.1 Localisation . . . 61

9.1.2 Navigation . . . 62

9.2 Recommendations and Further Work . . . 62

References 62

A Kinematic Model Calculations 66

(9)

Contents

C Evaluation Course Descriptions 70

C.1 Evaluation Course POND . . . 70

C.2 Evaluation Course WOODS . . . 71

D Rehandling Cycle Demonstration 73

E Heuristic Drift Reduction 75

F Sensor Specifications 77

F.0.1 Calibrated Outputs of Inertial Measurement Unit . . . 77

(10)

List of Figures

1.1 A wheel loader of the 120F model used as platform in the Autonomous Machine project. 2

1.2 An overview of the platform hardware. . . 7

2.1 Illustration of the local tangental plane and its Cartesian coordinate system. . . 8

2.2 The vehicle body fixed coordinate system. . . 9

3.1 Illustration of the conceptual idea of GPS positioning. . . 13

4.1 Illustration of the lateral slip angle and the related velocity vectors. . . 19

4.2 Schematic illustration of an articulated vehicle. . . 20

4.3 A schematic description of the soft sensor based on the full kinematic model. . . 22

4.4 Comparison between the angular velocity as measured by the gyro and the soft sensor. 23 4.5 The absolute error between the gyro measurement and the calculated angular velocity. 23 5.1 Direct pre-filtering implementation scheme. . . 30

5.2 Direct filtering implementation scheme. . . 31

5.3 Indirect feedforward implementation scheme. . . 31

5.4 Indirect feedback implementation scheme. . . 32

6.1 Path estimated by the full model filter, compared to the path measured by the GPS. 37 6.2 Estimation of the gyro bias. . . 41

6.3 Estimation of the average wheel radius. . . 41

6.4 The estimated path illustrated together with the path as measured by the GPS. . . 42

6.5 Estimation of the gyro bias. . . 43

(11)

List of Figures

6.7 Illustration of the estimated path in comparison with a true ground reference. . . 44

6.8 The positional error illustrated together with the horizontal dilution of precision. . . 45

6.9 The absolute orientation error illustrated. . . 45

6.10 The difference between the integrated gyro orientation and the orientation derived from GPS velocity components. . . 46

6.11 Illustration of the estimated lateral body slip together with the articulation angle. . 48

7.1 The velocity measured by the rotary encoder illustrated together with the given setpoint. 50 7.2 The geometry of the pure pursuit algorithm. . . 52

7.3 The geometry relating the curvature to the articulation angle. . . 54

7.4 The estimated driven path illustrated together with the given waypoints. . . 55

7.5 Comparison between the articulation angle setpoint and the measured angle. . . 56

8.1 A schematic description of the implemented system. . . 60

8.2 Illustration of the subsystem architecture with a resource access selector. . . 60

B.1 Geometry of circular paths. . . 69

C.1 An approximation of the evaluation course POND. . . 70

C.2 The logged Horizontal DOP value during the three laps. . . 71

C.3 The evaluation course denoted as WOODS. . . 72

C.4 The logged Horizontal DOP value during the lap. . . 72

D.1 The complete autonomous load and haul cycle. . . 73

E.1 Heuristic drift reduction implementation scheme. . . 76

E.2 Comparison between gyro bias estimations conducted by an EKF and the HDR algo-rithm. . . 76

(12)

List of Tables

F.1 Accelerometer specification. . . 77

F.2 Rate gyroscope specification. . . 77

F.3 Magnetometer specification. . . 77

(13)

Nomenclature

This nomenclature lists all abbreviations and parameters used throughout the thesis, only the most commonly used variables are listed.

Parameters

R Average wheel radius [m]

L1,2 Distance between the articulation hinge and a wheel axle [m]

P1,2 Wheel axle midpoint position [m, m]

Variables, greek letters

ϕ Articulation angle [rad] ˙

ϕ Articulation angle rate [rad s−1] v Vehicle velocity [m s−1] ω Wheel angular velocity [rad s−1] θ Vehicle orientation / yaw [rad]

˙

θ Vehicle angular rate / yaw rate [rad s−1]

β Body slip [rad]

Variables, latin letters

b Gyro bias [rad s−1] l Look-ahead distance [m] T Engine throttle [%] E Eastward position [m] N Northward position [m]

(14)

Abbreviations

Volvo CE Volvo Construction Equipment AGV Autonomous Ground Vehicle GPS Global Positioning System

DGPS Differential Global Positioning System CEP Circular Error Probability

HDOP Horizontal Dilution of Precision VDOP Vertical Dilution of Precision WGS84 World Geodetic System 1984 MEMS Microelectromechanical Systems

ALVINN Autonomous Land Vehicle In a Neural Network ANN Artificial Neural Network

IMU Inertial Measurement Unit LIDAR LIght Detection And Ranging ADC Analogue-to-Digital Converter DAC Digital-to-Analogue Converter GPIO General purpose Input/Output DSP Digital Signal Processor KF Kalman Filter

EKF Extended Kalman Filter UKF Unscented Kalman Filter LHD Load-Haul-Dump (vehicle) IIR Infinite Impulse Response PP Pure Pursuit

SMA Simple-Moving-Average

PID Proportional-Integral-Derivative (regulator) PI Proportional-Integral (regulator)

UDP User Datagram Protocol TCP Transmission Control Protocol

SLAM Synchronous Localisation and Mapping MATLAB MATrix LABoratory

IEEE Institute of Electrical and Electronics Engineers MDU Mälardalen University

PIP Packaged Industrial Personal Computer GUI Graphical User Interface

CAN Controller Area Network LSB Least SigniÞcant Bit

(15)

Chapter 1

Introduction

1.1

Background

1.1.1

Autonomous Machine Project

The Autonomous Machine project is a initiative taken by Volvo Construction Equipment (Volvo CE) to develop and build autonomous wheel loaders and in the prolonging autonomous haulers. The project has been ongoing for three years, where the development has mainly been based on the work of Master’s Theses.

1.1.2

Volvo Construction Equipment

Volvo CE is one of the leading companies on the construction equipment and heavy machinery market. The company’s history begins back in 1950 when AB Volvo bought an agricultural machine manufacturer renamed to Volvo BM AB. The company expanded globally during the 1980s and 1990s by purchasing companies in America, Asia and Europe. Today Volvo CE’s product range is of a big diversity featuring for an instance, wheel loaders, haulers, crawling and wheeled excavators, motor graders, demolition equipment and pipelayers.

1.1.3

Wheel Loaders

A wheel loader is a versatile vehicle able to perform a wide variety of work tasks in many different environments. In its typical configuration a wheel loader is equipped with an actuator arm with two degrees of freedom. At the endpoint of the arm a tool is attached. The probably most common tool is a bucket, but there exist a vast selection of tools suited for different situations and tasks.

(16)

Chapter 1: Introduction

hydraulic hinge divides the vehicle in two sections making them able to rotate relative each other. Another characteristic feature of a wheel loader is the stiff front axle. Having a stiff front axle makes the vehicle stable and able to handle heavy loads. The rear axle is often freely suspended in a pivot for increased mobility in rough terrain.

Figure 1.1: A wheel loader of the 120F model used as platform in the Autonomous Machine project.

1.2

Problem Specification

The intention of this thesis is to enable the autonomous wheel loader used in the Autonomous Machine project to navigate in a given task space. In short, the problem at hand could be divided into two distinct parts. Firstly, the system needs to be capable of localising itself in its task space using available sensors mounted on the vehicle. The second problem is to autonomously steer the wheel loader in its task space to and from given positions.

In order to solve the first and the second problem, it is necessary to understand how an articulated vehicle such as a wheel loader behaves and responds in terms of steering i.e. a model needs to be derived accordingly. The localisation of the vehicle will be based on the readings of the vehicle’s internal and external sensors. Therefore, it is essential to establish models of the sensors in terms of errors and noise. Another concern is how to, with respect to errors and noise, combine the information provided by the sensor readings. Finally, steering the vehicle requires a stable and accurate control law, but also a way to represent and communicate the intended drive path.

(17)

Chapter 1: Introduction

1.3

Objectives

The main objective of this thesis is to conceptually demonstrate the possibility of utilising an au-tonomous wheel loader for the purpose of conducting a simple and repetitive work task at a produc-tion site. The targeted producproduc-tion site in the Autonomous Machine project is an asphalt plant.

Material rehandling is the simplest and most repetitive task found at such site; gravel or similar materials is stocked at the site and where a wheel loader is utilised for the purpose of transporting the material from the stock into the production. In the case of the asphalt plant the different materials are unloaded in pockets leading to a conveyor belt feeding the plant.

The motivation of the main objective, and thus this Master’s Thesis, is given as follows. A human driver becomes tired and unfocused by performing the same task for the duration of an entire shift. Two consequences arise; a tired driver is potentially dangerous and could cause lethal accidents or serve injuries; the second consequence is decreased productivity. Another motivation is the ability of an autonomous vehicle to always drive in an economical way if the situation allows it, which could be overlooked by an unfocused driver.

1.4

Safety

The vehicle is assumed to be operating in an enclosed and secured area. No humans will be present in the vicinity of the autonomous vehicle nor in its task space during autonomous operation. A wireless industrial classified safety stop has been installed in the vehicle allowing the responsible operator to shut it down, at shutdown the parking brake is automatically activated bringing the vehicle to a halt.

All personnel involved in the development of this autonomous vehicle have been educated and certified accordingly for operating wheel loaders.

1.5

Delimitations

The vehicle’s task space is assumed to be moderately planar, as a consequence only planar motion will be treated. Trajectory drive paths are calculated or stored in a strategic high level software, and thus no such planning will be conducted in the following work. Furthermore, obstacles located in the task space is assumed to be static. Obstacle avoidance will therefore not be a subject of this thesis.

(18)

Chapter 1: Introduction

1.6

Thesis Outline

This section is intended to give a brief overview of the contents found in the chapters constituting this Master’s Thesis report.

The coordinate systems used throughout the work presented in this Master’s Thesis, are defined in the brief chapter 2.

Chapter 3 gives a study of the internal and external sensors mounted on the vehicle. The study includes the modelling of the sensors and a discussion regarding the involved measurement errors.

Chapter 4 starts with a study of related work regarding the modelling of articulated vehicles. The study tries to outline the main aspects to consider during the modelling process. Thereafter a kinematic model is derived based on the findings in the previous study.

In chapter 5 the reader is introduced to the concept of multisensor systems and sensor fusion with Kalman filters, both the classical linear Kalman filter and the extended Kalman filter are presented. A short review of another common extension is given, namely the unscented Kalman Þlter. The chapter ends with an overview of different implementation methods and schemes.

Chapter 6 continues the report by applying previously discussed technique of sensor fusion to the problem of localising the vehicle in its task space. Thereafter the second problem of steering the vehicle is attended in chapter 7. The chapter does also describe other aspects regarding the vehicle’s control.

The design and work of integrating the developed solution into the existing system architecture is described in chapter 8, an description of the implemented communication interface is also given.

Chapter 9 finalises the report by discussing the findings and results made through the work of this thesis.

1.7

Related Work

Autonomous vehicles have traditionally been a subject of military usage. However, as in the case of many technologies they emerge from the military sector into civilian applications as the price of the technology gets more affordable. Autonomous vehicles are complex systems requiring computa-tional power and often equipped with a wide variety of sensors. The rapid progress of computers becoming smaller, more powerful and affordable enhances the possibility to develop autonomous systems without a military founding. Regarding sensors, the technology of microelectromechanical systems (MEMS) revolutionised sensors, both regarding their price and size. Today rather advanced sensors such as accelerometers and gyros could be found in cars, mobile telephones and video gaming devices.

(19)

Chapter 1: Introduction

The term autonomous is often interchanged with and equalled to the term unmanned. The latter term refers only to a vessel that carries no human operator, hence it by definition could be remotely operated by a human. In contrary, an autonomous vehicle operates without a human operator’s intervention.

1.7.1

Autonomous Ground Vehicles

One of the earliest designs of a fully autonomous ground vehicle (AGV) was the Autonomous Land Vehicle In a Neural Network (ALVINN) developed 1993 at the Carnegie Mellon University. The concept was based on learning an artificial neural network (ANN) to follow a road. The road was sensed with an image array constituted by 30 times 32 pixels. The system successfully drove 145 kilometres without human assistance, and is capable of driving on both dirt roads and paved roads with multiple lanes.

The probably most recognised AGVs today are the participators of the DARPA Grand Challenge held in 2004 and 2005, and the DARPA Urban Challenge held in 2007. In the latest Grand Challenge, an off-road course stretching over 210 kilometres was the subject of autonomous driving with a time limit of 10 hours. The winning team from Stanford University completed the course in just under 7 hours [1]. The course was completed by 5 of the 23 participating teams, in the first challenge non of the 15 vehicles managed to complete the course. The Urban Challenge featured a 96 kilometre long urban course to be completed within 6 hours, won by the Carnegie Mellon University completing the course in a little over 4 hours.

The participating vehicles are standard cars retrofitted with computers and sensors. Typical sensors are Global Positioning System (GPS) receivers, laser rangers (LIDAR), cameras, microwave radar and inertial measurement units (IMU). The large amount of data produced by the vehicles’ sensor systems put high demands on the software architecture; emphasis is on high efficiency and config-urability. The high complexity of the involved control and optimisation problems led to the usage of adaptable and learning algorithms.

An interesting project, and maybe a little more related, known as Autonomous Navigation for Forest Machines is held as a part of the research conducted at the Umeå University [2]. The project’s purpose is the design and development of algorithms and techniques intended for the navigation of autonomous vehicles in off-road environments. A forest machine has successfully been used for autonomously drive along previously learnt paths. The vehicle is able to localise itself by using GPS in conjunction with laser ranging and odometry.

(20)

Chapter 1: Introduction

1.7.2

Autonomous Mining Equipment

One established actor providing autonomous vehicle’s for the use in the underground mining industry is Sandvik’s AutoMine system [28]. There are several benefits of autonomous vehicle’s in the mining industry. First of all it increases the safety and improves the working conditions of the personnel by removing them from the hazardous environment of an underground mine. The more economical benefits are, among others, stated as increased productivity and lower maintenance cost.

The system is not fully autonomous since the bucket load sequence is tele-operated by a human. However, one human operator is capable of controlling a number of vehicles since they take turns loading their buckets. The communication is managed over a common wireless local area network with added realtime features.

There are several publications related to the autonomous loaders used in underground mines, espe-cially regarding the modelling of such vehicles. Due to their similarity to a wheel loader; a more in depth related work study is given in chapter 4 treating the modelling of the autonomous wheel loader.

1.8

Platform

The selected platform for the autonomous wheel loader is conventional wheel loader of the 120F model; a model belonging to the midsize wheel loaders offered by Volvo CE. With a weight of 20 ton it is capable of lifting some 6 to 7 ton in normal operation and is typically used in applications involving material rehandling.

A ruggedised industrial computer, a PIP8 manufactured by MPL AG, featuring a realtime software environment is used for interfacing the vehicle’s different functions and sensors. The sensors are interfaced via RS232 and by the PIP8’s analogue-to-digital converters (ADC). The realtime envi-ronment, known as xPC Target, is capable of running compiled Simulink models in realtime. The models are developed in the standard Simulink environment and then compiled through C to a single file that is downloaded to the PIP8 computer.

The hydraulic functions powering the actuator and the articulated steering are controlled by the PIP8’s digital-to-analogue converters (DAC) via an electrical servo system. The throttle and the brake system is controlled in a similar manner. Utility functions with on-off characteristics are controlled by general purpose input/output (GPIO) ports. Besides the DAC and the GPIO interface, the PIP8 is connected to the vehicle’s Controller Area Network (CAN) bus. At the current stage the CAN bus is mostly used for supervision purposes.

(21)

Chapter 1: Introduction

An additional computer is utilised for non-realtime tasks denoted as the high level strategic software. The high level software is developed in parallel with this thesis as a separate thesis [3]. The main responsibility of the high level software is to coordinate and command the vehicle accordingly to fulfil the current objective, and provide an graphical user interface (GUI). This computer is connected to the PIP8 computer through a standard Ethernet connection.

LIDAR Vision Algorithms

Strategic Software GUI

Sensors Realtime System

Vehicle Hardware

Figure 1.2: An overview of the platform hardware.

A third computer dedicated to the execution of vision algorithms on the data gathered from a SICK MLS291 laser ranger (LIDAR), mounted with a servo on the cab’s roof edge, was added to the hardware architecture during the timeline of this thesis. The vision algorithms are able to detect piles and other objects that the vehicle autonomously needs to interact with. The algorithms are developed by the Örebro University as a part of their research in the field of autonomous vehicles. In-teraction with the vision algorithms are conducted by the high level software over Ethernet, utilising a TCP/IP interface. The interface delivers a vector relative the vehicle to the found objects.

(22)

Chapter 2

Coordinate Systems

2.1

Local Planar Frame

Commonly positions are given in geodetic coordinates comprised by latitude, longitude and altitude, where the Earth is approximated by an ellipsoid. A widely known such system is the World Geodetic System 1984 (WGS84) used by for an instance the Global Positioning System (GPS).

However, since the vehicle is intended to just travel on a relative small geographical region, rendering the effect of the Earth’s curvature rather insignificant, it is feasible to approximate that region with a planar system. The planar system is given by the tangental plane to the Earth’s surface fixed to a reference point located in the region of interest.

E (east) N (north)

U (up)

(23)

Chapter 2: Coordinate Systems

The transformation from longitude λ and latitude φ in the geodetic coordinate system to the eastward E and northward N coordinates in the local tangental plane are done in accordance with

E = Re∆λcos(φ)

N = Re∆φ

where Reis the Earth’s radius at the current latitude, and

∆λ = λ − λref erence

∆φ = φ − φref erence.

2.2

Vehicle Body Fixed Frame

The vehicle body fixed frame is a coordinate system having its origin in the middle point of the vehicle’s rear wheel axle. The reason why the rear body of the wheel loader was selected to host the vehicle body frame was simply due to the fact that all vital sensors are located there. The x-axis points in the forward direction denoted as the longitudinal axis. Perpendicular to the x-axis, the y-axis points to the left. The y-axis is also referred to as the lateral axis. Lastly the z-axis is specified to point skywards, which completes the right hand system.

ˆ x

ˆ

y zˆ

(24)

Chapter 3

Sensors

In this section the reader will be introduced to the sensors installed on the vehicle. Sensors that do not concern the vehicle’s translational movement are, for the cause of simplicity and comprehensibility, omitted.

3.1

Sensor Measurement Model

It is crucial to understand what a particular sensor actually measures in order to utilise the obtained data in an appropriate manner. The measurement of a physical quantity is not a direct reflection of its true value since it contains errors of different characteristics. In the simplified measurement model (3.1.1) of the arbitrary physical quantity x, errors such as scalar error kx, bias error bx, normal

distributed white noise ηxand quantisation error ∆xare taken into account. In reality other errors

exist, but they will be assumed to be rather small in comparison to the errors already accounted for in the model and are therefore neglected.

˜

x = (1 + k)x + bx+ ηx+ ∆x (3.1.1)

3.2

Odometry Sensors

In order to perform simple odometry one needs to measure the articulation angle and the linear velocity of the vehicle as shown later in chapter 4. The two sensors described below were mounted prior the work of this thesis.

(25)

Chapter 3: Sensors

3.2.1

Articulation Angle Sensor

The sensor measuring the articulation angle is a linear potentiometer working in the range 0.5 to 4.5 volts, where 0.5 volt corresponds to a -60 degrees angle and 4.5 volt to 60 degrees. However, only a limited span of the available range is utilised since the mechanical range of the articulation angle is between -36 to 36 degrees. The given model below models the sensor in terms of degrees

˜

ϕ = ϕ + bϕ+ ηϕ (3.2.1)

where the bias error bϕhas been experimentally determined to 0.3 degrees and the noise ηϕhas been

approximated to have one standard deviation of 0.03 degrees.

3.2.2

Rotary Encoder

An encoder wheel together with two inductive sensors, mounted on the centre drive shaft delivering power to the front and rear wheel axles, are used to calculate the angular velocity of the wheels. The output from the inductive sensors are counted in a register, where one least significant bit (LSB) correlates to a drive shaft rotation of 0.148 radians. A 18.37:1 drive shaft to wheel ratio gives a wheel rotation of 0.008 radians per LSB. The wheel angular velocity derived from the encoder, by differentiation and averaging, is modelled by

˜

ω = (1 + kω)ω + ∆ω (3.2.2)

where kω and ∆ω denote a scalar error and a quantisation error, respectively. Regarding the scalar

error kω, it is believed to be rather small due to excessive calibrations. The quantisation error ∆ω

has been approximated to ±0.05 radians per second. The vehicle’s linear velocity is calculated in accordance with

˜

v = R˜ω (3.2.3)

where R denotes the average wheel radius. However, the wheel radius will change over time depend-ing on tyre wear down and the current load of the the wheel loader. Therefore it would be beneficial to estimate the radius with for an instance a Kalman filter (KF), which will be further investigated in chapter 6.

(26)

Chapter 3: Sensors

3.3

Inertial Measurement Unit

The Xsens MTi-G inertial measurement unit (IMU) is capable of outputting raw, calibrated and processed sensor measurements, where the latter is processed by an extended Kalman filter (EKF) in the unit’s internal digital signal processor (DSP). It also features a built-in Global Positioning System (GPS) receiver. In the calibrated output mode the raw readings from the 16-bit ADC are converted to physical quantities and where calibration with, among others, respect to temperature, bias, sensitivity and misalignment1 has been accounted for. Important to say is that no filtering or

other temporal processing has been applied to the calibrated measurements.

The calibrated output contains the measurements of acceleration, angular velocity and magnetic field strength2, in six degrees of freedom. Appendix F provides the sensor specifications. An acceleration

measurement is given by

˜

a = a + ηa (3.3.1)

where only the noise ηa has been considered as an error.

Even though the gyroscopes measuring angular velocities are carefully calibrated, they drift i.e. there will always be an apparent angular velocity in their measurements. For that reason the gyroscope model will contain a bias error denoted as bψ. The noise ηψ needs to be considered as well, thus the

resulting model is

˜

ψ = ψ + bψ+ ηψ. (3.3.2)

Lastly the magnetometer measurements are modelled by

˜

β = β + bβ+ ηβ (3.3.3)

where the bias bβ could be an effect of, for an instance, permanent magnets and ferromagnetic

materials in the vicinity of the IMU.

Even though the IMU provides a sophisticated sensor fusion by its internal EKF serve difficulties regarding drift in the heading estimation has been experienced. An analysis of the magnetometer data shows major disturbances during turning, which is caused by the nature of a wheel loader’s mechanical arrangement. The movement of the substantial masses of steel seems to skew the Earth’s magnetic field to a large extent. Different EKF configurations and physical placements on the wheel loader itself have not resolved the problem to a satisfactory level. The best performance was achieved with the configurations not accounting the magnetometers. However, then the unit looses an observer of the heading, making the GPS the only available source of heading corrections. As a consequence

1Misalignment relative the sensor housing. 2Normalised to the Earth’s magnetic field strength.

(27)

Chapter 3: Sensors

the heading estimate became a subject to a serve drift during standstills. For that reason it was decided that the calibrated sensor output and the GPS output will be the only outputs utilised from the IMU for localisation purposes i.e. its internal EKF will not be used. It was believed that a sensor fusion utilising more sensors than available to the IMU would be capable of achieving a more satisfactory performance, which is further explained in chapter 5.

3.4

Global Positioning System

The Global Positioning System (GPS) is a Global Navigation Satellite System (GNSS) used in a vast range of both military and civilian applications where localisation and time synchronisation is essential. In short, the GPS consist of a set of satellites continuously transmitting a radio signal containing a time stamp of the transmission and the orbital position of the satellite in question. A GPS receiver is thereby able to locate itself by calculating the received signals’ travel times and interpret the times as imaginary spheres originating from the corresponding satellites. The intersections of those spheres define the possible positions of the receiver’s antenna. Figure 3.1 illustrates the principle of GPS positioning, where a, b, c denotes three visible satellites and r the position of the receiver antenna.

a b c

r

Figure 3.1: Illustration of the conceptual idea of GPS positioning.

The GPS receiver used in this particular thesis is capable to give measurement updates at a rate of 4 Hz and features 50 channels. It is also prepared for the GALILEO system - a future alternative to the GPS. The types of measurements provided by the receiver are as follows, firstly the position measurement " ˜E ˜ N # = " E N # + ηp (3.4.1)

where ˜E and ˜N are the measured east and north positions on the local tangent plane frame, respec-tively. Regarding the positional noise ηpit is specified as 2.5 meters circular error probability (CEP).

That is, that 50% of the measurement samples are located within a circle of 2.5 meters radius. The standard deviation is calculated by

(28)

Chapter 3: Sensors

σp=

1.2CEP √

2 (3.4.2)

which in this particular case gives a standard deviation σp of roughly 2 meters applicable to both

the east and north position measurements.

The used GPS receiver compares the apparent carrier frequencies of the satellites with a local oscillator and can thereby calculate the Doppler shifts. The knowledge of the Doppler shifts, in conjunction with the quite small wavelength of the GPS signals, it is possible to determine the receiver’s velocity with a rather high accuracy. In contrary, GPS receivers of an older model or receivers with limited computational capabilities are often incapable of utilising this technique [4]. Instead the velocity is a derivative of the current and previous positions, yielding a very coarse accuracy. The measured velocities in the tangent plane are given in the eastward direction ˜vE and

the northbound direction ˜vN in accordance with

" ˜ vE ˜ vN # = " vE vN # + ηv. (3.4.3)

A significant advantage of accurate GPS velocity measurements is the possibility to calculate the heading with rather high accuracy, even in low speeds. If no vehicle side slip occurs the heading could simply be calculated by

˜ θGP S = arctan( ˜ vN ˜ vE ). (3.4.4)

Trials have shown that the GPS heading is reliable in speeds above 1.5 meters per second.

3.4.1

Dilution of Precision

The additional loss in precision due to the GPS satellites’ relative geometrical configuration is reflected through certain dilution of precision (DOP) measurements. Generally speaking, the highest precision is obtained when the available satellites are evenly distributed on the sky, hence a minimal DOP value will be obtained. In contrary, a high DOP value will be obtained if the satellites are clustered together. Another factor having a major impact on the DOP measurements are obstacles blocking or reflecting the satellite signals.

There are several independent DOP measurements characterising the current precision that could be expected. The GPS receiver used in this thesis gives for every update a horizontal dilution of precision (HDOP) and a vertical dilution of precision (VDOP). Regarding the positioning in the plane, the HDOP measurements can be used for detecting invalid GPS positions.

(29)

Chapter 3: Sensors

3.4.2

Differential GPS

The fundamental concept behind a Differential Global Positioning System (DGPS) is the fact that receivers in the same geographical region experiences roughly the same atmospheric interference. Furthermore, receivers using the same set of satellites will be the subject of the same satellite clock errors affecting the positioning precision. Therefore it would be beneficial if one receiver could be utilised to determine the error in its region, an error that is forwarded to other user receivers in the region making them able to correct their measurements. For an instance, a common method is to mount a receiver at a fixed known position and broadcast the calculated error over radio. Even though the atmospherical interference is approximately the same, it will vary. As a consequence the correction accuracy will degrade with the user’s distance from the reference transceiver. Another problem, affecting the accuracy negatively, arises if a user does not see the same set of satellites used by the reference.

(30)

Chapter 4

Vehicle Modelling

A model describing how a vehicle’s position and other important vehicle parameters evolve over time is an essential feature in a successful autonomous navigation system. Even though a wheel loader has many attributes to model this report will focus on modelling the traversing motion of a wheel loader in its task space. In other words the modelling of the wheel loader’s actuator will not be covered.

4.1

Related Work

The following section summarises selected work described in literature regarding the modelling of articulated vehicles. The reader should be aware of that a commonly appearing vehicle in such literature is the Load-Haul-Dump (LHD) vehicle - a heavy equipment vehicle moving ore from the rock face to a dump point in underground mining facilities. Even though the LHD operates in a different environment than a wheel loader; the two vehicles are more or less identical in the modelling point of view.

Regarding autonomous navigation of articulated vehicles there has been some debate in literature about whether lateral slip should be neglected or not in the model [5]. The latter requires the vehicle’s dynamics to be accounted for in contrary to the former where only the vehicle’s kinematic geometry is sufficient for deriving a model.

Scheding et al. [6] compares different navigation systems of an autonomous LHD vehicle. It was concluded that the vehicle slipped to such an extent, especially during cornering, that a system only utilising dead reckoning based on a non-slip model through odometer data and articulation angle simply failed. The effect of excluding the slip causes the model to overestimate the vehicle’s turning rate. As a consequence the model error needs to be corrected continuously. The non-model was derived from rigid body and rolling motion constrains and is stated by

(31)

Chapter 4: Vehicle Modelling ˙ x = V cos(θ) ˙ y = V sin(θ) (4.1.1) ˙ θ = Vtan( ϕ 2) L

where x, y and θ denotes the vehicle position and orientation relative to a global frame. V is the vehicle’s linear velocity, ϕ the articulation angle between the two body halves and L is the length from a body’s wheel axle and the articulation hinge1.

The more successful system involved a model explicitly accounting for slip, where the slip was esti-mated with inertial sensors and an Extended Kalman Filter (EKF) since it is not directly observable. It was found that modelling the slip accurately was extremely difficult since it depended on numerous of other parameters in a highly nonlinear manner. The model accounting for slip is given by

˙ x = V cos(α + θ) ˙ y = V sin(α + θ) (4.1.2) ˙ θ = V sin(β − α + ϕ) L(cos(β) + cos(β + ϕ) − ˙ϕ L cos(β) L(cos(β) + cos(β + ϕ)

where the new variables α and β denotes the slip angles of the wheel pairs on respective axle. A slip angle is defined as the angle between the kinematically indicated velocity vector and the true velocity vector of that particular body section. Observe the utilisation of the first derivative of the articulation angle ϕ.

In the approach purposed by Dragt et al. [7] Lagrangian dynamics is used for the purpose of modelling a LHD vehicle. The vehicle model includes tyre models in an attempt to account for basic tyre dynamics such as cornering slip. However, their work was only conducted to the extent of limited simulations i.e. no comparisons were made with the respect to a real vehicle. One difficulty experienced in their work was the missing data on tyre parameters.

Altafini [8] derives a non-slip model and states that the dynamic effects could be neglected since the vehicle in question operates in slow speeds. The derived non-slip model accounts for vehicle specific features such as lengths between wheel axles and the articulation hinge. Compared to the previous presented non-slip model (4.1.1) it also takes the articulation angular rate in account when computing the vehicle’s rate of change in orientation. In fact, the model derived by Altafini [8] is very similar to the dynamic model purposed by Scheding et al. [6]. The difference is the two additional slip angles found in the latter model. The model derived by Altafini [8] is given by

(32)

Chapter 4: Vehicle Modelling ˙ xi = V cos(θi) ˙ yi = V sin(θi) (4.1.3) ˙ θ1= V sin(ϕ) L2+ L1cos(ϕ) − ˙ϕ L2 L2+ L1cos(ϕ) ˙ θ2= V sin(ϕ) L2+ L1cos(ϕ) + ˙ϕ L2cos(ϕ) L1+ L1cos(ϕ)

where i = {1, 2} and the indexes of x, y, L and θ refer to one of the two body halves.

In the work by Corke et al. [9] the model (4.1.3) by Altafini [8], referred to as the full kinematic model, is adopted and compared to the simpler kinematic model (4.1.1) used in Scheding et al. [6]. It is shown both experimentally and theoretically that the simpler model has a significant bigger error in orientation than the full kinematic model (4.1.3), especially at lower speeds. The trials were carried out on a full size LHD using a set of IMUs as references.

4.2

Wheel Slip

The literature previously studied indicated the importance of accounting for wheel slip. Therefore a short review of both longitudinal and lateral slip will be given in this section.

4.2.1

Longitudinal Slip

The longitudinal wheel slip s is commonly defined as the difference between wheel circumference velocity and the vehicle’s longitudinal ground velocity in accordance with

s =vx− ωre vx

(4.2.1)

where vx is the vehicle’s longitudinal velocity and ωre the angular velocity of the wheel multiplied

with the wheel’s effective radius, the latter is also known as the unloaded radius.

Longitudinal slip will occur if the friction force between the wheel and the ground surface is exceeded by the force induced by the torque applied to the wheel. For small values the relationship between slip and the utilised friction coefficient µ = Fx/Fz, where Fxis the friction force and Fz the wheel

normal force in a wheel hub centred coordinate system, is linearly given by µ = ks. On solid surfaces the wheel normal force Fz could be approximated by the normal force N . However, in

off-road conditions the ground is deformed by the wheel, and as a consequence the wheel normal force Fz is shifted towards the direction of motion.

(33)

Chapter 4: Vehicle Modelling

4.2.2

Lateral Slip

A vehicle negotiating a turn is the subject of an angular velocity changing its orientation and an lateral acceleration directed towards the instantaneous centre of rotation, thus a side force Fy will

arise in the contact surface between the wheel and the ground surface. Consequently the side force Fy induces an lateral slip defined, more precise than previously, as the angle α between the wheel’s

rotational direction and the velocity vector of the ground contact point i.e. the true ground velocity. For small values of lateral slip α, the side force Fy could linearly be related to α through a wheel

specific parameter known as the cornering stiffness C. That is, Fy = Cα.

ωre

v

α

(34)

Chapter 4: Vehicle Modelling

4.3

Kinematic Model

In the following section the full kinematic model (4.1.3) described in [8] will be derived. According to [9] this particular kinematic model seems to be the more accurate one of the two discussed non-slip models, consequently the simpler kinematic model is neglected in favour of the full kinematic model. Even though the selected model may not be as accurate as the models accounting for slip, it is of a much lower complexity since modelling the slip is a non-trivial problem [6]. However, if it is later concluded that lateral slip must be accounted for, it will be rather straightforward to adapt the model due to its similarity with the slip model (4.1.2).

ˆ x ˆ y θ1 θ2 ϕ L1 L2 P1(x1, y1) P2(x2, y2)

Figure 4.2: Schematic illustration of an articulated vehicle.

Referring to figure 4.2, the geometrical relationship between the two wheel axle midpoints P1and P2

located on the distances L1and L2from the articulation joint is given by the two following equations

x2= x1+ L1cos(θ1) + L2cos(θ2) (4.3.1)

y2= y1+ L1sin(θ1) + L2sin(θ2)

and the relationship between the angles by

ϕ , θ2− θ1 (4.3.2)

where θ1 and θ2 are the orientation angles of respective body halve and ϕ is the articulation angle.

The motion of an axle midpoint Pi is described by

˙

xi= vicos(θi) (4.3.3)

˙

(35)

Chapter 4: Vehicle Modelling

where i = {1, 2} and vi is the velocity vector associated to that midpoint during motion. The

assumption of no lateral slip during motion implies that there are no velocity components perpen-dicular to the two body halves, which is stated by

˙

x1sin(θ1) − ˙y1cos(θ1) = 0 (4.3.4)

˙

x2sin(θ2) − ˙y2cos(θ2) = 0.

Differentiating (4.3.1) with respect to time and substituting the obtained velocity components with (4.3.3) and (4.3.4) will after simplification using (4.3.2) and the angle sum and difference identities for sine and cosine result in

˙ θ1= v1 sin(ϕ) L2+ L1cos(ϕ) − ˙ϕ L2 L2+ L1cos(ϕ) . (4.3.5)

Using same the methodology but solving for θ2gives

˙ θ2= v1 sin(ϕ) L2+ L1cos(ϕ) + ˙ϕ L1cos(ϕ) L2+ L1cos(ϕ) . (4.3.6)

The complete calculations of how to derive (4.3.5) and (4.3.6) from the given equations are avaiable in appendix A.

Let, for simplicity, the set ( ˙x1, ˙y1, ˙θ1) refer to the rear body halve and the set ( ˙x2, ˙y2, ˙θ2) to the front

body halve. Then the rear body could be described by the following kinematic model

        ˙ x ˙ y ˙ θ         =         cos(θ) 0 sin(θ) 0 sin(ϕ) L2+ L1cos(ϕ) − L2 L2+ L1cos(ϕ)             v ˙ ϕ     (4.3.7)

and the front body by

        ˙ x ˙ y ˙ θ         =         cos(θ) 0 sin(θ) 0 sin(ϕ) L2+ L1cos(ϕ) L1cos(ϕ) L2+ L1cos(ϕ)             v ˙ ϕ     (4.3.8)

(36)

Chapter 4: Vehicle Modelling

4.3.1

Model Verification

The most essential aspect of the full kinematic model previously derived is the vehicle angular velocity; a physical quantity that is measured by the gyro as well. The validity of the model is thereby possible to verify by comparing the angular velocity measured by the gyro with the value obtained from the model.

The concept of a soft sensor was applied to the model. Even though the implementation in figure 4.3 seems rather straight forward, there are some issues to account for. It is not feasible to differentiate the articulation angle ϕ due to noise. For that particular reason a second order Infinite Impulse Response (IIR) low-pass filter has been added prior the differentiator. The low-pass filter’s cut-off frequency needs to be low enough to yield an acceptable differentiation, but a too low cut-cut-off frequency will introduce a noticeable phase lag degrading the soft sensor’s performance. A cut-off frequency of 1 Hz and a damping of √1

2 where found to be appropriate.

ω

ϕ Model ψ

IIR Low-pass δϕδt

(37)

Chapter 4: Vehicle Modelling

In figure 4.4 and 4.5 the soft sensor is compared to the gyro during a manual drive at the track denoted as POND. The former figure, comparing the two angular velocities, clearly indicates a tendency of the calculated angular velocity to be overestimated. Still it must be concluded that the model reflects the behaviour of the vehicle.

0 50 100 150 200 250 300 350 400 450 −30 −20 −10 0 10 20 30 Time [s]

Yaw Rate [deg/s]

Gyro Soft Sensor

Figure 4.4: Comparison between the angular velocity as measured by the gyro and the soft sensor.

0 50 100 150 200 250 300 350 400 450 0 1 2 3 4 5 6 7 Time [s]

Absolute Error [deg/s]

(38)

Chapter 5

Sensor Fusion

In this chapter the reader will initially be given an introduction to multi-sensor fusion and one of the most common tool used for that particular purpose, namely the Kalman filter (KF). The chapter continues with an overview of selected KF extensions and common implementation methods.

5.1

Multisensor Systems

Sensor fusion is the technique of combining measurements from multiple sensors in a system such that the combined result is more advantageous than if the measurements were individually used. The provided advantages gained from multi-sensor fusion are; a more wider and diverse aspect of the system is measured; the robustness of the system is possible to increase by statistical methods, complementary measurements and sensor redundancy [10].

5.2

Kalman Filters

A Kalman filter estimates the state of a noisy linear dynamic system by noisy measurements that could be linear related to the system’s state. If the corrupting noise is independent, white and normal distributed with a zero mean; the KF will be a statistically optimal estimator with respect to any reasonable quadratic function of estimation error.

5.2.1

Historical Background

The first formal method of acquiring an optimal estimation from noisy measurement was the method of least squares. A method whose discovery often is credited to Carl Friedrich Gauss in the late eighteenth century (1795) at an age of eighteen. Gauss recognised that the least square method

(39)

Chapter 5: Sensor Fusion

was just the beginning of numerous interesting studies. However, he stated that the time was not right for these studies and must be reserved for future occasions when faster computations could be accomplished [11].

Taking a 140-year leap in history until the early years of the Second World War, Norbert Wiener was involved in a military project regarding an automatic controller aiming anti-aircraft guns based on noisy radar tracking information. The controller needed to predict the position of an airplane at the time of the arrival of the projectiles. His work lead to the formulation of the Wiener filter, that is a linear estimator of a stationary signal and is optimal in terms of the minimum mean square error.

In 1958 Rudolf Emil Kalman got the idea of applying state variables to the Wiener filter. Two years later his filter, now known as the Kalman filter, was introduced to the researchers involved in the trajectory estimation and control problem of the Apollo program. In the following year of 1961 the filter was incorporated as a part of the Apollo onboard guidance system. In fact it was a modified version of the original KF called the Kalman-Schmidt filter; today known as the extended Kalman filter (EKF).

The Kalman filter is said to be one of the biggest discoveries in the history of statistical estimation theory. Without it many achievements after its discovery would not have been possible. It has become a vital component in a wide variety of applications spanning from estimating the trajectory of a spacecraft, to predicting the finance market.

5.2.2

Linear Dynamic System Model

One fundamental assumption made by the KF is that the true dynamic system state, denoted as x, evolves from the time instance k − 1 to k in accordance with a noisy linear dynamic system

xk = Fkxk−1+ Gkuk+ wk−1 (5.2.1)

where the different matrices and vectors are explained as follows. The state transition matrix Fk

relates the state xk−1to the proceeding state xk. Gkdenotes the input matrix and relates the input

vector uk to xk. The process noise wk is assumed to be normal distributed white noise with zero

mean and a covariance of Qk, more formally stated as

wk∼ N (0, Qk). (5.2.2)

Likewise the input noise γk is given by

(40)

Chapter 5: Sensor Fusion

Furthermore, if an observation zk is made of the state xk, the observation is described by the

observation model

zk = Hkxk+ vk (5.2.4)

where Hk is the observation matrix mapping the state space into the observation space. The

observation is assumed to be perturbed by normal distributed white noise vk with zero mean and a

covariance of Rk, known as the observation noise

vk ∼ N (0, Rk). (5.2.5)

5.2.3

Linear Kalman Filter

As previously stated the original KF assumes the underlying dynamic system to evolve in a linear manner and is for that particular reason commonly referred to as the linear Kalman filter (LKF).

Algorithm

A Kalman filter could be divided into two distinct phases. In the first phase, the prediction phase, the KF estimates the current state of the system based on the previous state using a given model of the system. This estimation is called the a priori state estimate since it is an estimate prior the observation of the system state. The latter is done in the second phase, the correction phase, where the current state is observed through sensor measurements. The observations are then combined with the a priori state estimate to form the a posteriori state estimate - an improved state estimation of the true system state.

Prediction Equations

Looking at the prediction phase, the a priori state estimate vector ˆxk|k−1 and the a priori estimate

error covariance matrix Pk|k−1 is given by

ˆ

xk|k−1= Fkxˆk−1|k−1+ Gkuk (5.2.6)

Pk|k−1= FkPk−1|k−1FTk + GkΓkGTk + Qk. (5.2.7)

In this phase the KF essentially drives the state forward in accordance with the system model and input model matrices represented by Fk and Gk, respectively. Through the covariance matrices Γk

and Qk, the KF is given knowledge about the noise corrupting the system state. Consequently the KF utilises this knowledge to, with the estimate error covariance matrix, reflect the uncertainty of the a priori state estimate.

(41)

Chapter 5: Sensor Fusion

Correction Equations

The correction phase begins with calculating the innovation1 y˜k reflecting the difference between

the actual observation zk and the predicted observation

˜

yk= zk− Hkxˆk|k−1 (5.2.8)

and where the innovation covariance is given by

Sk= HkPk|k−1HTk + Rk. (5.2.9)

The Kalman gain Kk is a factor determining the extent of the innovation accounted for when

calculating the a posteriori state estimate. The factor is determined by the relative uncertainty between the a priori state estimate and the innovation

Kk = Pk|k−1HTkS −1 k = Pk|k−1H T k(HkPk|k−1H T k + Rk)−1. (5.2.10)

Lastly the a posteriori state estimate ˆxk|k, where the predicted state is corrected by the knowledge

obtained from the observation, and the a posteriori estimate error covariance Pk|kare calculated by

ˆ

xk|k= ˆxk|k−1+ Kky˜k (5.2.11)

Pk|k= (I − KkHk)Pk|k−1 (5.2.12)

which completes the KF.

Kalman Gain

In order to understand the behaviour of the filter one should consider the Kalman gain calculation stated in equation (5.2.10). When Rkapproaches zero, implicating that the noise in the observation

is approaching zero, the Kalman gain increases

lim

Rk→0

Kk= H−1k . (5.2.13)

As a consequence the KF converges rapidly to the observed state since it is believed to contain more accurate knowledge about the true state than the a priori state estimate. On the contrary, the opposite effect occurs when the a priori estimate error covariance Pk|k−1 approaches zero

(42)

Chapter 5: Sensor Fusion

lim

Pk|k−1→0

Kk= 0. (5.2.14)

In other words the KF trust the prediction more than the observation and will act accordingly by converge slowly to the observed state. The time it takes depends on the process and input noise added at every iteration of the prediction phase, which is defined by Qk and Γk, respectively.

Initialisation

The initialisation of a KF is done by setting the state vector ˆxk−1|k−1to the true state of the system,

or what the current state could be assumed to be during initialisation. The uncertainty of the initial state is reflected by adjusting the covarience matrix Pk−1|k−1 accordingly.

5.2.4

Kalman Filter Extensions

Since the world, as we know it, often practices nonlinear dynamic systems the linear KF was limited in regard of its practical real world usage. Therefore the original KF was modified accordingly to conquer the applications involving the nonlinear systems.

Extended Kalman Filter

The extended Kalman filter (EKF) was the result of the inability to apply the LKF onto nonlinear systems, although the EKF resembles the LKF in many aspects. As implicated in the case of the EKF the state transition and observation models could be nonlinear functions

xk = f (xk−1, uk; γk) + wk−1 (5.2.15)

zk = h(xk; vk). (5.2.16)

The function f (· ) can directly be used by the EKF to compute the a priori state estimate

ˆ

xk|k−1= f (ˆxk−1|k−1, uk). (5.2.17)

However, it is not possible to apply f (· ) directly to the computations of the a priori estimate error covariance. To deal with this problem the EKF linearises f (· ) around the a posteriori state estimate ˆ

(43)

Chapter 5: Sensor Fusion Fk= ∂f ∂x xˆ k−1|k−1 (5.2.18) Gk= ∂f ∂u ˆx k−1|k−1 (5.2.19)

where the Jacobian matrices Fk and Gk contains the first-order partial derivatives of f (· ) with

respect to the state x and input u vectors, respectively. The prediction phase is completed by calculating the a priori estimate error covariance

Pk|k−1= FkPk−1|k−1FTk + GkΓkGTk + Qk. (5.2.20)

Similar to the case of f (· ) the function h(· ) is directly applied to the a priori state estimate in the purpose of calculating the innovation

˜

yk = zk− h(ˆxk|k−1). (5.2.21)

Again, in order to utilise h(· ) in the remaining equations it needs to be linearised. This time the linearisation takes place around the a priori state estimate ˆxk|k−1 as stated by

Hk= ∂h ∂x xˆ k|k−1 (5.2.22)

where Hk is the the Jacobian matrix of h(· ) with respect to the state x. Finally the correction

phase is completed by Kk = Pk|k−1H T k(HkPk|k−1H T k + Rk)−1 (5.2.23) ˆ xk|k= ˆxk|k−1+ Kk˜yk (5.2.24) Pk|k= (I − KkHk)Pk|k−1 (5.2.25)

(44)

Chapter 5: Sensor Fusion

Unscented Kalman Filter

The unscented Kalman filter (UKF) is applicable to nonlinear system without the need of analytically linearising the system and measurement models as in the case of the EKF. Instead of calculating the Jacobian matrices the UKF utilises a deterministic weighted sampling method known as the unscented transform.

The sample points selected, denoted as sigma points, are chosen in such way that a fixed and minimal number of points propagated through the nonlinear transform, gives an estimate of the transformed mean and covariance.

The advantage of the UKF is its ability to circumvent the inaccuracy found and problems associ-ated with the first-order linearisation used by the EKF. In Julier et. al. [12] the following aspects are described. Firstly, the linearisation is only reliable if the error propagation is approximately linear, otherwise the EKF will perform inadequately and may even in the worst-case diverge. Fur-thermore, the linearisation is only possible if the Jacobian matrices exist, which is not the case of system with discontinuities. For an instance, discontinuities may arise due to highly quantised sensor measurements. Lastly, the actual calculation of the Jacobian matrices could be prone to errors.

5.2.5

Implementation Methods

The Kalman filter could be utilised in many ways to fuse sensor measurement signals. In short, there are two main implementation methods; the direct method and the indirect method [13].

Direct Pre-filtering

The direct pre-filtering method applies a KF to each individual signal. Thereafter an error is formed by subtracting the estimates, and lastly one of the estimates are compensated with the error.

Signal A Kalman Filter xˆ

Signal B Kalman Filter

+ +

− −

(45)

Chapter 5: Sensor Fusion

Direct Filtering

In the direct filtering method all measured signals are directly feed into a KF as illustrated in figure 5.2. The KF utilises a full model of the system and is thereby able to estimate the system state by the provided measurement updates. A major advantage of the direct method is the ability of the KF to still conduct corrections with only one signal available, yielding a robust implementation.

Signal A

Kalman Filter xˆ Signal B

Figure 5.2: Direct filtering implementation scheme.

Indirect Feedforward

The indirect feedforward method compares the signals by subtraction prior the KF. In this method the KF utilises a sensor error model instead of a system model, in other words the KF estimates the measurement error instead of the system state. The estimated error is feedforward and removed from one of the sensor measurements, resulting in a corrected state estimate. If the system is complex in terms of modelling an indirect method could be advantageous since the sensors and their errors could be simpler to model. The implementation scheme of the indirect feedforward method is shown in the following figure 5.3.

Signal A xˆ Kalman Filter Signal B + + − −

(46)

Chapter 5: Sensor Fusion

Indirect Feedback

The indirect feedback method is very similar to the previous method. The difference lies within how the error estimate is utilised. In the indirect feedback method the error is actually used to correct one of the signals. As a consequence the error estimate is bounded to a finite limit, which is not the case of the indirect feedback method.

Signal A xˆ Kalman Filter Signal B + − −

(47)

Chapter 6

Localisation

In this chapter the reader will be presented a selection of the sensor fusion solutions implemented and applied to the problem of localisation. The solutions is based on the subjects introduced and discussed in the previous chapters. In appendix C an overview is given of the two courses used in this chapter for evaluating the found solutions.

6.1

Full Model Kalman Filter

The following Kalman filter (KF) will be based on the vehicle’s kinematic equation (4.3.7) previously derived in chapter 4, hence the filter’s state will be driven forward by the odometric sensors in the prediction phase. In the correction phase the filter will be corrected by GPS measurements.

6.1.1

Model

The filter is implemented in a straightforward manner using the direct filtering implementation scheme. The scheme was selected due to its simplicity and robust features. The filter’s state vector xk is constituted by the following sate variables

xk= [ xkykθk vk] T

. (6.1.1)

The model is given by

      xk yk θk vk       =       xk−1 yk−1 θk−1 vk−1       +         ωkR cos(θk)T ωkR sin(θk)T ωkR sin(ϕk) L + L cos(ϕk) T − ˙ϕk L L + L cos(ϕk) T ωkR         (6.1.2)

(48)

Chapter 6: Localisation

where L denotes the distance between a wheel axle and the articulation hinge, and R the average wheel radius measuring approximately 0.8 meters. More concisely the model is written as

xk = f (xk−1, uk; γk) + wk−1. (6.1.3)

where the input vector uk is constituted by the wheel angular velocity ωk, the articulation angle ϕk

and the articulation angle rate ˙ϕk

uk = [ ωk ϕkϕ˙k] T

. (6.1.4)

Differentiating the model with respect to xk gives the following system Jacobian matrix Gk

Fk = ∂f ∂x ˆx k−1|k−1 =       1 0 −ωkR sin(θk)T 0 0 1 ωkR cos(θk)T 0 0 0 1 0 0 0 0 1       (6.1.5) (6.1.6)

The input Jacobian matrix Gk is obtained by differentiating the model with respect to uk in

accor-dance with Gk = ∂f ∂u ˆ xk−1|k−1 =       g11 0 0 g21 0 0 g31 g32 g33 0 0 0       (6.1.7) g11= R cos(θk)T g21= R sin(θk)T g31= R sin(ϕk) L + L cos(ϕk) T g32= (ωkR cos(ϕk) L + L cos(ϕk) − ˙ϕk L2sin(ϕ k) (L + L cos(ϕk))2 + ωkR L sin(ϕk)2 (L + L cos(ϕk))2 )T g33= − L L + L cos(ϕk) T.

(49)

Chapter 6: Localisation

The filter’s observation function h(· ) is linear and thus the observation Jacobian matrix Hkis given

by Hk = ∂h ∂x ˆx k|k−1 =       1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1       (6.1.8)

and the measurement vector zk by

zk= h ˜ Ek N˜kθ˜k ˜vk iT (6.1.9)

where the variables ˜Ek and ˜Nk are measured by the GPS and given as a position in the local

tangental plane. The orientation ˜θk is calculated from the GPS velocity components in accordance

with equation (3.4.4), and ˜vk as

˜ vk= ( p˜v2 E+ ˜v 2 N if ωk ≥ 0 − p˜v2 E+ ˜vN2 if ωk < 0 (6.1.10)

since the GPS receiver is not able to determine if the vehicle is driving forward or reversing.

6.1.2

Covariance Matrices

System and Input Covariance

The system covariance matrix Qk was initially given values by intuition, and tuned until achieving

a desired filter behaviour.

Qk=       0.22 0 0 0 0 0.22 0 0 0 0 0.12 0 0 0 0 0.12       (6.1.11)

Regarding the tuning of the input covariance matrix Γk, the initial values were selected in accordance

with the sensors’ noise as discussed in chapter 3.

Γk=     0.082 0 0 0 0.0012 0 0 0 0.12     (6.1.12)

(50)

Chapter 6: Localisation

Measurement Covariance

The initial values of the measurement covariance matrix Rk where based on the performance values

specified for the GPS unit. However, it was found that the GPS measurements did drive the filter too hard and for that reason the initial values where increased until a satisfactory filter performance was obtained. Rk =       7.52 0 0 0 0 7.52 0 0 0 0 0.92 0 0 0 0 0.052       (6.1.13)

(51)

Chapter 6: Localisation

6.1.3

Evaluation and Performance

Position Estimation at POND

In figure 6.1 the estimated path is illustrated together with the path measured by the GPS; the contribution of performing sensor fusion is quite evident when comparing the two paths. At the last lap a GPS intermission was enforced making it possible to see the consequences of the overestimation done during corners.

−60 −40 −20 0 20 40 60 80 −40 −20 0 20 40 60 80 100 East [m] North [m] GPS EKF EKF (intermission)

Figure 6.1: Illustration of the path estimated by the full model filter, compared to the path measured by the GPS.

The final position and orientation were compared to their equivalents in a case where no GPS inter-mission was enforced, yielding a relative error between the two. The relative error was determined to 22.3 meters in position and 36.4 degrees in orientation.

Figure

Figure 1.1: A wheel loader of the 120F model used as platform in the Autonomous Machine project.
Figure 2.1: Illustration of the local tangental plane and its Cartesian coordinate system.
Figure 4.1: Illustration of the lateral slip angle and the related velocity vectors.
Figure 4.2: Schematic illustration of an articulated vehicle.
+7

References

Related documents

So a composite measurement gives the positions of any features tracked, the estimated pose change for the robot and the absolute (z,θ,φ,ψ) of the robot at the end of the interval..

Therefore, this thesis will identify the possibility to determine the direction to a transmitter with randomly placed sensors by using the Direction Finding method MUltiple

He has been employed by Saab since 1994 as system engineer working mainly with modelling and simulation of airborne platforms and synthetic natural environments. His

This thesis investigates the extraction of semantic information for mobile robots in outdoor environments and the use of semantic information to link ground-level occupancy maps

treatment, using droplet digital PCR, did however not reveal any significant change in mtDNA copy number compared to untreated cells (Fig. Increasing concentrations of CpG-C

This thesis aims to solve this problem using SLAM and object detection with 2D LiDAR and camera as sensor input, looking at the performance in terms of accuracy

Specially constructed extremum seeking schemes for single and multi-agent systems are presented, where the agents have only access to the current value of their individual

With aid of the findings in the theory and state of the art part, methods, hardware (sensors) and data fusion algorithms have been chosen to implement on ˚ AFs robot platform called