Institutionen för systemteknikliu.diva-portal.org/smash/get/diva2:354085/FULLTEXT01.pdf · 2010....

Institutionen för systemteknikDepartment of Electrical Engineering

Examensarbete

Increased Autonomy for Construction Equipmentusing Laser

Examensarbete utfört i Reglerteknikvid Tekniska högskolan i Linköping

av

Tobias Andersson

LiTH-ISY-EX--10/4349--SE

Linköping 2010

Department of Electrical Engineering Linköpings tekniska högskolaLinköpings universitet Linköpings universitetSE-581 83 Linköping, Sweden 581 83 Linköping

Increased Autonomy for Construction Equipmentusing Laser

Examensarbete utfört i Reglerteknikvid Tekniska högskolan i Linköping

av

Tobias Andersson

LiTH-ISY-EX--10/4349--SE

Handledare: Karl Granströmisy, Linköpings universitet

Torbjörn MartinssonVolvo CE

Examinator: Thomas Schönisy, Linköpings universitet

Linköping, 30 June, 2010

Avdelning, InstitutionDivision, Department

Division of Automatic ControlDepartment of Electrical EngineeringLinköpings universitetSE-581 83 Linköping, Sweden

DatumDate

2010-06-30

SpråkLanguage

Svenska/Swedish Engelska/English

RapporttypReport category

Licentiatavhandling Examensarbete C-uppsats D-uppsats Övrig rapport

URL för elektronisk versionhttp://www.control.isy.liu.se

http://www.ep.liu.se

ISBN—

ISRNLiTH-ISY-EX--10/4349--SE

Serietitel och serienummerTitle of series, numbering

ISSN—

TitelTitle

Ökad automation för bygg- och anläggningsfordon med hjälp av laser scannerIncreased Autonomy for Construction Equipment using Laser

FörfattareAuthor

Tobias Andersson

SammanfattningAbstract

At working sites all around monotonic tasks are performed. If one were ableto automatize these kinds of tasks there would be a large economical profit tocollect. Volvo CE are in the process of developing an autonomous wheel loader,to perform these types of monotonic, uniform tasks. The project is intended tobe performed mainly be thesis workers. This report is the eighth thesis in thisproject. Earlier work has made the loader able to see a pile using a laser scanner.The machine can also see and fill a hauler. The usage of the laser scanner canonly be made while the loader is standing still. The aim of this thesis work hasbeen to make the loader able to scan its environment while it is moving. To dothis an inertial measurement unit has been used for keeping track of the scannersorientation during a scan. The work of this thesis has resulted in a working set-upon the machine, and a robust framework for future work.

NyckelordKeywords IMU, EKF, vision

AbstractAt working sites all around monotonic tasks are performed. If one were ableto automatize these kinds of tasks there would be a large economical profit tocollect. Volvo CE are in the process of developing an autonomous wheel loader,to perform these types of monotonic, uniform tasks. The project is intended tobe performed mainly be thesis workers. This report is the eighth thesis in thisproject. Earlier work has made the loader able to see a pile using a laser scanner.The machine can also see and fill a hauler. The usage of the laser scanner canonly be made while the loader is standing still. The aim of this thesis work hasbeen to make the loader able to scan its environment while it is moving. To dothis an inertial measurement unit has been used for keeping track of the scannersorientation during a scan. The work of this thesis has resulted in a working set-upon the machine, and a robust framework for future work.

SammanfattningPå arbetsplatser runt om utförs dagligen monotona uppgifter. Om dessa kundeautomatiseras finns en ekonomisk vinst att hämta. Volvo CE är mitt uppe i ut-vecklingsprocessen av en autonom hjullastare, tänkt att utförda dessa monotona,likformiga uppgifter. Tanken är att detta projekt huvudsakligen ska färdigställasmed hjälp av examensarbetare. Detta examensarbete är det åttonde i ordningen.Tidigare insatser har möjliggjort för lastaren att se grushög, med hjälp av en la-serscanner. Maskinen klarar även av att se och fylla en dumper. Hittills så klararmaskinen dock detta bara i stillastående läge. Mitt arbete har gått ut på att fåden att även klara av detta under körning. För detta har en IMU (tröghetsna-vigeringssensor) använts. IMUs uppgift har varit att hålla koll på laserscannernsorientering under en körning. Denna rapport har resulterat i en fungerande setuppå maskinen, men även ett robust ramverk för framtida exmanensarbetare attbygga vidare på.

v

Acknowledgments

I would like to thank my supervisor at Volvo CE, Torbjörn Martinsson, for allof his guidance and help along the way. My co-thesis-worker Jonatan Blom havealways supported me with all kinds of knowledge. Also a great thanks to Dr.Thomas Schön, my university examiner, for guidance and help regarding IMUs.Ph.D. student Karl Granström, my university supervisor, has provided me withvery much help, especially regarding the Kalman filtering. I would like to thankDr. David Törnqvist for his help regarding the quaternion dynamic. FurtherI would like to direct a great thank to Post-doc Martin Magnusson and Ph.D.student Håkan Almqvist from Örebro University.

Finally, a big thanks to my lovely wife Tove Solbeckar, for her great supportand belief.

vii

Contents

1 Introduction 31.1 Background . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31.2 Problem Formulation . . . . . . . . . . . . . . . . . . . . . . . . . . 3

1.2.1 Purpose . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41.3 Volvo CE . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

1.3.1 Products . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41.3.2 History . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

1.4 Outline . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

2 The Machine 72.1 Earlier Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72.2 Software Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . 82.3 Coordinate frames . . . . . . . . . . . . . . . . . . . . . . . . . . . 92.4 Laser Scanner . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 92.5 IMU . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 122.6 Translation Counter . . . . . . . . . . . . . . . . . . . . . . . . . . 18

2.6.1 Test . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 192.7 Waist Angle . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 212.8 Rigid Body Transform . . . . . . . . . . . . . . . . . . . . . . . . . 21

2.8.1 Translation . . . . . . . . . . . . . . . . . . . . . . . . . . . 222.8.2 Rotation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 222.8.3 Complete Transform . . . . . . . . . . . . . . . . . . . . . . 222.8.4 Real-Time Control . . . . . . . . . . . . . . . . . . . . . . . 23

3 Modelling and Filtering 253.1 Noise Examination . . . . . . . . . . . . . . . . . . . . . . . . . . . 26

3.1.1 Measurement Noise . . . . . . . . . . . . . . . . . . . . . . . 263.1.2 Process Noise . . . . . . . . . . . . . . . . . . . . . . . . . . 28

3.2 Dynamic Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 293.2.1 Attitude Dynamics . . . . . . . . . . . . . . . . . . . . . . . 293.2.2 Measurement Model . . . . . . . . . . . . . . . . . . . . . . 30

3.3 Extended Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . 31

ix

x Contents

4 Experimental Results 354.1 Results from the Machine . . . . . . . . . . . . . . . . . . . . . . . 35

4.1.1 Speed Bump . . . . . . . . . . . . . . . . . . . . . . . . . . 354.1.2 Gravel Surface . . . . . . . . . . . . . . . . . . . . . . . . . 374.1.3 Asphalt Mill . . . . . . . . . . . . . . . . . . . . . . . . . . 40

4.2 Simulation Results . . . . . . . . . . . . . . . . . . . . . . . . . . . 454.2.1 Connection to the Machine . . . . . . . . . . . . . . . . . . 54

5 Concluding Remarks 555.1 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 555.2 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55

Bibliography 57

A Quaternions 59A.1 Algebra . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59A.2 Rotation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60A.3 Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60

B Complete Dynamic Model 62B.1 Translation Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . 62B.2 Bias . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62B.3 Total Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63

Nomenclature

Table 1. Nomenclature.

IMU Inertial Measurement UnitEKF Extended Kalman FilterΘ = [φ, θ, ψ]T Euler angles (Roll,Pitch,Yaw).Roll Rotation around the x-axisPitch Rotation around the y-axisYaw Rotation around the z-axisb = [bφ, bθ, bψ] Bias-terms in Roll, Pitch and YawΩ = [ωx, ωy, ωz] Angular velocities along the x-,y- and z-axis.absAcc Absolute acceleration recorded by the IMUTVBE Time Varying Bias EstimationTVZBE Time Varying Zero Bias EstimationWL Wheel LoaderVolvo CE Volvo Construction Equipmentα Tilt angle for scannerβ Bearing angle for scannerq = (q0 q1 q2 q3)T The unit quaternion

1

Chapter 1

Introduction

Volvo CE are in the middle of a project that will result in a fully autonomoussolotion for a wheel loader and a hauler. The part of the project presented hereis to solve the problems that occur when you are using a laser scanner duringmovement. The work has been divided into two separate solutions; one moretheoretical involving modelling and filtering of the system. The other solution ismore pragmatic, where the goal has been to implement a working set-up on thewheel loader.

1.1 BackgroundThe problem of scanning while moving is well defined and a sufficient amount ofwork has been made on it. Still, no one-for-all solution has been reached. Differentmethods has been tested, for instance scan-matching [1], but never resulting in aworking solution for velocities over 1-2 m s−1. For the most applications, thisresult is much to slow. As a reference it can be noted that a wheel loader drivercan manoeuvre the machine at a average speed of about 5-6 m s−1.

The use of a proper, working solution for the scanning while moving problemcan be seen when looking at the more general autonomous problem. An au-tonomous machine has to get a reliable knowledge of its environment to produceany successful results and with the efficient parameter in mind it is realised thatthe machine cannot slow down or stop every time it has to update its picture ofthe environment.

1.2 Problem FormulationAt the start of this thesis work the machine worked well in stationary mode. Theproblem with the dynamic mode is the tilting part. Consider the case when thewheel loader is driving against a wall at 10 km/h (≈ 2.8 m s−1). When the tilting isstarted the scanner will detect the wall at distance x0. During the tilting process,which takes approximately two seconds, the machine has moved over five metres,

3

4 Introduction

which will lead the scanner to detect the wall at distance x = x0−∆x, where ∆xis the distance covered by the machine during the tilting. The difference will resultin a leaning wall, even though it is the scanner that has been moving and not thewall. This deformation of the point cloud is called a distortion. The distortion willbe effected by the translation of the scanner (as in the example above), but also bythe rotation of the scanner. Obviously, the distortion will create big problems andtherefore it has to be compensated for. One easy but expensive way of doing thiswould be to buy better equipment [2]. But with one on the long time goals beingindustrialisation that solution is not an option. Therefore an Inertial MeasurementUnit (IMU) has been mounted on the scanner to measure the scanner’s movement.If the scanner’s movement (translation and rotation) at every time during a scansweep is known, it can be compensated for.

1.2.1 PurposeThe purpose of this work is to solve the scanning while moving problem for theautonomous wheel loader.

1.3 Volvo CEThis section contains a brief summary about the company where the master thesiswere performed. All the information below can be found at Volvo CE’s web site,http://www.volvo.com/constructionequipment/corporate/en-gb/home.htm.

Volvo Construction Equipment is a part of the worldwide Volvo Group. TheVolvo Group is one of the world leaders of transport solutions, including trucks,buses and construction equipment. The retailer for Volvo CE in Sweden is SweconAnläggningsmaskiner.

1.3.1 ProductsVolvo CE are one of the world’s leading manufacturers of articulated haulers andwheel loaders, but they are manufacturing a significant amount of other construc-tion equipment machines, such as excavators, motor graders, skid steer loadersetc.

1.3.2 HistoryAn extensive history overview can be found at http://www.volvo.com/constructionequipment/corporate/en-gb/AboutUs/history/introduction.htm

1.4 OutlineIn Chapter 2 the machine’s sensors are presented together with a brief review ofthe earlier work made on the machine. This is followed by the modelling and

1.4 Outline 5

filtering part of the system in Chapter 3. The experimental results are presentedin Chapter 4. Finally some concluding remarks are given in Chapter 5.

Chapter 2

The Machine

Previous thesis workers have constructed a wheel loader which can operate, withsome limitations, in autonomous mode. To make that possible a sufficient amountof hardware is needed. In the sections below some of this hardware is describedtogether with a presentation of the earlier work made on the wheel loader. Also,a brief introduction to the software is presented.

2.1 Earlier Work

This thesis work is the eighth in order in the Volvo CE project. Before this workstarted earlier thesis workers had built a working hardware platform, and been ableto control the machine through a computer [3]. The next step was to remove anykind of remote controlling and implement a control system to make the machinecompletely autonomous [4]. Later on the machine was made able to automaticallyfill one bucket of gravel [5] and a basic vision system was installed [6]. This visionsystem is the base for this thesis work, and is briefly presented in Section 2.4. Thelatest improvements on the machine have been to equip it with an algorithm whichextracts the surface normal of the pile. The surface normal is used to calculatethe optimal way of approaching the pile. A hauler-finder-algorithm [7] has alsobeen added to the machine’s features.

7

8 The Machine

Figure 2.1. Volvo L120F, to the right. Picture taken from the Volvo Group pressarchive.

The autonomous machine is of type L120F, and can be seen in Figure 2.1.

2.2 Software OverviewThe brain of the system is a laptop-computer which gathers information from theexternal systems. All calculations are done in this computer. To communicate withthe machinery a Packaged Industrial PC (PIP8) running the real-time operatingsystem xPC Target from MathWorks is used. xPC Target allows the user to createa control program on a host computer, which is then downloaded and compiledonto a target computer [8]. In this case the laptop downloads a simulink-modelto the PIP8, which then can read machine signals (such as breaking pressure,tilting angle, lift acceleration etc.) and communicate these to the host computerfor calculations. An overview of the data topology is shown in Figure 2.2.

Figure 2.2. Data topology for the autonomous machine.

Figure 2.2 shows that there also exists a camera among the hardware. This

2.3 Coordinate frames 9

has not yet been implemented but can be used as a support to the laser scannerin future work.

2.3 Coordinate frames

As mentioned in earlier sections the wheel loader has its own coordinate frame.This has been called the World frame, which may seem a bit confusing when itobviously is not stationary. However, the loaders coordinate frame is as close toa global frame the current application will ever need. This is because the onlyinformation that is interesting is how the scanner has been moving during everyscan sweep. The following coordinate frames are used:

• World (w) - A coordinate frame with its origin in the centre of the scanner.See Figure 2.5 for definition.

• Body (b) - A body fixed coordinate frame, with its origin in the center ofthe IMU. The orientation of the frame is in the same way as for the Worldframe. Notice that the center of the IMU is not the same as the centre ofthe laser scanner. This will yield a translation of the scanner when the IMUmeasures an angular velocity. This translation has to be compensated for.

It is important to note that the origin of the World frame is being set when a scansequence is started, then the origin is stationary during the sequence, while theBody frame’s origin constantly moves with the scanner. If the scanning sequenceis assumed to be arbitrary long the World frame acts like a World frame usuallydoes, i.e. it is fixed in a global origin.

2.4 Laser Scanner

To support the machine with vision-feedback a laser scanner (SICK LMS 231) isused. The scanner is mounted on the machine at the top front of the cabin. Fromits position the scanner gets a good overview of the situation. The mounting ofthe laser scanner can be seen in Figure 2.3.

10 The Machine

Figure 2.3. The mounting of the laser scanner.

Figure 2.4. Laser scanner. Picture taken from [6].

Table 2.1. Data for the SICK LMS291-S05.

Field of view 180Angular resolution 0.25, 0.5, 1.0Operating range 0,. . . ,80mScanning Frequency 75 Hz

The scanner uses laser technology, i.e. it sends out a laser beam and by mea-suring the time it takes for the beam to reflect and return to the scanner thedistance to the reflecting point can be calculated since the speed of the laser beamis known. This method gives information about one single point, but by rotatingthe scanner at the same time as it emits laser beams knowledge of a number ofpoints on a line will be provided, as shown in Figure 2.4. By doing this rotation

2.4 Laser Scanner 11

the scanner can provide a 2D-point cloud consisting of 180, 360 or 720 points de-pending on which angular resolution has been set (see Table 2.1 for more scannerdata).

However, a two dimensional description of the environment is not good enough.To expand the point cloud into three dimensions the scanner has to be tilted aroundits y-axis, see Figure 2.5 for a definition of the coordinate frame.

Figure 2.5. Definition of the scanner’s coordinate frame.

Figure 2.6. Definition of scanner-angles. Here α is the angle describing how the scanneris tilted and β is the heading angle for the scanner.

The tilting mechanism consists of a PID-controller, EPOS70/10 from MaxonMotor which controls a DC-motor, see [6] for more information. The DC-motorprovides the system with the tilt angle (α). Since the output from the scanner ismeasured distance (R) and bearing (β) a transformation from polar to cartesiancoordinates is needed:

x = R cos(α) cos(β)y = R cos(α) sin(β)z = z −R sin(α)

where z is the height at which the scanner is mounted. In terms of the currentapplication, the angular resolution responds to how large steps the β-angle in

12 The Machine

Figure 2.6 will take. The α-resolution responds to how large steps the DC-motorwill take when controlling the tilting. This is how a point cloud is built, byrotating and tilting a scanner using laser technology. The procedure is called a"scan sweep". Figure 2.7 shows a comparison of the outputs from a scan sweepand a camera, of an office space.

(a) Camera image (b) Point cloud from scanner

Figure 2.7. Comparison of an image by a regular camera and a scanner.

2.5 IMU

As mentioned in Section 1.2 the movement during a scan will be compensated forusing an inertial measurement unit. The IMU used in this report is a Crossbow400CA-200. Accelerometers and gyroscopes measures acceleration and angular ve-locity along all three axes. If the IMU:s output would to be perfect and non-biased,the machine could integrate the outputs and always have a perfect knowledge ofits position. However, this is not the case since there exists a bias-term in everysensor of the IMU. Due to the integration, the bias-terms will introduce a drift inposition and angle. One way of dealing with this problem is to start the IMU withan initialising sequence from which the bias-terms are estimated. This estimatedbias-term is then removed from the outputs. A comparison of the outputs frombias-term-compensation and raw output can be found in Figure 2.8 and Figure2.9. In this analysis, the IMU has been collecting data for 1 second for the initialbias estimation and the IMU has been stationary, i.e. the outputs should be zerofor every sensor.

2.5 IMU 13

Figure 2.8. Illustration of the impact of bias compensation on the gyroscopes.

Figure 2.9. Illustration of the impact of bias compensation on the accelerometers.

As shown in Figure 2.8 and Figure 2.9, there is a significant improvement when

14 The Machine

the estimated bias-term is removed from the data.

Figure 2.10. Bias-compensated output from gyroscopes.

Figure 2.10 shows that also the bias-compensated gyroscope-output is affectedby a drift. This drift occurs because the initial bias cannot be estimated goodenough in the starting sequence, due to noisy measurements. Therefore the so-lution with an initial bias estimate is considered insufficient. Instead a differentsolution has to be derived, one which controls the bias-terms in a more dynamicway.

2.5 IMU 15

Figure 2.11. Bias-compensated output from accelerometers.

Figure 2.11 clearly shows the inaccuracy in the accelerometers. A displacementof 10 metres after 60 seconds is much too inaccurate. The present application needsa accuracy level of centimetres. However, even though the accelerometers are toopoor for describing the translation, they can be used as indicators of when themachine is standing still in so called "micro-breaks". Some assumptions can bemade based on the dynamics of a wheel loader. For instance, the machine cannotturn around the IMUs axis. This knowledge can be used to update the bias-termsduring these micro-breaks, as described in Algorithm 1. This method would failif the machine were able to turn around its own axis. This is because there is noacceleration in that case, which would lead the accelerometers to detect a micro-break and update the bias-terms with wrong information. While the value of theabsolute acceleration absAcc is smaller than the standard deviation σ (calculatedfrom the initialising sequence) the algorithm assumes that the IMU measurementsΩ only detect the bias-terms. A counter k′ is used to keep track of how many timesteps the machine appears to be stationary. If the counter reaches a certain levelthe bias-terms b is updated by taking the mean value of the IMU measurementsfrom the last k′ time steps. The bias-terms are used to calculate the Euler anglesΘ, as seen in the last part of Algorithm 1. However, due to the inaccuracy of themeasurements, this method works poorly when the machine is moving. Figure2.12 shows the output from a test drive where the machine drove over a bumpwith full load at 10 km/h.

16 The Machine

Algorithm 1 Time varying bias estimation, TVBE.b = [0, 0, 0]Tif |absAcck − g| < σg thenk′ = 1while |absAcck+k′ − g| < σg dok′ = k′ + 1

end whileif 10 < k′ thenb =

∑k+k′

l=kΩl

k′

end ifend ifΘk = Θk−1 + ∆t ·

(Ωk − b

).

Figure 2.12. Output from ’time-varying-bias-estimation’, TVBE.

Figure 2.12 illustrates the problem with the method described in Algorithm 1.The noisiness in the sensors can lead the machine into thinking that it is having amicro-break when it in fact is moving. This obviously has a great impact on theangle outputs. Instead a method where the angular velocities are said to be zerowhen the accelerometers are detecting zero-acceleration has been used.

2.5 IMU 17

Algorithm 2 Time varying zero bias estimation, TVZBEif |absAcck − g| < σg then

Ωk = 0end ifΘk = Θk−1 + ∆t · Ωk

If an IMU with more accurate accelerometers had been used Algorithm 1 wouldhave been a better solution than Algorithm 2, but with the current situation thelatter is more desirable due to its robustness. The results from the same experimentas in Figure 2.12, but with the data processing method described in Algorithm 2,is shown in Figure 2.13.

Figure 2.13. Output from ’time-varying-zero-bias-estimation’, TVZBE.

Consider the reference in the Figures 2.13 and 2.12. It is the angle outputfrom the data set where only a initial bias have been estimated. Due to the shorttime of the experiment the angles will stay close enough to the actual truth, whichcannot be measured.

With a longer time-horizon even the TVBZE gets some problem. This is shownin Figure 2.14, where the IMU have been stationary for a longer time. Since it hasbeen standing still the outputs should be zero.

18 The Machine

Figure 2.14. Output from a stationary IMU over a longer time-horizon.

Figure 2.14 indicates that the TVBE is better for this case. However, due to itspoor performance in the dynamic case the TVBE will not be used. The algorithmused in this report is the TVBZE.

2.6 Translation Counter

Due to the inaccuracy in the accelerometers, the translation part of the distortionis handled by a translation counter, which is mounted on the machine. The counterregisters a tick when the wheel axis has rotated 6.4 mm.

A possible error source is the compression of the tires, which depends on theload of the ladle - if the tires are compressed the machine has not moved as far ona tick as if the tires was not compressed. This has not been compensated for inthis work, due to the lack of measuring availability. Another problem is the factthat it is not enough just to know how far the machine has travelled during a scansweep, but also in which direction. This can be solved by fusing the angle outputfrom the IMU with the translation counter value.

2.6 Translation Counter 19

2.6.1 Test

To test the accuracy of the translation counter, the scanner was mounted onthe wheel loader which drove against a wall while the scanner made a sweep.Afterwards the output from the translation counter was used to compensate for themovement and the standard deviation of the length to the compensated wall wascalculated. This standard deviation was then compared with one were the scannerhad been stationary during the scan-sweep. The same test was then performedwhen the wheel loader drove against a stationary hauler. Here the length to theside of the truck bed has been compared. The results can be found in Table 2.2.It is worth mentioning the differences in the length to the objects during the twotests - 20 m to the wall and 5 m to the hauler. Both tests were performed at 10km/h.

Table 2.2. Standard deviations, in cm, for the length to the scanned object.

Object Stationary Non-compensated CompensatedWall 11.21 79.85 17.88Hauler 1.98 17.73 2.59

As seen in Table 2.2 there appears to be a significant improvement on the dataafter the compensation. Even though the deviations do not reach as low as thestationary levels, they are considered to be good enough. This can be illustratedin Figures 2.15 and 2.16, from which the data that appears in Table 2.2 has beenextracted.

(a) Not compensated (b) Compensated

Figure 2.15. Scan-picture of a wall, before and after un-distortion.

Figures 2.15(a) and 2.16(a) clearly shows the distortion of the point cloudwhen scanning while moving. As mentioned earlier and shown in Table 2.2, thetranslation counter takes care of the translation part of the distortion. This canbe seen in Figures 2.15(b) and 2.16(b).

20 The Machine


Figure 2.16. Scan-picture of a hauler, before and after un-distortion.

The above tests were performed when the machine was driving straight towardsthe objects it has scanned. Hence, it has so far only been shown that the translationcounter handles translation along the x-axis. This is rarely the case for a wheelloader, and it cannot be considered sufficient that the translation counter onlyhandles translation along the x-axis. Therefore, tests have also been made whenthe machine drove against an object with a non-zero heading angle. Normally, theassumption that vehicles moves in two dimension is made, and then (2.2) can beused to describe its movement

tx = T cos(ψ), (2.2a)

ty = T sin(ψ), (2.2b)

where T is the measured translation value, i.e. the covered distance and ψ is theheading angle. The coordinate frame is defined in Figure 2.5. However, due tothe vibrations in the machine the two dimensional assumption cannot be done.Hence, a three dimensional transform is needed, and is presented in (2.3)

t = T

cycz −cysz sycxsz + sxsycz cxcz − sxsysz −sxcysxsz − cxsycz cxsysz + sxcz cxcy

. (2.3)

(2.3) uses the Euler sequence z−y−x, meaning the main rotations will be aroundthe z-axis. The notation ci = cos(i) and si = sin(i) is used for simplification, i.e.cy = cos(pitch)

2.7 Waist Angle 21


Figure 2.17. Scanning of a wall, made while the loader were turning.

Figures 2.17 and 2.18 contain the point clouds after turning while scanning.Figure 2.17(b) shows some inaccuracy, but the result is still considered to be goodenough to state that the translation counter combined with (2.3) is sufficient fordescribing the three dimensional translation.


Figure 2.18. Scanning of a hauler, made while the wheel loader was turning.

2.7 Waist AngleEarlier thesis workers have installed a sensor that measures the waist angle of themachine. It has not been used in this report, but for future work a framework forusing it has been derived. The typical way of using the waist angle would be tosupport the IMU with measurements of the yaw angle.

2.8 Rigid Body TransformTo make use of all of the above mentioned sensors a 3D-transform (see [9] formore information about the transform) has been implemented. The transform isperformed on every point in the distorted point cloud, re-distorting the picture andprovides the laptop-computer with a smooth, clean point cloud, see Figure 2.19.

22 The Machine

Figure 2.19. Flow chart showing the transformation procedure.

The transform consists of two parts; a translation part and a rotation part. Inthe software the transformation has been implemented as a function which is runon each scanned point in the point cloud.

2.8.1 TranslationTo solve the translation part of the problem, the covered distance in each directionhas simply been added to the scanned point. One approximation that has beenmade is that the machine has no translation during the time it takes to do onescan line. Clearly this is not true, but the translation is assumed to be sufficientlylow to approximate to zero. A scan line typically takes 6-7 ms to scan, whichyields a translation of 2.7 cm when the machine is running at high working speed(15 km/h).

2.8.2 RotationThe implemented transform uses Euler angles to describe the rotation. The maindrawbacks is the singular points. However, since these will only occur when thepitch angle is ±90, Euler angles can be used for this specific application. Themain reason why Euler angles are used in this work is their simplicity to imple-ment, compered to the more complex quaternion approach, see Appendix A. Eventhough quaternions are not used in the actual machine set-up, they are used inChapter 3, Modelling and Filtering.

2.8.3 Complete TransformThe complete 3D-transform is described by (2.4), where pτ is a scanned point attime τ .

pτ = Rτ pτ + tτ =

cycz −cysz sycxsz + sxsycz cxcz − sxsysz −sxcysxsz − cxsycz cxsysz + sxcz cxcy

pτ +

tx,τty,τtz,τ

(2.4)

2.8 Rigid Body Transform 23

where ci = cos(i) and si = sin(i) (i.e. cy = cos(pitch)) and tj,τ is translation in thedirection j at time τ . The transform in (2.4) moves all points to where they shouldhave been if the machine would have been stationary during the scan sweep. Thisleads to some problems. For instance, consider once again the situation when theloader is moving towards a wall during a scan sweep. If the first point during thescan is detected at x0, then the final point will be detected at x = x0 −∆x andthen the translation part of the transform moves all the points as if they were atdistance x0, when they in fact now (at the end of the scan sweep) are at distancex. Therefore the final translation value has to be removed from every value

p′

τ = pτ − tT . (2.5)

This goes for the rotation part of the transformation as well, the points have tobe rotated so that the final point becomes the origin. This is done by multiplyinga transformed point with the inversed rotation matrix containing the final angles,R−1T .

pτ = R−1T p

′

τ . (2.6)

Combining (2.4), (2.5) and (2.6) gives the complete transform (2.7) that transformsa distorted point pτ into an un-distorted point pτ at time τ .

pτ = R−1T (pτ − tT ) = R−1

T (Rτ pτ + tτ − tT ) . (2.7)

2.8.4 Real-Time ControlIt is vital that all the above works well together in real-time. For instance, if thelaptop-computer cannot associate the right scan data with the corresponding IMUdata the un-distortion will not work. The IMU is connected to the laptop-computervia RS232 and the translation counter (and the waist angle) is controlled by aAVR which is connected to the laptop-computer. The laptop-computer creates apackage containing data from all of the external hardware. This is done for everyscan line in a scan sweep. The IMU has a frequency of 133 Hz, to be comparedwith the 75 Hz of the scanner, which will yield a small miss-match in the dataassociation. In the worst case scenario this miss-match will end up in a 7.5 ms offsetbetween scanner and IMU data. In some applications this would be a problem,but due to the slow dynamics of a wheel loader this problem is overlooked.

Chapter 3

Modelling and Filtering

Describing the system demands a mathematical model. For linear models this isusually done on the form:

x = Ax+Bu+ w, (3.1)y = Cx+Du+ e, (3.2)

where x is the state of the system, u is the input, y is the measurement, w isprocess noise and e is measurement noise.

Example 3.1Consider a model describing a vehicle’s basic movement. The states of the systemhave been chosen as position, x1 and velocity, x2. Assume that the position ismeasured in some way, for instance with a GPS. Due to the lack of knowledge ofthe acceleration of the system it is modelled as (process) noise, w. Note that thereis no input to this system, it is driven only by noise. The dynamic equations ofthe system are:

x1 = x2 (3.3)x2 = w. (3.4)

The output from the GPS would provide the position of the vehicle, but themeasurements would be affected by a (measurement) noise, e:

y = x1 + e. (3.5)

Rewritten on the state space form:

x =(

0 10 0

)x+

(01

)w (3.6)

y =(

1 0)x+ e. (3.7)

25

26 Modelling and Filtering

The model in example 3.1 is very basic. For instance, if the vehicle would be acar, the assumption that there is no input would probably be inaccurate. Then themodel would have to be augmented to also handle acceleration (as input) of the car.Because of inaccuracy in the accelerometer measurements the states describing thepure translation (position, velocity and acceleration) have been excluded. Nor thebias-terms have been included in the model. Hence, a model consisting only of theorientation-states is used. A derivation of this model can be found below, and fora more extensive model, see Appendix B.1. Much of the modelling of a rigid bodyin three dimensions have been extracted from [10].

3.1 Noise ExaminationIt is of big importance to get a good knowledge of the noise in the model. Model-wise, there exist two kinds of noise; measurement noise (et) and process noise (wt).The noises are modelled as stochastic variables with a distribution according to:

et ∼ N (0, Rt), (3.8a)

wt ∼ N (0, Qt), (3.8b)

where N is the normal distribution and Qt and Rt are covariance matrices. Thematrices Qt and Rt will later be used in the Extended Kalman Filter (EKF). Inthe sections below it is investigated if the noise from the sensors in fact can bedescribed as normally distributed stochastic variables, as (3.8) suggests.

The sensors have been examined for two different cases, one where the IMUhave been standing still on the ground and one where the IMU have been mountedon the wheel loader while driving over a bump, to include the main movements ofthe scanner during a short loader cycle. The two data sets will be refereed to asDSC (data set constant) and DSWL (data set wheel loader).

3.1.1 Measurement Noise

Every sensor will be affected by a measurement noise, so also the IMU. Becauseof the presence of offsets in the sensors, it can be a bit tricky to investigate themeasurement noise. If one does not model the bias as a state, it should be seen asa part of the measurement noise. As mentioned above this work does not modelthe bias as a state. This is because of lack of other sensors helping the IMU sortingout what is angular velocity and what is bias. However, in this noise examinationthe mean value has been removed to study the measurement noise because themodel will include the bias as a state later on in the project, when a new sensoris implemented.

The procedure of removing the mean value is only done on the DSC. TheDSWL does not consist of pure noise, due to its movement and has only been usedas a reference.

3.1 Noise Examination 27

Accelerometer

After the mean value has been removed from the accelerometer measurements, themeasurement noise can be studied. Figure 3.1 shows the noise from the accelerom-eter.

Figure 3.1. Measurement noise from the accelerometers with DSC.

Figure 3.1 suggest that the accelerometer noise can be expressed as:

eacc ∼ N(0, Qacct ), (3.9)

where Qacct is evaluated to

QDSCacc = 10−3 ·

4.6185 0.0581 0.04300.0581 4.8226 0.11790.0430 0.1179 6.6536

. (3.10)

It can be worth mentioning that there seems to be some kind of correlation inthe measurements, otherwise the matrix would be diagonal. This "stationary"covariance matrix can be compered to the dynamic one, QDSWL

acc :

QDSWLacc =

1.4225 0.0267 0.14000.0267 0.6700 0.07370.1400 0.0737 1.4269

The unit of the covariance matrices is m2 s−2.

Gyroscope

The same calculation were performed on the gyroscope’s output. The result canbe seen in Figure 3.2.


Figure 3.2. Measurement noise from the gyroscopes with DSC.

In the same way as with the accelerometers, a normal distribution approxima-tion seems to be reasonable. Hence,

egyro ∼ N (0, Qgyrot ) (3.11)

where Qgyrot is evaluated to

QDSCgyro = 10−5 ·

0.7338 −0.0337 −0.0001−0.0337 0.6185 0.0108−0.0001 0.0108 0.7631

(3.12)

which can be compered to the "dynamic" covariance matrix:

QDSWLgyro = 10−3 ·

0.4333 −0.1173 0.0391−0.1173 2.1156 −0.21510.0391 −0.2151 0.0982

. (3.13)

The unit for these covariance matrices is rad2 s−2. Also the gyroscope measure-ment seems to be correlated in some way. One can see that the noise of thegyroscope is small, for instance a factor 1000 smaller than the covariance fromthe accelerometers. This implies that the angular rates can be seen as (almost)known inputs to the system, a fact that will be used later in the derivation of thedynamic model. The above mentioned correlation could of course pose a problem,but it seems to be small enough to be negligible.

3.1.2 Process NoiseA complete examination of the noise would also cover the process noise. However,because of the structure of the dynamic model that has been used, the only processnoise is the gyroscope’s measurement noise.

3.2 Dynamic Model 29

3.2 Dynamic ModelTo make use of the inertial sensor measurements, a mathematical model has to bederived. Due to the rotational dynamics the model will be non-linear on the form

xt+1 = f(xt, ut, vt), (3.14)yt = h(xt, ut) + et. (3.15)

A common model of this system would consist of two parts, one linear trans-lation part and one non-linear rotation part which is describing the orientation,or the attitude, of the system. Due to the fact that the accelerometers are tooinaccurate to use for positioning, this report will only include a model consistingof a dynamic model describing the attitude of the system. See Appendix B formore about the complete model.

Another simplification of the model is the exclusion of the bias term fromthe gyroscopes. This is because of the lack of other sensors. If no other sensorsthan the IMU is being used, then it is impossible to divide the measurement intomovement and bias, and therefore the bias state is not included in the model.

3.2.1 Attitude DynamicsTo describe the orientation of the body quaternions will be used. For more infor-mation about quaternions, see Appendix A. To be able to use the quaternion as astate, knowledge of the quaternion’s derivative is needed. This dynamic is derivedin [10], Appendix B. The result is:

q = 12S(ω)q, (3.16)

where ω is the angular velocity and

S(ω) =

0 −ωx −ωy −ωzωx 0 ωz −ωyωy −ωz 0 ωxωz ωy −ωx 0

. (3.17)

Here ω is the angular velocity of the IMU relative the world frame, described inthe body frame. Here it is worth thinking about if one wants to model the angularvelocity as a state or as a control input to the system. The latter outlook impliesthat the angular velocity can be measured deterministically, without any noise.Section 3.1.1 suggests that this is not the complete truth. However, the noise isassumed to be sufficiently small to exclude the angular velocities as states. Still,the noise is not sufficiently small to approximate it to zero. Hence, (3.16) can beapproximated as:

q ≈ 12S(ω + v)q. (3.18)


The choice of modelling the angular velocity as a control input also reducesthe size of the model, which is preferable. By using the result from (A.14) inAppendix A, (3.18) can be re-written as

q ≈ 12(S(ω) + S′(q)v), (3.19)

where v is the process noise, distributed according to Section 3.1.2. On a statespace form this can be written as:

q = Aq +Bv = 12(S(ω)q + S′(q)v). (3.20)

(3.20) has to be on discrete form qt+1 = Fqt + G(qt)v. This can be done by thediscretization

F = eAT ,

G =T∫

0

eATBdt.

and the assumption that the control input (angular velocity) is constant betweenthe sampling times. By applying the above discretization (3.20) becomes:

qt+1 = e12S(ω)T qt. (3.21)

This is a quite complex expression that one would like to simplify. Though,this is not very simple but [11],[10] has one way of solving the problem. They areusing series expansion and the fact that

S(ωt) = −||ωt||2I.

Doing this leads to the following results:

qt+1 =

cos(‖ω‖T

2

)I +

sin(‖ω‖T

2

)‖ω‖

S(ω)

qt. (3.22)

Due to the high sample rate of the IMU (133Hz) and the low angular velocityof the wheel loader the small angle approximation can be made. This gives thetime-discrete system on state space form as:

qt+1 = Fqt +Gv =(I + T

2 S(ωt))qt + S′(qt)v. (3.23)

3.2.2 Measurement ModelThe IMU will always provide measurements within its own coordinate frame.Therefore a model that describes that measurements in the global frame is needed.This can be done by a rotation matrix R(q) which can be found in Appendix A

3.3 Extended Kalman Filter 31

yacct = R(q)(awt + gw) + eacct (3.24)

where yacct is the measured acceleration at time t, awt is the acceleration in theworld frame at time t, gw is the force of gravity in the global frame and eacct isthe measurement noise at time t. However, due to the fact that the accelerometeris unable to recognise any difference between actual acceleration and the force ofgravity one have to assume that the acceleration is equal to zero. Hence,

yacct = R(q)gw + eacct . (3.25)

This assumption implies that the measurements from the accelerometers areonly used as an indicator for the direction of gravity.

3.3 Extended Kalman Filter

To make use of the measurements together with the mathematical models derivedin earlier sections an Extended Kalman Filter (EKF) is used. This report willnot give any introduction to the Kalman Filter, since there already exists verymuch information about it. The interested reader is referred to [12], [13] and theexcellent website by Prof. Greg Welch and Prof. Gary Bishop, http://www.cs.unc.edu/~welch/kalman/index.html. The reason why a KF is not sufficient isthe non-linear rotation part. To cope with this the EKF linearize the model aroundthe latest state estimate. Other non-linear methods and filters (such as UncentedKalman Filter, UKF and Particle Filter, PF) could have been used. It is worthmentioning that EKF cannot guarantee optimality, which is the case for KF. Still,the EKF often provide good results. The filter consists of two parts; a time updateand a measurement update. In the time update a prediction of the state vectoron the basis of the dynamic model is made. The measurement update uses thisestimate together with the measurements to update the state vector. This is themain idea, but as mentioned in Section 3.2.2 the assumption of no accelerationhas been made. Therefore the measurement update part of the EKF will not bemade if the system detects acceleration. This means that if the system detects anyacceleration, the estimation will only depend on the dynamic model. Algorithm 3describes the procedure.


Algorithm 3 Extended Kalman Filter, EKF1. x0|0 = x0, P0|0 = P0

2. Time update:

xt+1|t = ft(xt|t, ut) (3.26a)Pt+1|t = FtPt|tF

Tt +GtQtG

Tt (3.26b)

3. Measurement update:if |absAcck − g| < σg then

Kt+1 = Pt+1|tHTt+1(Ht+1Pt+1|tH

Tt+1 +Rt+1)−1 (3.27a)

xt+1|t+1 = xt+1|t +Kt+1(yt+1 − h(xt+1|t, ut+1)) (3.27b)Pt+1|t+1 = (I −Kt+1Ht+1)Pt+1|t (3.27c)

end if

4. t := t+ 1→ repeat from step 2.

In Algorithm 3 x is the state vector and u is known inputs to the system, in thiscase the angular velocities. Further, F , G and H are the Jacobians with respectto the previous state vector, in other words the linearization:

Ft = ∂f(x, u, v)∂x

|(x,u,v)=(xt|t,ut,0), (3.28a)

Ht = ∂h(x, u)∂x

|(x,u)=(xt|t−1,ut), (3.28b)

Gt = ∂f(x, u, v)∂v

|(x,u,v,)=(xt|t,ut,0). (3.28c)

Remember the zero-acceleration assumption made in Section 2.5, which is notthe same as the one made in the above sections. The assumption in Section 2.5is made to control the bias-terms in the gyroscopes. The assumption made inSection 3.2.2 is made because it has to be made, it is impossible to sort out actualacceleration from the force of gravity. With this in mind, Algorithm 4 describesthe complete filter that has been used for the simulations.

Chapter 4

Experimental Results

In this chapter the experimental results will be presented. The EKF solution hasonly been implemented in MATLAB and therefore it has not been possible to usethe output from the filter to compensate the distorted point cloud. This is notany restriction in the result, because the un-distortion part would be the same. Itis only the extraction of angles from the IMU that differs in the two sections.

4.1 Results from the Machine

There have been a large amount of tests on the machine. In this section three of thescenarios will be presented; driving over a speed bump on an asphalt range, drivingtowards a hauler and pile on gravel and driving towards a pile and material pocketsat NCC’s asphalt mill in Kjula. The latter test is probably the most interesting,since it is made at an actual site.

4.1.1 Speed Bump

This test was performed to get knowledge of how well the IMU would pick up bigmovements on the machine. The speed bumps were around 50 mm high and wideenough for all the wheels to hit them. Figures 4.1 and 4.2 shows the Euler anglesfrom the experiment. The data presented in Figure 4.1 was collected at a speedof 10 km/h and with full load. Experiments were made for several other speeds,up to 15 km/h, with and without load. Only the 10 km/h with and without loadwill be presented, but the rest of the results were similar to the ones presented.

35

36 Experimental Results

Figure 4.1. Output from IMU when driving over a speed bump at 10 km/h, full load.

Figure 4.2. Output from IMU when driving over a speed bump at 10 km/h, no load.

Figure 4.1 and Figure 4.2 show that the main movement for the machine, ex-

4.1 Results from the Machine 37

cept the translation, is the rotation around the y-axis (pitch angle). As mentionedearlier, this test was made to see if the IMU would provide the correct angles. Tocheck this, the experiment was filmed and looked at afterwords. The ocular reviewsuggested that the IMU is good enough to pick up the movements. Obviously thismethod has an uncertainty, but the un-distorted point clouds in Figures 4.3(b)and 4.3(d) indicate that the IMU in fact is good enough to use in this application.These figures also show a potential problem. As seen in Figure 4.1 there is a ro-tation in pitch, which yields a movement along the x-axis. In the above case themovement is, at most, 20 cm. This movement will not be picked up by the trans-lation counter nor the IMU when the IMU is not used to measure the translation.Now, it should be noted that this test was performed under extreme circumstances,a bump with a height of 50 mm cannot be considered a usual feature at a workingsite. Still, this x-movement may be a potential source of errors.

4.1.2 Gravel Surface

The next test performed on the machine was made at Volvo CE’s simulated work-ing site. A hauler was placed next to a gravel pile and the machine was to scanthe environment. The point of this set-up is to imitate the set-up when the ma-chine is doing a short-loader-cycle. The experiment was done at various velocities(5 km/h, 10 km/h and 15 km/h), with the loader coming in straight or crookedagainst the hauler.

Figures 4.4(a) and 4.4(b) show a reference scan of the environment, made whenthe machine was stationary. One important aspect of this test was to see how wellthe transformation can describe the pile structure. The pile structure is later usedin algorithms for calculating how to attack the pile.

Figure 4.4(b) show some interesting characteristics of the pile, such as a con-vexity. This is a desirable attack point on the pile. This work will not presentany detailed information about the pile algorithms. The interested reader is re-ferred to [5] and [7]. The distorted (uncompensated) point clouds can be foundin Figures 4.4(c) and 4.4(d). In this scan the loader came in crooked against thehauler/pile with a velocity of 10 km/h.

Figure 4.4(c) shows the large impact of the distortion, where the hauler almostseems to be lying on the ground. This can also be seen in Figure 4.4(d), wherethe pile seems to be more steep. Hence the pile algorithms will tell the machineto approach the gravel pile in an adverse way. However, as Figure 4.4 shows, thecompensation takes care of this problem.

Figure 4.4(f) does not give an exact copy of Figure 4.4(b). However, it seemsto reproduce the reference pile sufficiently for the pile algorithms to calculate thecorrect approach. One very interesting test would be to apply the actual pilealgorithms on the compensated point cloud, to see if this really works. This issomething to be done for future thesis workers.


(a) Not compensated, with full load.

(b) Compensated, with full load.

(c) Not compensated, with no load.

(d) Compensated, with no load.

Figure 4.3. Point clouds from the scanner when the machine is moving at 10 km/h.


(a) Hauler and pile, reference scan. (b) Pile, reference scan.

(c) Hauler and pile, distorted point cloud. (d) Pile, distorted point cloud.

(e) Hauler and pile, un-distorted pointcloud.

(f) Pile, un-distorted point cloud.

Figure 4.4. Point clouds showing a hauler and a pile.


4.1.3 Asphalt MillOne of the intended working sites is NCC’s asphalt mill in Kjula. The environmentthere consists of piles with different materials which are moved to material-pocketsand later mixed together to produce asphalt. This site is thought suitable forautonomous use. The machine was driven to simulate a working task, includingdriving towards a pile and towards the material pockets. There were differentmaterials in the piles, shingle and fine grain. Figure 4.5 show reference picturesof the environment. The simulation was performed at three different velocities, 5,10 and 15 km/h.

(a) Shingle pile, no correction. (b) Shingle pile, with correction.

(c) Fine grain pile, no correction. (d) Fine grain pile, with correction.

(e) Material pockets, no correction. (f) Material pockets, with correction.

Figure 4.6. Point clouds taken from the scanner while driving at 5 km/h.


(a) Shingle pile.

(b) Fine grain pile.

(c) Material pockets.

Figure 4.5. Reference pictures of the scanned environment.


Figures 4.6(a) - 4.6(d) show that the structure of the pile can be detected afterthe un-distortion. The results are very positive and indicate that scanning whilemoving may be performed, at least at the speed of 5 km/h, just as Section 4.1.2indicated. Even more interesting is the results shown in Figures 4.6(e) and 4.6(f),where the material pockets are scanned when driving at 5 km/h. From these figuresit is seen how the pockets have been straightened up and now the structure of thematerial in the pocket can be viewed. These results indicate that the un-distortionalso handles the scanning of pockets. Combined, it can be stated that a workingtask at the asphalt mill could be performed, at 5 km/h. When the simulation wasmade for 10 and 15 km/h, the results were similar, but not as good as 5 km/h,due to increased vibrations etc. Figures 4.7(a) - 4.8(e) show the results. In thepocket case the distorted point cloud is too distorted to show anything of value.Therefore only the un-distorted point clouds have been included. As Figures 4.7(a)- 4.7(e) show, the results are satisfying even when the speed is increased. Whenincreasing the speed even more, to 15 km/h, the machine’s vibrations affects thepoint clouds even more. However, the un-distorted point clouds are considered tobe good enough input to the forthcoming algorithms. It should be noticed thatall these results has been evaluated only by the naked eye. No scientific measureshave been used.




(e) Material pockets, with correction.


4.2 Simulation Results 45

4.2 Simulation Results

As mentioned in the introduction of this chapter, the EKF has not been imple-mented together with the rest of the c-code. Therefore no un-distortion has beenmade with the data presented below.

Three different dynamic models have been implemented, all using quaternionsas rotation representation. The results are presented in Euler angles. The trans-form from quaternions to Euler angles is described by (A.13) and is performedafter the filtering is done. The three different quaternion models are:

• ’IMU1’ - A 3D-model with the quaternions as states and the angular ratesfrom the IMU considered as known inputs to the system.

• ’IMU2’ - A full 3D-model with position, velocity, acceleration and quater-nions as states and the angular rates as inputs.

• ’IMU3’ - A expansion of the ’IMU2’, now containing bias-terms and theangular rates as states. In other words no inputs to this system.

Before looking at the plots, one could make some predictions of what the resultmay be. For instance, the model using bias terms should not give any successfulresults, due to the lack of an extra sensor. It is impossible for the IMU aloneto detect what is angular movement and what is bias. Also, the model havingposition, velocity and acceleration as extra states should not give any better resultthan the one containing only quaternions as states. This is because of the non-acceleration assumption.

Every model has been tested for three different cases; one where the IMUwas stationary, one where the IMU was affected by a movement which meant tosimulate the machine dynamics, and one where the IMU was actually mountedon the wheel loader. The different cases are called stationary mode, WL mode(the simulated movement) and dynamic mode. Figures 4.9 - 4.18 shows the result,compared to dead reckoning.


Figure 4.9. Rotation angles using the ’IMU1’, stationary mode.

Figure 4.10. Rotation angles using the ’IMU1’, WL mode.


Figure 4.11. Rotation angles using the ’IMU1’, dynamic mode.

Figures 4.9 - 4.11 show an improvement in the stationary case, and a similarbehaviour in the dynamic case. However, for the WL case the EKF estimate doesnot perform well. This can be explained by the fact that the zero-accelerationassumption fails for this case, but the IMU fails to detect it. Hence, the IMUthinks the machine is having a micro break and makes the measurement updatephase of the EKF while the loader is in fact moving. Figure 4.12 shows the WLand the dynamic mode once again, but now with a "micro-break-indicator” whichturns high when a micro-break is detected. Figure 4.12 shows that in the WL modemicro breaks are detected very often, compered to the dynamic mode. Anotherreason for the poor behaviour in the WL case can be a poor initialisation of theIMU for this case, leading it to remove the wrong bias term. Obviously this can bea problem, but due to the fact that Figure 4.11 shows good performance the IMUseems to pick up the actual machine movements, even though it cannot managethe simulated ones. In the stationary mode one sees the most improvement. After60 seconds, roll and pitch has an error of less then ±0.05. The error here is theoutput, due to the fact that the IMU was standing still, i.e. the ground truth iszero. The yaw angle is not as good. This is because of its unobservability in yaw -the direction of which the gravity affects the IMU will not change when the IMUis rotated in yaw.


(a) WL mode

(b) Dynamic mode

Figure 4.12. Micro break indicator.



As mentioned earlier there is no improvement in the extended model, ’IMU2’.The reason why it has been implemented is for future work.





The model using bias terms as a part of the state vector is similar to the other


ones in the first two cases. However, in the dynamic mode a clear deteriorationis seen. This is because the EKF tries to sort out the bias terms, which it cannotdo due to lack of sensors. Therefore a part of the dynamics ends up in the biasstates. This is shown in Figure 4.19. The bias terms should be close to normal,which apparently is not the case here.

Figure 4.19. Bias states using the ’IMU3’, dynamic mode.

The above figures show that ’IMU1’ and ’IMU2’ give the same angular outputs,which they should do since they are using the same formula for calculating theorientation. The difference in these models are the translation-part in ’IMU2’.However, the model estimating bias gives different results. Figure 4.20 shows acomparison of the models ’IMU1’ and ’IMU3’, for all data sets. The dead reckoningis also included, as a reference.


(a) Stationary mode

(b) WL mode

(c) Dynamic mode

Figure 4.20. Comparison of the result with and without bias-estimation. The blackcurves are the outputs from the models without bias-estimation (’IMU1’), the light graycurves are the outputs from the models without bias-estimation (’IMU3’) and the darkgray curves are the outputs from the dead reckoning.


4.2.1 Connection to the MachineIt is seen that the EKF provides more accurate IMU data than the methodsdescribed in Section 2.5. Hence, expanding the TVZBE with the filter would giveeven better results than the ones in Section 4.1. The reason why this has notbeen done is the small size of the improvement, compered to the work load ofintegrating it into the c-code.

Chapter 5

Concluding Remarks

In this chapter some conclusions based on the work described in earlier parts willbe presented. Section 5.1 shows that the goals described in Section 1.2 have in factbeen reached. Also some proposals for how to improve the autonomous machinewill be given.

5.1 ConclusionsChapter 4, and Section 4.1.3 in particular, shows that scanning while moving canbe done. One should note that this work has not in any way solved the generalscanning-while-moving-problem, but a pragmatic solution which works for thisspecific application has in fact been obtained. And with Section 4.2 showing suchpromising results, everything indicates that the machine can perform even betterin the future, without any many major change. Some possible changes are broughtup in the section below.

5.2 Future WorkTo make the most of the EKF derived in Section 3.3 at least one more sensor needsto be added to the system. For instance, [10] uses an additional magnetometerto obtain observability in yaw. Another option would be to use a camera-basedvision system to aid the IMU. An approach intended to be tested is scan matching.This will be performed within the co-operation with the members from ÖrebroUniversity, who have experience of this method, see [14] and [9]. To make use ofany of the additional sensors, the filter has to be translated into c-code.

A further suggestion for improvement would be to integrate the work describedin this work with [7]. It is vital for the forthcoming work that this part works well.As of today all parts run good on their own, but the integration has not yet beenstarted. The integration will be a part of the next thesis work in this project.

The biggest job left to do on the autonomous machine is the path-planing part.Without any path-planing, the machine will never be able to work in an arbitrary

55

56 Concluding Remarks

environment in autonomous mode. Also the path-planing is intended to be solvedby thesis works.

Bibliography

[1] Michael Bosse and Robert Zlot. Continuous 3d scan-matching with a spinning2d laser. In Proceedings of the IEEE International Conference on Roboticsand Automation, pages 4312–4319, Kobe, Japan, June 2009.

[2] Michael Montemerlo, Jan Becker, Suhrid Bhat, Hendrik Dahlkamp, DmitriDolgov, Scott Ettinger, Dirk Haehnel, Tim Hilden, Gabe Hoffmann, BurkhardHuhnke, Doug Johnston, Stefan Klumpp, Dirk Langer, Anthony Levandowski,Jesse Levisson, Julien Marcil, David Orenstein, Johannes Paefgen, IsaacPenny, Anna Petrovskaya, Mike Pflueger, Ganymed Stanek, David Stavens,Antone Vogt, and Sebastian Thrun. Junior: The stanford entry in the urbanchallenge. Journal of Field Robotics, 25(9):569–597, September 2008.

[3] David Gunstad and Peter Alnefelt. Autonomous Machine. Master’s thesisno.1, Mälardalen University, 2008.

[4] Jonatan Björkman. Control of an Autonomous Wheel Loader. Master’s thesisno.3, Department of Automatic Control, Lund University, 2008.

[5] Håkan Almqvist. Automatic Bucket Fill. Master’s thesis no.4, Departmentof Electrical Engineering, Linköping University, 2009.

[6] Etienne Abgrall. Vision System for Autonomous Wheel Loader. Master’sthesis no.5, KTH, Royal Institute of Technology, 2009.

[7] Jonatan Blom. Autonomous Hauler. Master’s thesis no.7, Mälardalen Uni-versity, 2010.

[8] The MathWorks. xPC Target - For Use with Real-TimeWorkshop, 1.1 edition,February 2007.

[9] Martin Magnusson. The Three-Dimensional Normal-Distributions Transform- an Efficient Representation for Registration, Surface Analysis, and LoopDetection. Phd dissertation, Örebro University, 2009.

[10] David Törnqvist. Estimation and Detection with Applications to Navigation.Phd dissertation, Department of Electrical Engineering, Linköping University,2008.

[11] Fredrik Gustafsson. Statistical Sensor Fusion. Studentlitteratur AB, 2010.

57

58 Bibliography

[12] Rudolph Emil Kalman. A new approach to linear filtering and predictionproblems. Transactions of the ASME–Journal of Basic Engineering, 82(SeriesD):35–45, 1960.

[13] Gary Bishop and Greg Welch. An introduction to the kalman filter. InACM Press, editor, Annual Conference on Computer Graphics & InteractiveTechniques, Los Angeles, CA, USA, August 2001.

[14] Todor Stoyanov and Achim Lilienthal. Maximum likelihood point cloud ac-quisition from a mobile platform. In Proceedings of the 14th InternationalConference on Advanced Robotics (ICAR), Munich, Germany, June 2009.

Appendix A

Quaternions

The quaternion set, Q, is defined as:

Q = C + Cj, (A.1)

where C is the set of complex numbers and j2 = −1, ij = −ji. Hence, an elementq ∈ Q can be written as:

q = q0 + q1i+ (q2 + q3i)j = q0 + q1i+ q2j + q3ij, (A.2)

wherei2 = j2 = k2 = ijk = −1, ij = k, jk = i, ki = −ik = j. (A.3)

Just as a complex number, z = a+bi, the quaternion can be described as a vector.However, instead of a two dimensional vector, it has to be described as a fourdimensional vector. This way of representing the quaternion is as a combinationof a scalar, q0 and a three dimensional vector, q. Hence,

q = (q0 q1 q2 q3)T =(q0q

). (A.4)

A.1 AlgebraConsider two quaternion elements, q, p ∈ Q. Then the algebraic properties listedbelow are valid.

Additionp+ q = (p0 + q0,p + q) (A.5)

Multiplication

p q =(

p0q0 − pTqp0q + q0p + p× q

)(A.6)

Conjugate

q =(

q0−q

)(A.7)

59

60 Quaternions

Norm‖q‖ =

√q q =

√q20 + q2

1 + q22 + q2

3 (A.8)

Inverseq−1 = q

‖q‖2, if q 6= 0 (A.9)

(p q)−1 = q−1 p−1 (A.10)

A.2 RotationThe rotation matrix used to describe the local measurements in the global worldframe is given by

R(q) =

(q20 + q2

1 − q22 − q2

3 2(q1q2 + q0q3) 2(q1q3 − q0q2)2(q1q2 − q0q3) (q2

0 − q21 + q2

2 − q23) 2(q2q3 + q0q1)

2(q1q3 + q0q2) 2(q2q3 − q0q1) (q20 − q2

1 − q22 + q2

3)

.

(A.11)The corresponding rotation matrix for the Euler angles is

R(Θ) =

cθcψ cθsψ −sθsφsθcψ − cφsψ sφsθsψ + cφcψ sφcθcφsθcψ + sφsψ cφsθsψ − sφcψ cφcθ

(A.12)

where ci = cos(i) and si = sin(i). By combining (A.11) and (A.12) the relationbetween euler angles and quaternions can be derived. These are described in (A.13)

φ = tan−1(R12

R11

)= tan−1

(2q1q2 − 2q0q3

2q20 + 2q2

3 − 1

)(A.13a)

θ = −sin−1 (R13) = −sin−1 (2q1q3 + 2q0q2) (A.13b)

ψ = tan−1(R23

R33

)= tan−1

(2q2q3 − 2q0q1

2q20 + 2q2

3 − 1

)(A.13c)

A.3 DynamicsThe derivative of the quaternion has to be conducted to use it in an EKF. Thisis a quite complex procedure, but is done in [10], Appendix B.5. With the use ofthe above listed quaternion algebra [10] reaches the results described in (A.14).In [10] ω is the angular velocities, according to:

ω =

0ωxωyωz

=(

0ω

)

A.3 Dynamics 61

q = 12q ω = 1

2

(−q · ω

q0ω + q × ω

)= 1

2

0 −ωx −ωy −ωzωx 0 ωz −ωyωy −ωz 0 ωxωz ωy −ωx 0

︸︷︷︸

S(ω)

q0q1q2q3

(A.14)

= 12

−q1 −q2 −q3q0 −q3 q2q3 q0 −q1−q2 q1 q0

︸︷︷︸

S′ (q)

ωxωyωz

Appendix B

Complete Dynamic Model

For a complete description of the system the model has to be expanded. Forinstance, any adequate results should involve a bias state. Also, the translationpart of the system has to be included.

B.1 Translation DynamicsThe translation of the body system relative to the world system will be describedby the state vector xtr = (p, v, a)T , where p is the position, v is the velocity anda is the acceleration of the body. The acceleration-derivative, i.e. the jerk of thebody is unknown, so therefore it will be modelled as (process) noise. Hence, thestate equation for the translation part will be: p

va

=

0 I 00 0 I0 0 0

pva

+

00I

w1. (B.1)

Due to the discrete measurements the model has to be discretized. A timediscrete variant of (B.1) is given by: pt+1

vt+1at+1

=

I TI T 2

2 I0 I TI0 0 I

ptvtat

+

T 3

6 IT 2

6 ITI

w1,t (B.2)

B.2 BiasA known problem with gyroscopes is the existence of bias. To derive the anglefrom the gyroscopes, the idea is to integrate the angular velocity. However, if thismeasurement is disturbed with a bias, a drift in the output angle will appear. Oneway to cope with this is to add the bias term as a state in the model and estimateit. The dynamics of the bias is unknown, so its derivative will be modelled as

62

B.3 Total Model 63

noise,b = w3

B.3 Total ModelPut together, the complete model becomes:

pt+1vt+1at+1qt+1bgyrot+1

=

I TI T 2

2 I 0 00 I TI 0 00 0 I 0 00 0 0 S(ω)I 00 0 0 0 I

ptvtatqt

bgyrot

(B.3)

+

0 0 00 0 0TI 0 00 T

2 S′(q)I 0

0 0 TI

w1,t

w2,tw3,t

.

For a proper result a study of the process noise wt is needed. This can be donein the same way as in Section 3.1.1, but instead of taking the output from theaccelerometers and gyroscopes, the differentiated output is studied. Due to thefact that no other sensors have been added, the examination of the measurementnoise earlier made is valid here as well.

Of course the gyroscope measurements could be included in the measurementmodel in Section 3.2.2. Then the straightforward way would be to include theangular velocities, ωt, as a state in the model. In that case the measurementequations would be according to:

yacct = R(q)(awt − gw) + eacct , (B.4)ygyrot = ωt + egyrot . (B.5)

Still, the assumption of no acceleration has to be made. Hence,

yacct = R(q)(−gw) + eacct , (B.6)ygyrot = ωt + egyrot . (B.7)

Institutionen för systemteknikliu.diva-portal.org/smash/get/diva2:354085/FULLTEXT01.pdf · 2010....

Documents

Transcript of Institutionen för systemteknikliu.diva-portal.org/smash/get/diva2:354085/FULLTEXT01.pdf · 2010....