Tuning for Ride Quality in Autonomous Vehicle

UPTEC F15030

Examensarbete 30 hp Juni 2015

Tuning for Ride Quality in Autonomous Vehicle

Application to Linear Quadratic Path Planning Algorithm

Jenny Eriksson

Lars Svensson

Teknisk- naturvetenskaplig fakultet UTH-enheten

Besöksadress:

Ångströmlaboratoriet Lägerhyddsvägen 1 Hus 4, Plan 0

Postadress:

Box 536 751 21 Uppsala

Telefon:

018 – 471 30 03

Telefax:

018 – 471 30 00

Hemsida:

http://www.teknat.uu.se/student

Abstract

Tuning for Ride Quality in Autonomous Vehicle

Jenny Eriksson, Lars Svensson

When introducing autonomous functionality in personal vehicles the ability to control the quality of the ride is transferred from the driver to the vehicle control system. In this context, a reference method for quantifying ride quality may be a useful tool in the development and tuning process.

This master’s thesis investigates whether general quantitative measures of ride quality can be of value in the tuning of motion controllers for autonomous vehicles. A set of tools is built for a specific case study, analyzing a lateral path planning algorithm, based on a finite horizon linear quadratic tracking controller, and how its tuning affects ride quality performance.

A graphical user interface is built, with functionality for frequency domain analysis of the path planning algorithm, individually and in combination with the remaining lateral control system, as well as ride quality evaluation based on lateral acceleration data, from logged test runs and simulation results. In addition, a simulation environment for the lateral control system is modified to be used in combination with the evaluation tool. Results of the case study indicate a measurable difference in ride

quality performance when comparing manual and autonomous driving with the current implementation. Attempts were made to improve ride quality by re- tuning the path planning algorithm but little or no improvement from the previous tuning was made.

The work has recognized the potential of using ride quality measures in the development and tuning process for autonomous vehicles as well as devising a tuning strategy incorporating frequency analysis and ride quality evaluation through simulation for the lateral control system. To further increase ride quality performance via the path planning algorithm an altered controller structure, such as a frequency weighted linear quadratic controller is suggested.

ISSN: 1401-5757, UPTEC F15030

Examinator: Tomas Nyberg

Ämnesgranskare: Thomas Schön

Handledare: Mohammad Ali

Sammanfattning

Vid inf¨ orandet av autonoma personbilar ¨ overf¨ ors ansvaret att ˚ astadkomma en behaglig k¨ orupplevelse fr˚ an f¨ oraren till bilens styrsystem, med h¨ oga krav p˚ a komfort som f¨ oljd.

Syftet med denna studie ¨ ar att integrera metoder f¨ or att utv¨ ardera ˚ akkomfort direkt i al- goritminst¨ allningen f¨ or autonoma bilar. Detta har gjorts via utveckling av ett verktyg som specifikt har tagits fram f¨ or en lateral v¨ agplaneringsalgoritm baserad p˚ a linj¨ arkvadratisk servoreglering, LQT.

Verktyget inneh˚ aller tre separata funktionaliteter: simulering, komfortutv¨ ardering och frekvensdom¨ ansanalys. Simuleringsmilj¨ on har tagits fram f¨ or att generera r¨ orelsedata vid givna referenssituationer. R¨ orelsedatat har senare kunnat anv¨ andas i verktygets komfort- utv¨ ardering. Det best˚ ar av ett flertal metoder f¨ or evaluering av komfort och illam˚ aende och

¨

ar resultatet av en litteraturstudie. Verktygets tredje funktion, frekvensdom¨ ansanalysen, ger en m¨ ojlighet att direkt kunna avg¨ ora hur olika frekvenser f¨ orst¨ arks och d¨ ampas i det laterala styrsystemet. Via det sammansatta verktyget har tydliga slutsatser kunnat dras om hur olika inst¨ allningar p˚ averkar bilbeteende.

Tester i bil visade en m¨ atbar skillnad i komfort och ˚ aksjuka n¨ ar man j¨ amf¨ or autonom och manuell k¨ orning. F¨ ors¨ ok gjordes f¨ or att f¨ orb¨ attra den nuvarande inst¨ allningen, utan framg˚ ang. Vidare unders¨ okning antydde att de modeller som anv¨ andes i verktyget var otillr¨ ackliga. F¨ or att avg¨ ora hur v¨ al strategin i sig fungerar beh¨ ovs fler tester i bil samt b¨ attre indata vid simulering.

Studien har uppm¨ arksammat potentialen i att anv¨ anda komfortm˚ att i utvecklingen och

inst¨ allningen av algoritmer f¨ or autonoma bilar. En strategi har utformats f¨ or hur man

kan anv¨ anda frekvensdom¨ ansanalys och komfortevaluering av simulerad data i inst¨ all-

ningsprocessen. F¨ or vidare f¨ orb¨ attring av komfort via v¨ agplaneraren f¨ oresl˚ as en ny regu-

latorstruktur med st¨ orre designfrihet, till exempel en frekvensviktad linj¨ arkvadratisk reg-

ulator (FSLQ)

Acknowledgements

First of all we would like to thank Volvo for giving us the opportunity to carry out this master’s thesis work. It has been a great pleasure to take part in such high-technology development. A special thanks goes out to Mohammad Ali, our supervisor at Volvo who came up with the idea for this thesis and has provided us with many theoretical insights within control theory. It is safe to say that we have learned a lot. The friendly atmosphere at Decision and Control has encouraged us to reach out for help and second opinions from other colleagues within the department, we are sincerely grateful for you taking the time even when our projects were quite separated. Furthermore we would like to thank our supervisor at Uppsala University, Thomas Sch¨ on, for all valuable comments and guidelines for improving the quality of this thesis. Hans Norlander was also very kind to thoroughly read the report and give constructive feedback which was much appreciated in the process of refining the report.

Finally, we would like to add how tremendous it has been to spend the last few months with our new friends in Gothenburg. We were told that there is nothing like a spring in Gothenburg and we quite agree.

Uppsala, June 2015

Jenny and Lars

Notation

z , Reference vector for path planning algorithm u , System control signal vector

x , State vector in path planning algorithm y _T /y _P , lateral position of trail/path

Ψ _T /Ψ _P , Heading of trail/path Ψ ˙ T / ˙ Ψ P , Yaw rate of trail/path

κ T , Curvature of trail

V x , Longitudinal velocity in vehicle’s coordinate system δ wheel,des , Desired front wheel steering angle

δ _wheel , Front wheel steering angle Ψ ˙ _inj , Desired yaw rate of vehicle

∆y , Lateral error with respect to desired path

∆Ψ , Yaw angle error with respect to desired path y _L2 , Lateral error at lookahead point

L _{ef f} , Look ahead distance Ψ ˙ , Yaw rate of vehicle

Ψ ˙ inj , Disturbance on yawrate error with respect to desired path

∆y inj , Disturbance on lateral error with respect to desired path Y , Global position of car

x , Longitudinal axis y , Lateral axis z , Vertical axis

a _{i,W j} (t) , Frequency weighted acceleration time series

a _{i,W j ,rms} , RMS of frequency weighted acceleration time series a i,W j ,P 95 , 95ht percentile of frequency weighted acceleration

T f , Time range of acceleration data

W b , Weighting function for ISO 2631-4 comfort measure W c , Weighting function for CEN 12299 comfort measure W _d , Weighting function for ISO 2631-1 comfort measure

W _f , Weighting function for ISO 2631-1 motion sickness measure M SDV , Motion sickness dose value, ISO 2631-1

K _m , Gain factor for motion sickness dose value

N _{V A} , Mean comfort complete method score CEN 12299 UIC rq note , Ride quality score of UIC’s combined method

¨

y 50 , 50th percentile of lateral acceleration data

Abbreviations

AD Autonomous driving M D Manual Driving

LQR Linear quadratic regulator

LQT Linear quadratic tracking controller

F SLQ Frequency shaped linear quadratic controller

SP AS Extensive simulation environment used for validation M SDV Motion sickness dose value

P SD Power spectral density

F F T Fast Fourier transform

SISO Single input single output

Contents

1 Introduction 1

1.1 Background . . . . 1

1.2 Objectives . . . . 2

1.3 Contributions . . . . 2

1.4 Thesis outline . . . . 3

2 Ride Quality Evaluation 4 2.1 Introduction to ride quality . . . . 4

2.1.1 Definitions . . . . 4

2.1.2 Limitations . . . . 5

2.1.3 Published work . . . . 5

2.2 Description of evaluation methods . . . . 5

2.2.1 Maximum values of acceleration and jerk . . . . 6

2.2.2 Frequency content of acceleration . . . . 6

2.2.3 ISO 2631 . . . . 7

2.2.4 UIC ride quality note . . . . 9

2.2.5 Summary of evaluation methods . . . . 9

2.3 Validation of methods for autonomous vehicle . . . 10

2.4 Usability . . . 12

3 System description 14 3.1 Path planning algorithm . . . 15

3.1.1 Input data . . . 15

3.1.2 Vehicle model . . . 16

3.1.3 LQT control algorithm . . . 17

3.1.4 Initiation . . . 19

3.2 Lateral controller . . . 19

4 Simulation of the lateral control system 20 4.1 Vehicle model in simulation environment . . . 20

4.2 Implementation . . . 21

4.3 Validation . . . 22

4.4 Usability . . . 22

5 Frequency domain analysis 24

5.1 Approximating the path planning algorithm . . . 25

5.1.1 Reduced LQT . . . 25

5.1.2 LQR . . . 26

5.1.3 Comparison of the approximations . . . 29

5.2 Full lateral system . . . 30

5.2.1 Lateral controller, actuators and car model . . . 31

5.2.2 Connecting the path planner to the remaining lateral system . . . . 31

5.3 Validation . . . 33

5.4 Usability . . . 33

6 Experiments and results 36 6.1 Comparing manual and autonomous driving . . . 36

6.2 Understanding the tuning parameters . . . 37

6.2.1 Effects of altering individual tuning parameters . . . 38

6.2.2 Effects of altering multiple tuning parameters . . . 39

6.3 Evaluation of new tuning sets . . . 44

6.3.1 Frequency analysis of new tuning . . . 44

6.3.2 Simulation with new tuning . . . 45

6.3.3 Vehicle test with new tuning . . . 45

6.3.4 Simulation with new tuning on logged drivable area trail input . . . 47

7 Concluding remarks 50 7.1 Conclusions . . . 50

7.2 Ideas for further development . . . 52

8 References 53 Appendices 55 A Preparation of data 56 A.1 Acceleration and Jerk . . . 56

A.1.1 The Rauch-Tung-Striebel (RTS) formulas . . . 56

A.2 ISO and UIC filters . . . 57

A.2.1 ISO 2631-1 filters . . . 57

A.2.2 ISO 2631-4 filter . . . 58

A.3 UIC ride quality note . . . 59

B Display of graphical tuning tool 60

1

Introduction

When launching autonomous cars the driver goes from full flexibility of influencing the style of driving to almost none. This lays the responsibility of generating comfortable rides on the development engineers. By developing a new strategy for how to tune a path planning algorithm that is based not only on responsiveness but also on ride quality it should be possible to reduce vehicle motion causing for example motion sickness. Through development of a tool to evaluate the algorithm based on these qualities the tuning process to find an optimal trade off between responsiveness and ride quality could be significantly shortened. Rather than having to test the different tunings in the car the developer can immediately determine the impact of modifying the tuning directly in the tool.

1.1 Background

Driver-less vehicles have been present in most people’s lives for decades through the world

of science fiction. However, autonomous vehicles are no longer restricted to fiction, but

something that will in the near future be made available to the public. In fact, the tran-

sition to autonomous driving has been going on for quite some time. Technology like

anti-lock breaking system (ABS), electronic stability control and power steering are all

excellent examples of assisted driving taking us towards full automation. Development

within driver-less vehicles goes back to the 1920’s when an empty car was placed in traffic

in Milwaukee, controlled through radio waves from a following vehicle. This was of course

not left unnoticed by the local paper, warning the people of Milwaukee of this car ”haunt-

ing the streets” [23]. It seems however as the daunting message did not scare people off,

in 1987 the German engineer Ernst Dickmanns and his team successfully drove the first

automated van on an empty highway in speeds up to 96 km/h. The development of the

robotic car, under the name VaMoRs, continued to advance, allowing for lane changes and

sensing of other vehicles [24]. Nowadays, all major car manufacturers are developing au-

tonomous cars, with the expectations to soon have them commercialised. There is already technology on the market for collision avoidance, adaptive cruise control and lane keep- ing assist. The next step facing most manufacturers, as defined by the National Highway Traffic Safety Administration in America [21], is to have a vehicle operating autonomously under certain conditions, with a present driver required to retake control in a ”sufficiently comfortable transition time” when needed. At Volvo, a project named DriveMe was initi- ated in 2014 with the aim to fulfill this level of autonomy by having 100 cars put in traffic in the city of Gothenburg by 2017 [22]. The last step towards full autonomy is a vehicle performing all safety-critical driving functions, provided navigation inputs, both occupied and unoccupied.

The possible benefits of autonomous driving are many, including less car accidents, less pollution, reduced traffic congestion and less space needed for infrastructure. However, once the technology is developed for having autonomous cars out in traffic the challenge still remains to convince the people of the advantages. Most importantly the passenger needs to feel safe and comfortable. When he or she has little or no direct influence on the control of the car, it is of vital importance that the autonomous system is set to generate movements that are perceived as pleasant. Therefore, studying effects of vehicle mo- tion on human beings might be crucial for the car manufacturers developing autonomous cars.

1.2 Objectives

The main objective of this thesis has been to develop a new strategy for tuning of a lateral path planner used in autonomous cars at Volvo. The idea was to put greater emphasis on the ride quality caused by the tuning rather than solely considering tracking properties. A MATLAB tool was to be developed that could evaluate for these characteristics. Therefore the thesis was organized into three areas of focus:

• Literature study on how to evaluate ride quality from motion data.

• Development of simulation environment to enable generation of system response for different road scenarios.

• Frequency domain analysis of the path planning algorithm and its effect on the vehicle response.

The idea was that when integrating these analyses in one tool it would be possible to make a decision on tuning parameters from inspection of both frequency domain characteristics as well as ride quality performance of simulated runs.

1.3 Contributions

The main contribution of this thesis is a conceptual strategy for incorporating quantified ride quality measures in the tuning process of motion controllers in autonomous vehicles.

The strategy utilizes classical control theory methods as well as batch simulation to find

tuning parameter sets that will evade undesired frequency content in the vehicle acceler- ation if possible. Applying the methods to frequency weighted control algorithms would be an interesting continuation.

1.4 Thesis outline

The thesis is organized into six chapters excluding this one. Chapter 2 defines the term

ride quality and introduces methods of evaluating it. The lateral control system, which

the path planning algorithm is part of, is described in Chapter 3 to supply the reader with

background on how the current system works. This system was modelled and implemented

as a simulation environment, which is described in Chapter 4. In Chapter 5 the methods

of deriving the mathematical expressions of the path planner system and the full lateral

system are presented and evaluated. This chapter contains a discussion of how this part

of the tool was useful. In Chapter 6 methods and results of using the tuning tool are

presented and discussed. The discussion of the performance of the tuning tool continues

in Chapter 7 where also some ideas for further work are presented.

2

Ride Quality Evaluation

The main objective of this thesis has been to find methods for incorporating ride quality evaluation in the tuning process of the lateral path planner. Several methods for evaluating ride quality have been found and compared for application to autonomous cars. The following section introduces the term ride quality and methods for evaluating it, as well as some initial results on logged data.

2.1 Introduction to ride quality

A driver feeling discomfort can easily adjust his or her style of driving to feel more at ease. At the introduction of autonomous functionality this ability is transferred to the car control system. In order to achieve a comfortable ride in an autonomous vehicle, methods of evaluating ride quality are required. Several studies exists that have been made to try to map different parameters to a perceived ride quality. Even though it is a highly subjective measure literature states that it is possible to trace ride quality perception to certain parameter values.

2.1.1 Definitions

Ride quality is a term describing a person’s subjective perception of a vehicle ride. Poor

ride quality gives rise to passenger discomfort and sometimes motion sickness. Good ride

quality is associated with passenger comfort and small risk of motion sickness. Comfort

is a general feeling of well-being while motion sickness is associated with dizziness, fatigue

and nausea.

2.1.2 Limitations

There are many parameters that can affect ride quality. Besides motion variables param- eters like temperature, posture and smell can have have a significant impact. The level of discomfort and motion sickness is also dependent on the individual. For example it has been shown that there is a noticeable difference when comparing both gender and age. In this thesis however only the effect of motion variables will be considered since the other parameters are not affected by the maneuvring of the vehicle. Moreover, impact from motion in the lateral direction will be the main focus since this thesis is regarding a lateral path planner.

2.1.3 Published work

There is not much published material within ride quality evaluation for autonomous driv- ing. However, for other means of transportation, a large amount of results can be found.

The public transportation sector has been especially ambitious in publishing reports, since they want to satisfy their customers with an enjoyable ride. There are a few suggestions on how to mathematically determine ride quality from logged motion data. A common method is described in an international standard, ISO2631, which determines the effect of vibrations on the human body [1]. This standard has been further extended by other research groups to include a broader variety of motions. An international project under the name UIC Comfort Tests carried out on behalf of the international Union of Railways, Banverket and Vinnova has performed a regression analysis of perceived ride quality in trains from their own experiments and added that to the ISO2631 method [10]. A research group for public transportation at the KTH Royal Institute of Technology in Stockholm has contributed with an evaluation method for ride quality on buses, which was also developed through regression analysis from experiment data [9].

2.2 Description of evaluation methods

When evaluating ride quality it is important to consider both discrete events and average vehicle motion. Motion sickness is induced by low frequent motions over a longer period of time while a passenger can experience discomfort from a single event, for example an abrupt lane change. As stated by F¨ orsteberg in [4], analysis of peak and average motion data is necessary for a full estimate of ride quality and the methods in this thesis have been chosen thereafter.

Documentation on limits for magnitudes and frequencies of different motion variables have

been collected and are described in Section 2.2.1 and 2.2.2. The international standard

ISO 2631 contains methods for evaluating average comfort which have been widely used

by researchers and vehicle designers, they are described in Section 2.2.3. The standard

can be used individually or in combination with peak values in order to achieve a more

complete ride quality estimate, described further in Section 2.2.4.

2.2.1 Maximum values of acceleration and jerk

Studies show that it is acceleration and its time derivative, jerk, that affect ride quality.

Jerk typically arises at swift lane changes and entrances and exits of curves. A high value of acceleration or jerk can cause discomfort even during shorter periods of time. When the levels get too high the passenger will find it difficult to maintain posture. Therefore it is advised to set restrictions on magnitudes of acceleration and jerk. Limit values vary between the studies but they fall within the same range. Most of these studies are carried out on behalf of the railway industry but since motion has the same impact on a passenger regardless of the type of vehicle these sources are considered relevant.

For acceleration the threshold for discomfort lies around 1 m/s ² , ranging up to 1.47 m/s ² in the study of Baybura et al, [8]. According to Kottenhoff, [9], bus traffic usually accepts higher levels, up to 2 m/s ² in order to follow the rhythm of the traffic.

The jerk threshold for discomfort lies around 0.5 m/s ³ , ranging up to 0.9m/s ³ as seen in [8] and [4].

Some of these thresholds are however determined by including both seated and standing passengers. A standing passenger in a train or on a bus has more difficulty to keep balance through high values of acceleration and jerk than a seated passenger. Since an automobile only carries seated passengers it is expected that the thresholds should be set on the higher side. For acceleration it might be reasonable to set the limit closer to 2 m/s ² and jerk to 0.9 m/s ³ .

2.2.2 Frequency content of acceleration

Low frequent motion is the main contributor to motion sickness while high frequent motion causes stress and discomfort. Modern roads and car suspension systems are designed not to give rise to high frequent motion but low frequent behaviour may arise for example during repeated lane changes or breaking maneuvres. The frequency content of acceleration data can be determined by estimation and inspection of its spectrum. For this thesis the power spectral density, PSD, has been used.

For motion sickness, horizontal acceleration in the frequency range 0.1-0.5 Hz has the most effect [13], [12]. Within this range 0.2 Hz is specifically pointed out in several studies, for example in [4], [7] and [5]. It also seems that lateral movement has greater impact on motion sickness than longitudinal movement [5].

Higher frequencies have a negative impact on a passenger’s ability to read and write.

Experimental studies in a lab environment conducted by Sundstr¨ om et. al, [11], have shown that for lateral vibration 2.5 Hz is critical for passengers leaning back in the seat while 4-5 Hz is critical when leaning forward without back support. Another experimental study by Hayward et. al, [6], shows that for longitudinal vibration, also known as fore-and- aft motion, 4 Hz has the greatest impact on a passenger leaning back in the seat.

One explanation for the connection between discomfort and spectral content is that the

human organs are put under stress when falling into resonance. The ear’s balance organ

undergoes resonance at frequencies below 1 Hz which might be the reason for motion sickness [9].

2.2.3 ISO 2631

ISO 2631 describes ways to evaluate vibration exposure to the human body. It defines methods for measurement of vibrations as well as how to process measurement data to standardized quantified performance measures concerning health, perception, comfort and motion sickness. The standard is applicable to situations in vehicles, buildings and in the vicinity of machinery. ISO 2631-1, [1], describes general methods and the related standard documents ISO 2631-2 through ISO 2631-5 describes the specifics of different applications of the methods. ISO 2631-4, [2], specifically describes the application of the ISO 2634-1 methods to railway vehicles, which was also relevant for this study.

The quantified performance measures of ISO 2631 are based on frequency weighted root mean square, RMS, computations of acceleration data. A performance measure for the lateral direction can be calculated as

a _{y,W i ,rms} =  1 T f

Z T f

0

a ² _{y,W i} (t)dt



1 2

, (2.1)

where T _f denotes the time range of the measurement, W _i denotes a frequency weighted function, a y,W i (t) denotes filtered lateral acceleration and a y,W i ,rms denotes the root mean square of a y,W i (t) which is the score value of the method.

Each performance measure is related to a specific frequency weighting function. For this study three performance measures from ISO 2631 have been focused. Table 2.1 displays the notation of the measures and their corresponding frequency weighting functions. Am- plitude responses of the weighting functions are displayed in Figure 2.1.

Table 2.1: Notation for performance measures and frequency weighting functions for ISO 2631 Performance measure notation frequency weighting function

ISO 2631-1 a _{y,W d ,rms} W _d

ISO 2631-4 a y,W b ,rms W b

MSDV M SDV W _f

The ISO 2631-1 comfort note is a general comfort performance measure while the ISO

2631-4 comfort note was specifically developed for rail vehicle applications. MSDV is

an abbreviation of motion sickness dose value and is a measure of the likelihood of nau-

sea.

10

10

10

10

−40

−35

−30

−25

−20

−15

−10

−5 0 5

Magnitude (dB)

Weighting filter amplitude responses

Frequency (Hz)

Wd Wb Wf Wc

Figure 2.1: Amplitude responses of weighting filters in ISO 2631 and CEN 12299

The MSDV is not computed as a generic frequency weighted RMS as in (2.1). Instead, it is calculated as

M SDV =

Z T f

0

a ² _{i,W f} (t)dt



1 2

.

Thus the MSDV is accumulated over time, in correspondence with how most people ex- perience motion sickness. However, to compare acceleration data of different time ranges it may also be useful to evaluate the mean MSDV-rate, M SDV /T f , which can be calcu- lated directly as RM S(a y,W _f (t)). This measure is independent of the time range of the measurement.

The standard provides approximate, likely reactions in terms of comfort to various values of the ISO 2631-1 comfort note, presented in Table 2.2. For the ISO 2631-4 comfort note, the standard only provides a method of measurement and analysis. Application specific experiments need to be conducted to get reference values.

Table 2.2: Likely reactions to various weighted RMS accelerations, specified in ISO 2631-1 a i,W d ,rms ≤ 0.32 not uncomfortable

0.32 ≤ a i,W _d ,rms ≤ 0.63 a little uncomfortable 0.50 ≤ a i,W d ,rms ≤ 1.00 fairly uncomfortable 0.80 ≤ a i,W d ,rms ≤ 1.60 uncomfortable 1.25 ≤ a i,W _d ,rms ≤ 2.50 very uncomfortable 2.00 ≤ a i,W d ,rms extremely uncomfortable

ISO 2631 only provides guidelines for interpretation of the M SDV for the vertical direc- tion, M SDV z . According to [1], the percentage of people who may vomit is K m · MSDV ^z , where K _m = 1/3, for a mixed group of test subjects. However, F¨ orstberg provides an interpretation of the M SDV for the lateral direction in [3]. Replacing K m = 1/3 with K m = √

2, makes the lateral dose value equivalent to that of the vertical dose value.

ISO 2631 recommends acceleration measurements conducted on the surfaces supporting a person’s weight. For this study however, measurements have been made using sensors built into the car body, thus assuming that the seats accelerate in sync with the rest of the car.

The methods described in ISO 2631 are applicable to several types of vehicles but only for fairly straight lines of motion. Due to the fact that the measures are evaluated as time averages some situations require additional methods to evaluate for transient motions motivating complementary analysis as concluded by Lauriks in [10].

2.2.4 UIC ride quality note

Since ISO 2631 is primarily developed for straight roads further research has been required to get an evaluation method covering more road scenarios. By extending the standards with absolute measures of acceleration and jerk curvaceous roads can be included in the evaluation.

One thorough study, [10], commissioned by the International Union of Railways (UIC), has done a regression analysis of ride quality for several motion parameters for 38 people on 64 different rides. The best fitting equation gives a score in the interval 0-5, with 5 corresponding to a highly uncomfortable ride, according to

UIC rq note = 0.094N V A + 0.6¨ y 50 + 0.37 ...

y ₅₀ + 0.11 ˙θ, where ¨ y is the lateral acceleration and ...

y is the lateral jerk of the car body. ¨ y 50 denotes the 50th percentile of lateral acceleration. ˙θ is the angular velocity but since roll motion is negligible in cars the term can be excluded. N _{V A} is from a European standard named CEN 12299. It is described according to

N V A = 4 · a z,W b ,P 95 + 2 · q a ² _y,W

d ,P 95 + a ² _z,W

b ,P 95 + 4 · a x,W c ,P 95

where a i,W j ,P 95 denotes the 95th percentile of acceleration data along the i axis filtered with the weighting function W j . The frequency weights, W j , for CEN 12299 are the same as for ISO 2631 (W _d and W _b ) with the addition of W _c . The amplitude response of all the weighting filters are illustrated in Figure 2.1.

2.2.5 Summary of evaluation methods

To evaluate local comfort the methods that should be used are maximum acceleration and maximum jerk. For average comfort ISO 2631 comfort note, UIC ride quality note and the PSD method should be evaluated. Risk of nausea should be evaluated with MSDV and the PSD method.

Acceleration and jerk are the two motion variables affecting ride quality. To minimize

risk of motion sickness frequencies in the range of 0.1-0.5 Hz should be suppressed. Local

discomfort typically arises when the acceleration reaches 2 m/s ² or when the jerk reaches 0.9 m/s ³ . The average ride quality can be determined by using the standard ISO 2631 or the UIC ride quality note which evaluates the acceleration data over time.

2.3 Validation of methods for autonomous vehicle

Prior to using the methods for ride quality evaluation to tune the LQ algorithm they needed to be validated. Acceleration was logged for a set of different driving situations where it was known whether the methods should yield a high or low score on ride quality.

This was compared to the passengers subjective perception of the ride.

The lateral acceleration profiles for the driving situations can be viewed in Figure 2.2.

They were chosen to test the methods sensibility to high levels of acceleration and jerk, oscillating driving and fairly straight driving. The data length is short since the strategy was to single out specific events that should have an impact on the methods. Some of the data is therefore not representative of a normal ride but should merely be regarded as a means to validate whether the methods functioned as designed.

time (s)

0 5 10

acceleration (m/s 2 ) -5

0 5

Acceleration data for slow lane change

time (s)

0 5 10

acceleration (m/s 2 ) -5

0 5

Acceleration data for swift lane change

time (s)

0 20 40

acceleration (m/s 2 ) -5

0 5

Acceleration data for sinus road

time (s)

0 100 200 300

acceleration (m/s 2 ) -5

0 5

Acceleration data for straight line

Figure 2.2: Log data used for validation of ride quality measures.

The results of applying the different ride quality measures on log data from the four

different driving situations are displayed in Table 2.3 and Figure 2.3. The passengers sitting

in the car when collecting the log data perceived the slow lane change as comfortable, the

swift lane change as very uncomfortable and the straight road driving as very comfortable.

The sine road driving was perceived as slightly nauseating, possibly more nauseating if it had been carried out over a longer period of time.

Table 2.3: Score of ride quality methods applied to log data for validation, presented in section 2.3

Method slow lane change swift lane change sine road straight road

Max acc (m/s ² ) 2.0 4.5 1.4 0.6

Max jerk (m/s ³ ) 3.5 10.9 1.3 0.3

ISO 2631-1 comfort 0.4 1.2 0.2 0.1

ISO 2631-4 comfort 0.1 0.5 0.1 0.1

MSDV 10min (%) 19.4 34.1 21.8 1.6

UIC rq note 2.5 8.5 1.4 0.3

frequency (Hz)

0 0.5 1 1.5 2

PSD (G 2 /Hz)

0 0.2 0.4 0.6 0.8

1 PSD for slow lane change

(a)

frequency (Hz)

0 0.5 1 1.5 2

PSD (G 2 /Hz)

0 0.5 1 1.5 2 2.5 3

3.5 PSD for swift lane change

(b)

frequency (Hz)

0 0.5 1 1.5 2

PSD (G 2 /Hz)

0 0.5 1 1.5

2 PSD for sinus road

(c)

frequency (Hz)

0 0.5 1 1.5 2

PSD (G 2 /Hz)

0 0.2 0.4 0.6 0.8 1 1.2 1.4

1.6 PSD for straight road

(d)

Figure 2.3: PSD of log data used for validation of ride quality measures.

The results show which methods that are reasonable to use for ride quality evaluation.

The maximum values for acceleration and jerk found in the literature study, 2 m/s ² and

0.9 m/s ³ respectively, are low in comparison to the logged validation data. The slow

lane change had a maximum acceleration of 2 m/s ² and was perceived as comfortable,

its corresponding jerk was 3.5 m/s ³ . However lane changing differs from lane following

and should naturally allow for higher values since it is a short maneuver that is often considered necessary. The sine road driving, where the car was kept in the lane, show more reasonable values, 1.4 m/s ² for acceleration and 1.3 m/s ³ for jerk. Therefore, the recommended maximum values could probably be used quite successful for lane keeping but for lane changes other studies need to be consulted in order to find better maximum values.

Both ISO2631-1 comfort and UIC ride quality note are developed to estimate average comfort. They share the same weighting curve W _d to filter lateral acceleration which will retain the frequency span 0.4-0.7 Hz. This will exclude the low frequency span causing nausea. UIC therefore has two additional terms, ¨ y 50 and ...

y ₅₀ which will work as to preserve impact of low frequent dynamics, for example curvaceous roads. Therefore the UIC ride quality note is designed to consider a larger span of motion affecting ride quality. There is little confirmation of this in the results, however, with only a minor indication that the UIC ride quality note gives a higher punishment on the slow lane change and sine road driving than ISO2631-1. Therefore it is suspected that the methods will yield similar results. Another result to enlighten is that the UIC ride quality note is specified on the interval 0-5 but has a value of 8.5 on the swift lane change. It is assumed that 5 is not the maximum but a threshold for very poor ride quality.

ISO2631-4 comfort yielded very low scores on all log data. This is due to the filtering function which will only retain the frequency span 3.4-18 Hz. Frequencies this high are suspected to be caused by road and car conditions rather than the style of driving. They have not been present in the Volvo log file database, since modern cars have been used on smooth roads. This will continue to be the case and therefore it was decided to not use this method when evaluating comfort.

The MSDV method gave expected results from all test cases. The swift lane change, with highest intensity in the interval 0.1-0.5 Hz resulted in a 34% likelihood of vomiting while the straight road driving gave the percentage 1.6%. That the percentage is high for the lane changes and sine road driving is because the method assumes the acceleration profile to be kept for the full ten minutes, rather than the actual length of the data.

The PSD method, evaluated using the Welch method described in [16] gave distinct spectra for all four driving situations. By inspection of Figure 2.3 it is seen that the sine road gave the predicted distinct peak at 0.2 Hz in Figure 2.3a and that the swift lane change contained more of the higher frequency components than the slow lane change, see Figure 2.3b and Figure 2.3c. It is also clear in Figure 2.3d that the straight road driving had no high-frequent impact at all. From this it can be concluded that inspection of the PSD is a powerful method for evaluation of ride quality

2.4 Usability

Due to limited amount of time it was outside the scope of this thesis to properly validate

how well the different ride quality evaluation methods performs on autonomous drive. It

is likely that the recommended values for each method should be adjusted to fit the field

of use. For the purpose of this thesis, it has still been possible to compare how different

tunings have either increased or decreased the ride quality. Therefore, they were useful in distinguishing between impact of different tunings, rather than evaluating the actual score. A summary of when to use which method can be viewed in Table 2.4.

Table 2.4: Applicability of methods for ride quality evaluation Method local comfort average comfort nausea

Max acc X

Max jerk X

ISO 2631-1 comfort X

MSDV X

UIC ride quality note X

PSD method X X

3

System description

This chapter aims to put the path planning algorithm in a context by describing the lat- eral control system in its separate components. Development in the Drive Me project is divided into seven areas: sensing system, sensor fusion, decision and control, architecture and system solution, human factors, dependability, vehicle build and integration and ver- ification. Of these, sensing system, sensor fusion and decision and control are directly related to the car control system.

The sensing system development area deals with evaluating and integrating feasible sensing solutions for the car. The current sensing system includes cameras, radars, laser sensing systems and GPS. Sensor fusion deals with the problem of extracting relevant information such as map position, vehicle state, road lanes, other vehicles, pedestrians etc. from sensor data. The decision and control development area contains the longitudinal and lateral control systems of the car as well as tactical decision making such as when to do lane changes and setting reference speed. This thesis work belongs to the decision and control development area and specifically deals with the lateral control system.

The lateral control system concept consist of two major components, the path planning algorithm and the lateral controller. In general terms the purpose of the path planning algorithm is to continuously transform a road geometry into a kinematically feasible path that the car can follow. The path is then passed to the lateral controller which calculates what steering action is to be taken in order for the car to follow the path. A simplified overview of the lateral control system is illustrated in Figure 3.1.

Although this thesis work is focused on analysing the behaviour of the path planning

algorithm, the dynamics of the lateral controller and the vehicle as well as the sensor

fusion output, must be taken into account in order to see how the tuning of the algorithm

affects vehicle motion.

Figure 3.1: Illustration of lateral control system

3.1 Path planning algorithm

The purpose of the path planning algorithm is to compute a kinematically feasible tra- jectory for the car to follow given a trail from sensor fusion. This is done by propagating a car model along the trail under the influence of an optimal control algorithm. The trajectory of the car model is then optimal with respect to a cost function and desirable path properties may be acquired by tuning the control algorithm. The reason for choosing this concept instead of a filtering or optimization approach is to enable use of analysis tools from the control theory domain. In theory, this will allow a linear time invariant system description for the full lateral control system. The application of optimal control to reference trajectory generation is described in [19].

3.1.1 Input data

The input data for the path planning algorithm comes from sensor fusion on a standard- ized format called a trail. A trail is described by trajectories in lateral position, y T (k), tangential angle, Ψ _T (k), and curvature, κ(k), as discrete functions of longitudinal distance in front of the vehicle. The trail starts in x(1) = 0 in the car coordinate system and ends at x(k _f ) = V x t _h where t _h denotes a time horizon for the path planning and k _f denotes the final spatial index. The trail used in autonomous mode for lane driving is called the

”drivable area trail” and describe the middle of a lane.

The trail data is used as the reference signal z(k) for the path planner control algorithm, with the curvature data translated to a reference signal in yaw rate for the path vehicle model state,

z(k) =



 y T (k) Ψ _T (k) Ψ ˙ _T (k)



 =



 y _T (k) Ψ T (k) κ T (k)V x



 . (3.1)

Sensor inaccuracy may result in noisy signals and inconsistency between trail states, mo-

tivating the use of the path planning algorithm.

3.1.2 Vehicle model

A three state vehicle model inspired by [20] is used in the path planning algorithm. The model is complex enough to catch the dominating lateral dynamics of a vehicle but is limited to three states to limit computational complexity. The basis for the vehicle model is the differential equation

˙y = V _x tan(Ψ) ≈ V x Ψ, (3.2)

which is derived from the mechanical situation of Figure 3.2.

x y

V

V x

˙y

Ψ

Figure 3.2: Simplified lateral dynamics relations used in the car model of the path planning algorithm

Under the assumption that the yaw acceleration is controlled, u(t) = ¨ ψ(t), a continuous time state space model based on (3.2) can be set up as

˙x(t) = A _ct x(t) + B _ct u(t), y(t) = C _ct x(t),

where the path state vector is

x(t) =



 y _P (t) Ψ P (t) Ψ ˙ P (t)



 , (3.3)

and the matrices are given by

A _ct =





0 V x 0

0 0 1

0 0 0



 , B _ct =



 0 0 1



 , C _ct =





1 0 0 0 1 0 0 0 1



 .

Assuming constant longitudinal velocity allows a description in space rather than time,

δ

δs x(s) = A _cs x(s) + B _cs u(s), y(s) = C cs x(s),

where s is the spatial variable and the matrices are given by

A _cs =





0 1 0

0 0 _V ¹

x

0 0 0



 , B _cs =



 0 0

1 V x



 , C _cs =





1 0 0 0 1 0 0 0 1



 .

The model is finally discretized with spatial indices k, yielding a system description dis- cretized in space just like the trail input data,

x(k + 1) = Ax(k) + Bu(k), y(k) = Cx(k),

with the system matrices

A =





1 dx 0 0 1 _V ^dx

x

0 0 1



 , B =



 0 0

dx V x



 , C =





1 0 0 0 1 0 0 0 1



 , (3.4)

where dx is the distance between trail samples.

3.1.3 LQT control algorithm

The controller used in the path planning algorithm is a discrete time, finite horizon, linear quadratic tracking controller, LQT, inspired by [14]. The controller structure allows for an analytical solution to the Ricatti Equation, which is preferred in safety critical systems, but it does not allow for constraints in states or control signals.

The controller is designed to minimize the cost functional

J(k 0 ) = 1

2 (Cx(k f ) − z(k f )) ^T F (Cx(k f ) − z(k f )) + 1

2

k f −1

X

k=k 0

((Cx(k) − z(k)) ^T Q(Cx(k) − z(k)) + u ^T (k)Ru(k)),

(3.5)

over a finite horizon k = [1, k f ]. A, B and C refers to the space discretized vehicle model

system matrices, (3.4). z(k) denotes the reference signal described in (3.1). x(k) and u(k)

refers to the state vector and the control signal respectively. Q, F and R are weighting

matrices used to tune the controller. Tuning the controller allows system designers to manipulate properties of the output path.

The minimization of (3.5) is achieved by solving the Ricatti difference equation P (k) = A ^T P (k + 1)(I + EP (k + 1)) ⁻¹ A + V,

V = C ^T QC, E = BR ⁻¹ B ^T ,

(3.6)

and the co-state difference equation

g(k) = M g g(k + 1) + W z(k),

M g = A ^T (I − (P ⁻¹ (k + 1) + E) ⁻¹ E), W = C ^T Q,

(3.7)

in the reverse direction, from the initial conditions P (k _f ) = C ^T F C,

g(k _f ) = C ^T F z(k _f ). (3.8)

The optimal state feedback, L(k), and feedforward, L g (k), can then be computed as L(k) = (R + B ^T P (k + 1)B) ⁻¹ B ^T P (k + 1)A,

L g (k) = (R + B ^T P (k + 1)B) ⁻¹ B ^T

Note that the resulting L and L _g will be discrete functions of k, as opposed to the infinite time LQ controller, where they are constant.

The optimal control is given by

u ^∗ (k) = −L(k)x ^∗ (k) + L g (k)g(k + 1), and the optimal state trajectory by

x ^∗ (k + 1) = (A − BL(k))x ^∗ (k) + BL g (k)g(k + 1).

The state trajectory is stored and made available as a reference path for the lateral con- troller.

Tuning of the path planning algorithm is done by altering the diagonal matrices Q in

(3.6), (3.7) and F in (3.8). The diagonal elements of Q acts as weights on the control

error in their respective states in the cost functional, (3.5), so an increase in Q 1 , Q 2 or

Q ₃ will decrease the control error in its respective state. Similarly, the elements of F acts

on the control error at the last spatial index, k f . The scalar input weight R is calculated

dynamically as a linear function of longitudinal velocity.

3.1.4 Initiation

The path must be updated continuously, due to the limited range of the sensors. In the current implementation, the path planner is reinitialized in every iteration of the control algorithm, subsequently at a rate of 40 Hz. The objective is to have the path planner work as a filter on the trail data rather than be part of the controller dynamics. To achieve this, the initiation of the path planner vehicle model has to be independent of the current car position. In the current implementation, the path from the previous time sample is stored and transformed to the current car coordinate system. Then the old path is interpolated at x = 0 in the current coordinate system giving the initial state x(1) for the next path.

The fact there is no feedback from the vehicle state to the path planner input provided a basis for how to set up the frequency analysis of the lateral control system, further described in Section 5.

3.2 Lateral controller

Historically, the reference for the lateral controller has been a one dimensional curve rather than a state trajectory. Therefore, the current lateral controller is designed to take an input of lateral position only. Therefore, only the state trajectory y P (k) of the path is used. Instead of reading the curvature from the path state, the lateral controller estimates it from y P (k) in a preprocessing block. The path is interpolated at the points x = 0 and x = L _{ef f} in the car coordinate system. The lookahead distance L _{ef f} is calculated dynamically as a function of longitudinal velocity. At these points, the lateral offset to the trail is computed and named ∆y and y L2 respectively. y L2 is used to compute an estimate of trail curvature which is used as a feedforward term in the control algorithm. ∆y is the lateral control error for the feedback part of the controller.

X Y

x y

∆y

L ef f

∆y L2

∆Ψ

Figure 3.3: Visualization of the inputs to the lateral controller

4

Simulation of the lateral control system

Several simulation environments of varying levels of complexity are currently in use in the autonomous functionality development group. Relatively simple and limited models are used for development and tuning and complex high dynamic models are used for ver- ification to minimize the amount of necessary physical testing. Normally, a simulation environment is built in Simulink, from blocks of code from the current vehicle imple- mentation, and a vehicle model of varying complexity, depending on the purpose of the environment.

For the simulation part of this thesis work, the goal was to produce an environment where the lateral behaviour of a vehicle model could be analysed in a situation where the user could freely specify a drivable area trail to enable isolated analysis of the lateral control system.

It was decided early on that the only feasible way to produce such a simulation environ- ment in the time available was to branch off from a similar, existing environment. An environment designed for development of collision avoidance path generation was chosen.

It contained the full lateral control system as well as a relatively simple vehicle model described in Section 4.1. The trail input to the path planner was generated by higher level systems and the user input was of a traffic situation. The work of adapting the environment to our purposes consisted of cutting out the higher level system and enable input of a user generated drivable area trail.

4.1 Vehicle model in simulation environment

The lateral controller group has developed a bicycle model similar to that of [15] describing

the lateral movement of the prototype vehicle at a constant longitudinal speed. The vehicle

model inputs δ _wheel and outputs ∆y, the lateral offset from the path, ∆Ψ, the yaw angle

error with respect to the path, y L2 , the lateral offset at the look ahead distance L ef f and Ψ, the yawrate of the vehicle with respect to the path. The outputs are chosen as relevant ˙ inputs for the lateral controller.

In addition to the vehicle model there is a separate model for the the steering actuator.

The steering actuator is a feedback system in itself and thus inputs requested steering angle δ _wheel,des and outputs the actual steering angle δ _wheel .

4.2 Implementation

The work with the simulation environment started with removing functionality that was redundant for our purposes in order to improve simulation speed. Updated versions of the path planning algorithm and the lateral controller functions was inserted directly from the repository of the current test vehicle implementation. Then the new functionality could be added.

vehicle model lateral controller

transform road to car coordinate system

simulate sensor fusion data output

run path planning algorithm

road data in global coordi- nate system

road in car coordinate system

drivable area trail

path vehicle state in

global coordinate system

Figure 4.1: Data flow of the simulation environment

A function generating road geometries represented on the standard trail format in global coordinates was constructed. The user may choose from a set of standard scenarios, use logged drivable area data or use any arbitrary vectors for the road geometry representation.

The road geometry is transformed to the car coordinate system and interpolated at the

points of the regular drivable area x-vector. The result is a road representation on the proper drivable area trail format which can be input to the path planning algorithm. The data flow of the simulation environment is illustrated in Figure 4.1.

4.3 Validation

Since the simulation environment was derived from a previous environment that had gone through an extensive verification process, and had been used in development in the past, it was decided that verifying conformity with another simulation model was sufficient for our purposes.

A lane change scenario was constructed and the result was compared with the output of the extensive SPAS simulation environment. SPAS is a detailed simulation environment of the full autonomous functionality system, normally used for verification and validation purposes. Results are presented in Figure 4.2.

0 50 100 150 200 250 300

−1 0 1 2 3 4 5

Comparison of lane change simulations

X (m)

Y (m)

LatSim SPAS

Figure 4.2: Comparison of car position during a lane change in the lateral simulation environment, denoted LatSim, and SPAS

Some difference in output was expected since the SPAS environment is more complex and contain a more complete description of the vehicle dynamics. It was concluded from this study that the performance of the lateral simulation environment was adequate and could be used in the tuning process.

4.4 Usability

A simulation is run by executing a script where the user may specify tuning parameter set, road scenario, longitudinal speed, simulation time etc. At the end of the script, resulting data is saved to a .mat file on a specific format that may be read by the tuning tool.

The graphical user interface described in appendix B contained functionality to visualize

the trails and paths calculated during the simulation. This functionality enabled analysis

of path properties as well as momentary ride quality for the simulation. Also, it proved valuable in the process of debugging the simulation environment implementation.

To enable analysis of how evaluation quantities varied as a function of a tuning parameter,

several scripts were built were the simulation run command was executed in a loop. These

batch simulation were very efficient in evaluating the impact of tuning parameters on ride

quality measures.

5

Frequency domain analysis

To determine how different sets of tuning parameters in the path planning algorithm affected the lateral system, frequency domain analysis functionality was developed. The focus was to analyze the behavior of the path planner in itself as well as in connection with the remaining lateral system, to see the effect on vehicle dynamics. The second being required since the lateral controller has its own dynamics to translate data from the path planner to an actual reference the car should follow. Therefore, a change in behavior of the path planner does not necessarily affect the behavior of the car. By expressing all components of the lateral system on transfer function form and connecting them to each other the characteristics of these system models could be inspected. The main challenge was to find good model representations of these systems; these approaches are further presented below. The setup for the system is displayed in Figure 5.1, where the lateral system is approximated by connection of the path planner to a vehicle model.

Path planner z(k)

Lateral controller

Actuator model

Vehicle model

Y (k)

Figure 5.1: Block diagram of the full lateral system.

5.1 Approximating the path planning algorithm

The path planning algorithm uses a Linear Quadratic Tracking controller, LQT, as de- scribed in Section 3.1. The LQT controller makes use of the full trail vector when con- structing the path. Therefore, there is a transfer function from each element z(k + N ) in the trail, to each element x(k) in the path, where k+N denotes an element further away on the trail according to N ∈ 0 (k f − k) .

However, the model of the lateral controller used in this thesis only acts on the first element in the path, as further explained in Section 5.2.1. Therefore, only the model of the first output of the path planner is required. Moreover, it would be desirable to only use one input - this way the path planning algorithm is on a comprehensible form and easy to analyze. This approximation can be done if it is verified that it captures the dynamics of the complete path planning algorithm. Two different approximations for the path planning algorithm were implemented and compared, one as a simplification of the LQT controller and one as approximating the LQT controller as a linear quadratic regulator, LQR.

5.1.1 Reduced LQT

The path planning algorithm is a multivariable system of transfer functions, where all of them influence the final path. Since a single-input-single-output (SISO) model was desired, the question arose whether simply picking one of the transfer functions to represent the complete system would yield a satisfying approximation. By inspecting how different elements in the trail influenced an element in the path differently, in other words comparing how z(k + N ) and z(k) influenced x(k) differently, a decision could be made on which transfer function to choose and analyze if that was good enough.

Putting the expressions for the states and co-states presented in (3.7) and (3.1.3) on a combined state space form yields

 g(k) x(k + 1)



=  M g (k) 0 BL _g (k) A − BL(k)

 g(k + 1) x(k)

 + W

0

 z(k),

which will give the transfer function from z(k) to x(k). By further expanding the expres- sion for g(k),

g(k) = M g (k)g(k + 1) + W z _k

= M g (k)(M g (k + 1)g(k + 2) + W z(k + 1)) + W z(k)

= M g (k)(M g (k + 1)(M g (k + 2)g(k + 3) + W z(k + 2)) + W z(k + 1)) + W z(k)

= ...

= (

N

Y

i=0

M _g (k + i))g(k + N + 1) + (

N −1

X

j=0

(

j

Y

m=0

M _g (k + m))W z(k + m + 1)) + W z(k),

and inserting it into the expression for x(k) it is possible to find the transfer function from

z(k + N ) to x(k). Prior to this the matrices L(k),L g (k) and M g (k) need to be determined.

They are time varying and are solved by looping through the Riccati equation for P, as described in (3.6) and (3.8).

Through this procedure it was possible to compare the different transfer functions with the complete path planning algorithm. They were compared by filtering a trail vector, designed as a unit step, through the different reduced LQT models and displayed in the same plot as the actual path from the path planner. Figure 5.2 shows the response for the state y P in the state vector x in four models, y T (1) → y P (1) (LQT1), y T (9) → y P (1) (LQT2), y T (15) → y ^P (1) (LQT3) and y T (23) → y ^P (1) (LQT4).

We see that all models give similar dynamics but with different gain. LQT1 is almost identical to the response from the path planning algorithm besides a delay of approximately 3 seconds. The path planning algorithm has access to the full trail vector when designing the path, which enables a reaction in the path before the step takes place. This non-causal behavior is not possible to mimic in a dynamic system, hence the delay in the reduced LQT models. However, since the main focus of the frequency analysis is to see how different frequencies are amplified, phase distortion can be allowed in the approximation. It was therefore decided to use LQT1 as the approximation, since it has the same gain as the path planner.

time (s)

0 1 2 3 4 5 6 7 8 9 10

y (m)

-0.01 0 0.01 0.02 0.03 0.04 0.05

0.06 Step response, reduced LQT

trail path LQT1 LQT2 LQT3 LQT4

Figure 5.2: Step responses y T → y ^P for four different LQT models compared with the actual path for the given trail. LQT1 is the model for y T (1) → y ^P (1), LQT2 y T (9) → y ^P (1), LQT3 y T (15) → y ^P (1), LQT4 y T (23) → y ^P (1). All have similar dynamics but different gain.

5.1.2 LQR

One approximation of the path planning algorithm, was to construct a system using an

infinite horizon Linear Quadratic Regulator, LQR. An LQR system solves the regulator

problem, which implies that the reference is set to zero. This differs from our path planner

that is a tracking system and solves the servo problem, which implies that the reference

varies, [14]. However, with a trick that will be explained further on the LQR can be

rewritten as a tracking controller. Another difference to the LQT controller is that this

controller is designed to minimize the error to a set point over infinite time rather than

tracking a reference over a fixed time interval. An advantage of this approximation is that

the feedback gain of the controller is time independent, resulting in a much simpler system than for the original path planning algorithm, where we have transfer functions from all elements in the reference to all elements in the path.

The LQR controls the plant through state feedback, where the feedback gain is computed in the steps described in 5.1-5.5. A block diagram of the system is shown in Figure 5.3

r(k) u(k) B ₁

q

x(k + 1)

x(k) C y(k)

A

L

Figure 5.3: Block diagram of a plant with a Linear Quadratic Regulator.

Given a discrete time system described by

x(k + 1) = Ax(k) + Bu(k), (5.1)

with a cost function according to

J =

inf

X

k=0

(x(k) ^T Qx(k) + u(k) ^T Ru(k)), (5.2)

the cost function will be minimized using the control

u(k) = −Lx(k), (5.3)

where

L = (R + B ^T P B) ⁻¹ B ^T P A, (5.4)

which is found by solving the discrete time algebraic Ricatti equation for P , [17].

P = Q + A ^T (P − P B(R + B ^T P B) ⁻¹ B ^T P )A (5.5)

This controller requires some modifications in the original path planning model to function

as a tracking system, by expanding the state vector in (3.3) with the reference state z the

new state vector, x, will become

x = x(k) z(k)



=















 y _P (k) Ψ P (k) Ψ ˙ P (k) y _T (k) Ψ _T (k) Ψ ˙ T (k)















 .

The reference r is now incorporated into the state vector, set as constant. It will be rewritten as an input later on.

With this new state vector the full model can be written as x(k + 1) = Ax(k) + Bu,

y = Cx(k), where

A = A 0 0 I



, B = B 0



, C = I 0 ,

and the A and B matrices are the discretized versions of the matrices presented in (3.4).

The LQ feedback gain L = L x L _z  forms the feedback u(k) = −Lx(k) which generates the closed loop system according to

x(k + 1) = Ax(k) + Bu

= (A − BL)x(k).

The closed loop system then becomes

x(k + 1) z(k + 1)



= A 0 0 I



− BL x BL z

0 0

 x(k) z(k)

 , where the first row, x(k), is described as

x(k + 1) = A − BL x  x(k) − BL z z(k).

This equation can in itself be interpreted as a state space representation where z(k) is the input to the system. Setting the states x(k) as outputs we get the system

x(k + 1) = A − BL x  x(k) − BL z  z(k), y = Ix(k),

which is the LQR approximation of the path planning algorithm. The step response for the approximation is presented in Figure 5.4. It is seen that the LQR approximation gives similar response as the path planning algorithm, but with a time delay, similarly to the reduced LQT approximation.

Even though the LQR approximation has an extended state vector the same tuning pa-

rameters could be used as for the path planner. The path planner’s tuning matrix Q is