A Localisation and Navigation System for anAutonomous Wheel Loader

List of Figures1.1 A wheel loader of the 120F model used as platform in the Autonomous Machine project.. Parameters L1,2 Distance between the articulation hinge and a wheel axle [m] P1,2

A Localisation and Navigation System for an Autonomous Wheel Loader

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)

Comments: This Master’s Thesis report is submitted as partial fulfilment of

the requirements for the degree of Master of Science in RoboticEngineering 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

Rights: c

Dedicated to my mother for her never-ending encouragement and support.

Autonomous vehicles are an emerging trend in robotics, seen in a vast range of applications andenvironments Consequently, Volvo Construction Equipment endeavour to apply the concept ofautonomous vehicles onto one of their main products In the company’s Autonomous Machineproject an autonomous wheel loader is being developed As an objective given by the company; ademonstration 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 navigateaccordingly 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; odometricsensors, a Global Positioning System receiver and an Inertial Measurement Unit

Navigational control is provided through an interface developed, allowing high level software tocommand the vehicle by specifying drive paths A path following controller is implemented andevaluated

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

Keywords: Autonomous Vehicle, Sensor Fusion, Kalman Filtering, Path Following

First of all I would like to thank my nearest colleagues Niclas Evestedt and Jonathan Blom forall the hilarious discussions and moments we had together for the past months, making the longhours spent in the wheel loader endurable Their technical and analytical feedback is appreciated aswell A special gratitude is directed to my friend and colleague Magnus Saaw for proof-reading thisthesis Special thanks to my supervisor at Mälardalen University, Dr Giacomo Spampinato, for hiscompetent and insightful discussion on Kalman filters Dr Martin Magnusson at Örebro Universitydeserves a special recognition for his work and assistance on the vision system A thank to StaffanBacken 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 inspirationand enthusiasm shown for the Autonomous Machine project

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

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

6.2.1 Model 38

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

C.1 Evaluation Course POND 70

C.2 Evaluation Course WOODS 71

F.0.1 Calibrated Outputs of Inertial Measurement Unit 77

F.0.2 Global Positioning System Receiver 78

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

6.6 Estimation of the average wheel radius 43

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

List of Tables

F.1 Accelerometer specification 77

F.2 Rate gyroscope specification 77

F.3 Magnetometer specification 77

F.4 GPS receiver specification 78

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

Parameters

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]

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]

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 NetworkANN 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

UDP User Datagram Protocol

TCP Transmission Control Protocol

SLAM Synchronous Localisation and Mapping

MATLAB MATrix LABoratory

IEEE Institute of Electrical and Electronics EngineersMDU Mälardalen University

PIP Packaged Industrial Personal Computer

GUI Graphical User Interface

CAN Controller Area Network

LSB Least SigniÞcant Bit

Chapter 1

Introduction

The Autonomous Machine project is a initiative taken by Volvo Construction Equipment (VolvoCE) 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 thework of Master’s Theses

Volvo CE is one of the leading companies on the construction equipment and heavy machinerymarket The company’s history begins back in 1950 when AB Volvo bought an agricultural machinemanufacturer renamed to Volvo BM AB The company expanded globally during the 1980s and1990s 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

A wheel loader is a versatile vehicle able to perform a wide variety of work tasks in many differentenvironments In its typical configuration a wheel loader is equipped with an actuator arm with twodegrees 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

High mobility and flexibility is obtained by utilising articulated steering; a type of steering where a

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 makesthe vehicle stable and able to handle heavy loads The rear axle is often freely suspended in a pivotfor increased mobility in rough terrain

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

The intention of this thesis is to enable the autonomous wheel loader used in the AutonomousMachine project to navigate in a given task space In short, the problem at hand could be dividedinto two distinct parts Firstly, the system needs to be capable of localising itself in its task spaceusing available sensors mounted on the vehicle The second problem is to autonomously steer thewheel 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 articulatedvehicle such as a wheel loader behaves and responds in terms of steering i.e a model needs to bederived accordingly The localisation of the vehicle will be based on the readings of the vehicle’sinternal 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 theinformation provided by the sensor readings Finally, steering the vehicle requires a stable andaccurate control law, but also a way to represent and communicate the intended drive path

Chapter 1: Introduction

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

au-Material rehandling is the simplest and most repetitive task found at such site; gravel or similarmaterials is stocked at the site and where a wheel loader is utilised for the purpose of transporting thematerial from the stock into the production In the case of the asphalt plant the different materialsare 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 humandriver 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 orserve 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

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 Awireless industrial classified safety stop has been installed in the vehicle allowing the responsibleoperator to shut it down, at shutdown the parking brake is automatically activated bringing thevehicle to a halt

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

The vehicle’s task space is assumed to be moderately planar, as a consequence only planar motionwill 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 thisthesis

In chapter 5 the reader is introduced to the concept of multisensor systems and sensor fusion withKalman 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 Thechapter ends with an overview of different implementation methods and schemes

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

The design and work of integrating the developed solution into the existing system architecture isdescribed 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 thisthesis

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 ofthe 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 computersbecoming smaller, more powerful and affordable enhances the possibility to develop autonomoussystems without a military founding Regarding sensors, the technology of microelectromechanicalsystems (MEMS) revolutionised sensors, both regarding their price and size Today rather advancedsensors such as accelerometers and gyros could be found in cars, mobile telephones and video gamingdevices

Chapter 1: Introduction

The term autonomous is often interchanged with and equalled to the term unmanned The latterterm refers only to a vessel that carries no human operator, hence it by definition could be remotelyoperated by a human In contrary, an autonomous vehicle operates without a human operator’sintervention

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

The probably most recognised AGVs today are the participators of the DARPA Grand Challengeheld 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 timelimit of 10 hours The winning team from Stanford University completed the course in just under 7hours [1] The course was completed by 5 of the 23 participating teams, in the first challenge non ofthe 15 vehicles managed to complete the course The Urban Challenge featured a 96 kilometre longurban course to be completed within 6 hours, won by the Carnegie Mellon University completingthe course in a little over 4 hours

The participating vehicles are standard cars retrofitted with computers and sensors Typical sensorsare Global Positioning System (GPS) receivers, laser rangers (LIDAR), cameras, microwave radarand inertial measurement units (IMU) The large amount of data produced by the vehicles’ sensorsystems 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 ForestMachines is held as a part of the research conducted at the Umeå University [2] The project’spurpose 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 forautonomously drive along previously learnt paths The vehicle is able to localise itself by using GPS

in conjunction with laser ranging and odometry

Chapter 1: Introduction

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 miningindustry 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 economicalbenefits 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 turnsloading their buckets The communication is managed over a common wireless local area networkwith added realtime features

There are several publications related to the autonomous loaders used in underground mines, cially regarding the modelling of such vehicles Due to their similarity to a wheel loader; a more

espe-in depth related work study is given espe-in chapter 4 treatespe-ing the modellespe-ing of the autonomous wheelloader

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

A ruggedised industrial computer, a PIP8 manufactured by MPL AG, featuring a realtime softwareenvironment is used for interfacing the vehicle’s different functions and sensors The sensors areinterfaced 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 Themodels are developed in the standard Simulink environment and then compiled through C to asingle file that is downloaded to the PIP8 computer

The hydraulic functions powering the actuator and the articulated steering are controlled by thePIP8’s digital-to-analogue converters (DAC) via an electrical servo system The throttle and thebrake system is controlled in a similar manner Utility functions with on-off characteristics arecontrolled 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 stagethe CAN bus is mostly used for supervision purposes

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 mainresponsibility of the high level software is to coordinate and command the vehicle accordingly to fulfilthe current objective, and provide an graphical user interface (GUI) This computer is connected tothe 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 SICKMLS291 laser ranger (LIDAR), mounted with a servo on the cab’s roof edge, was added to thehardware architecture during the timeline of this thesis The vision algorithms are able to detectpiles and other objects that the vehicle autonomously needs to interact with The algorithms aredeveloped 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

Chapter 2

Coordinate Systems

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 GeodeticSystem 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, renderingthe 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

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

The vehicle body fixed frame is a coordinate system having its origin in the middle point of thevehicle’s rear wheel axle The reason why the rear body of the wheel loader was selected to hostthe vehicle body frame was simply due to the fact that all vital sensors are located there Thex-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 isspecified to point skywards, which completes the right hand system

ˆx

Figure 2.2: The vehicle body fixed coordinate system

Chapter 3

Sensors

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

It is crucial to understand what a particular sensor actually measures in order to utilise the obtaineddata 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 measurementmodel (3.1.1) of the arbitrary physical quantity x, errors such as scalar error kx, bias error bx, normaldistributed white noise ηxand quantisation error ∆xare taken into account In reality other errorsexist, but they will be assumed to be rather small in comparison to the errors already accounted for

in the model and are therefore neglected

Chapter 3: Sensors

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

Chapter 3: Sensors

The Xsens MTi-G inertial measurement unit (IMU) is capable of outputting raw, calibrated andprocessed 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 PositioningSystem (GPS) receiver In the calibrated output mode the raw readings from the 16-bit ADC areconverted 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 orother temporal processing has been applied to the calibrated measurements

The calibrated output contains the measurements of acceleration, angular velocity and magnetic fieldstrength2, in six degrees of freedom Appendix F provides the sensor specifications An accelerationmeasurement is given by

˜

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 therewill always be an apparent angular velocity in their measurements For that reason the gyroscopemodel will contain a bias error denoted as bψ The noise ηψ needs to be considered as well, thus theresulting model is

of the heading, making the GPS the only available source of heading corrections As a consequence

1 Misalignment relative the sensor housing.

2 Normalised to the Earth’s magnetic field strength.

Chapter 3: Sensors

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

The Global Positioning System (GPS) is a Global Navigation Satellite System (GNSS) used in avast range of both military and civilian applications where localisation and time synchronisation isessential In short, the GPS consist of a set of satellites continuously transmitting a radio signalcontaining 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 timesand interpret the times as imaginary spheres originating from the corresponding satellites Theintersections of those spheres define the possible positions of the receiver’s antenna Figure 3.1illustrates the principle of GPS positioning, where a, b, c denotes three visible satellites and r theposition of the receiver antenna

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 positionmeasurement

" ˜E

˜N

#

=

"

EN

#

where ˜E and ˜N are the measured east and north positions on the local tangent plane frame, 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 Thestandard deviation is calculated by

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 ofprecision (HDOP) and a vertical dilution of precision (VDOP) Regarding the positioning in theplane, the HDOP measurements can be used for detecting invalid GPS positions

Chapter 3: Sensors

The fundamental concept behind a Differential Global Positioning System (DGPS) is the fact thatreceivers 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 clockerrors affecting the positioning precision Therefore it would be beneficial if one receiver could beutilised to determine the error in its region, an error that is forwarded to other user receivers inthe 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 Eventhough the atmospherical interference is approximately the same, it will vary As a consequence thecorrection accuracy will degrade with the user’s distance from the reference transceiver Anotherproblem, affecting the accuracy negatively, arises if a user does not see the same set of satellites used

by the reference

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 loaderhas many attributes to model this report will focus on modelling the traversing motion of a wheelloader in its task space In other words the modelling of the wheel loader’s actuator will not becovered

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

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

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

Chapter 4: Vehicle Modelling

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

The more successful system involved a model explicitly accounting for slip, where the slip was mated with inertial sensors and an Extended Kalman Filter (EKF) since it is not directly observable

esti-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

L(cos(β) + cos(β + ϕ)

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

In the approach purposed by Dragt et al [7] Lagrangian dynamics is used for the purpose ofmodelling a LHD vehicle The vehicle model includes tyre models in an attempt to account forbasic tyre dynamics such as cornering slip However, their work was only conducted to the extent oflimited simulations i.e no comparisons were made with the respect to a real vehicle One difficultyexperienced 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 sincethe vehicle in question operates in slow speeds The derived non-slip model accounts for vehiclespecific features such as lengths between wheel axles and the articulation hinge Compared to theprevious presented non-slip model (4.1.1) it also takes the articulation angular rate in account whencomputing 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 twoadditional slip angles found in the latter model The model derived by Altafini [8] is given by

1 The model assumes that the vehicle’s axles are located on the same distance to the articulation joint.

Chapter 4: Vehicle Modelling

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 kinematicmodel, 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 biggererror in orientation than the full kinematic model (4.1.3), especially at lower speeds The trials werecarried out on a full size LHD using a set of IMUs as references

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

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 betweenslip and the utilised friction coefficient µ = Fx/Fz, where Fxis the friction force and Fz the wheelnormal force in a wheel hub centred coordinate system, is linearly given by µ = ks On solidsurfaces the wheel normal force Fz could be approximated by the normal force N However, inoff-road conditions the ground is deformed by the wheel, and as a consequence the wheel normalforce Fz is shifted towards the direction of motion

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 anlateral acceleration directed towards the instantaneous centre of rotation, thus a side force Fy willarise 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’srotational 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 wheelspecific parameter known as the cornering stiffness C That is, Fy = Cα

Chapter 4: Vehicle Modelling

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-slipmodels, 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 amuch lower complexity since modelling the slip is a non-trivial problem [6] However, if it is laterconcluded that lateral slip must be accounted for, it will be rather straightforward to adapt themodel due to its similarity with the slip model (4.1.2)

ˆxˆ

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

Chapter 4: Vehicle Modelling

where i = {1, 2} and vi is the velocity vector associated to that midpoint during motion Theassumption of no lateral slip during motion implies that there are no velocity components perpen-dicular to the two body halves, which is stated by

˙y

˙θ

˙ϕ

˙y

˙θ

˙ϕ

Chapter 4: Vehicle Modelling

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

The concept of a soft sensor was applied to the model Even though the implementation in figure 4.3seems rather straight forward, there are some issues to account for It is not feasible to differentiatethe articulation angle ϕ due to noise For that particular reason a second order Infinite ImpulseResponse (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-offfrequency will introduce a noticeable phase lag degrading the soft sensor’s performance A cut-offfrequency of 1 Hz and a damping of √1

2 where found to be appropriate

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 trackdenoted as POND The former figure, comparing the two angular velocities, clearly indicates atendency of the calculated angular velocity to be overestimated Still it must be concluded that themodel reflects the behaviour of the vehicle

−30

−20

−10 0 10 20 30

Time [s]

Gyro Soft Sensor

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

0 1 2 3 4 5 6 7

Time [s]

Figure 4.5: The absolute error between the gyro measurement and the calculated angular velocity

Chapter 5

Sensor Fusion

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

Sensor fusion is the technique of combining measurements from multiple sensors in a system suchthat 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 ofthe system is measured; the robustness of the system is possible to increase by statistical methods,complementary measurements and sensor redundancy [10]

A Kalman filter estimates the state of a noisy linear dynamic system by noisy measurements thatcould be linear related to the system’s state If the corrupting noise is independent, white andnormal distributed with a zero mean; the KF will be a statistically optimal estimator with respect

to any reasonable quadratic function of estimation error

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 lateeighteenth century (1795) at an age of eighteen Gauss recognised that the least square method

Chapter 5: Sensor Fusion

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

Taking a 140-year leap in history until the early years of the Second World War, Norbert Wienerwas 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 squareerror

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

The Kalman filter is said to be one of the biggest discoveries in the history of statistical estimationtheory Without it many achievements after its discovery would not have been possible It hasbecome a vital component in a wide variety of applications spanning from estimating the trajectory

of a spacecraft, to predicting the finance market

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 inputvector uk to xk The process noise wk is assumed to be normal distributed white noise with zeromean and a covariance of Qk, more formally stated as

wk∼ N (0, Qk) (5.2.2)

Likewise the input noise γk is given by

γk∼ N (0, Γk) (5.2.3)

Chapter 5: Sensor Fusion

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

zk = Hkxk+ vk (5.2.4)

where Hk is the observation matrix mapping the state space into the observation space Theobservation is assumed to be perturbed by normal distributed white noise vk with zero mean and acovariance of Rk, known as the observation noise

vk ∼ N (0, Rk) (5.2.5)

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

of the true system state

KF utilises this knowledge to, with the estimate error covariance matrix, reflect the uncertainty ofthe a priori state estimate