A Comparative Study of the Kinematic and Dynamic Vehicle Models

(1)

IN

DEGREE PROJECT MECHANICAL ENGINEERING, SECOND CYCLE, 30 CREDITS

STOCKHOLM SWEDEN 2019,

Impact of Vehicle Dynamics

Modelling on Feature Based SLAM for Autonomous Racing

A Comparative Study of the Kinematic and Dynamic Vehicle Models

HUGO SKEPPSTRÖM LEHTO RICHARD HEDLUND

KTH ROYAL INSTITUTE OF TECHNOLOGY

(2)

KTH Industrial Engineering and Management

Master of Science Thesis TRITA ITM-EX 2019:133

Impact of Vehicle Dynamics Modelling on Feature Based SLAM for Autonomous

Racing

Hugo Skeppström Lehto, Richard Hedlund

Approved Examiner Supervisor

2019-06-10 Lei Feng Lars Svensson

Commissioner Contact person

ÅF AB Tor Ericson

In autonomous racing there is a need to accurately localize the vehicle while simultaneously creating a map of the track. This information can be delivered to planning and control layers in order to achieve fully autonomous racing.

The kinematic model is a commonly used motion model in feature-based SLAM.

However, it is a poor representation of the vehicle when considering high lateral accelerations since the model is only based on trigonometric relationships. This Master’s Thesis investigates the consequence of using the kinematic model when undertaking demanding maneuvers; and if by switching to a dynamic model, which takes the tire forces into account, can improve the localization performance.

An EKF-SLAM algorithm comprising the kinematic and dynamic model was implemented on a development platform. The pose estimation accuracy was compared using either model when subject to typical maneuvers in racing-scenarios.

The results showed that the pose estimation accuracy was in general similar when using either of the vehicle models. When exposed to large slip angles, the implications of switching from a kinematic model to a dynamic model resulted in a significantly better pose estimation accuracy when driving in an unknown environment. However, switching to a dynamic model had little effect when driving in a known environment.

The implications of the study suggest that, during the first lap of a racing track, the kinematic model should be switched to a dynamic model when subject to high lateral accelerations. For the consecutive laps, the choice of vehicle model has less impact.

Keywords: SLAM, EKF-SLAM, Localization, Estimation, Vehicle Dynamics, Kinematic Model, Dynamic Model, Autonomous Racing

(3)

KTH Industrial Engineering and Management

Examensarbete TRITA ITM-EX 2019:133 Fordonsmodelleringens påverkan på SLAM

för autonom racing

Hugo Skeppström Lehto, Richard Hedlund

Godkänt Examinator Handledare

2019-06-10 Lei Feng Lars Svensson

Uppdragsgivare Kontaktperson

ÅF AB Tor Ericson

I autonom racing är det viktigt att kunna lokalisera fordonet med hög noggrannhet samtidigt som en karta över banan skapas. Den här informationen kan vidare bli hanterad av planerings- och reglersystem för att uppfylla autonom racing fullt ut.

Den kinematiska modellen är en vanligt förekommande rörelsemodell i SLAM.

Den är däremot en bristande representation av fordonet vid höga laterala accelerationer eftersom modellen enbart är baserad på trigonometriska samband. Det här masterarbetet undersöker den kinematiska modellens påverkan vid olika manövrar och huruvida den dynamiska modellen, som modellerar däckkrafterna, kan förbättra prestandan.

En EKF-SLAM algorithm innehållande den kinematiska- och dynamiska modellen implementerades på en utvecklingsplattform. Estimeringsnoggrannheten av positionen och orienteringen jämfördes vid typiska manövrar för racingscenarier.

Resultatet visade att estimeringsnoggrannheten av positionen och orienteringen var generellt sett lika vid användandet av antingen den kinematiska eller den dynamiska modellen. Implikationerna av att byta från den kinematiska modellen till den dynamiska modellen vid höga glidvinklar, resulterade i en signifikant bättre es- timeringsnoggrannhet av positionen och orienteringen vid körning i en okänd miljö.

Emellertid så var effekterna av att byta till en dynamisk modell insignifikanta vid körning i en känd miljö.

Implikationerna av denna studie föreslår att under det första varvet av racing- banan byta från den kinematiska modellen till den dynamiska vid höga laterala accelerationer. Under kommande varv har valet av fordonsmodell mindre effekt.

Nyckelord: SLAM, EKF-SLAM, lokalisering, estimering, fordonsmodellering,

(4)

(5)

Acknowledgements

We would like to take the opportunity to express our gratitude to people who have helped and supported us throughout this Master’s Thesis.

Lars Svensson, our supervisor at KTH Royal Institute of Technology, we are very thankful for your feedback and supervision including the help with forming a research proposal as well as giving us continuous guidance in terms of vehicle modelling, implementation, testing, and report writing. Also a big thank you to our examiner Lei Feng.

Fredrik Asplund, Master’s Thesis coordinator and Researcher at the Mechatron- ics department, KTH Royal Institute of Technology, thank you for making sure that our research question and field are based on supporting papers and research.

Colleagues at ÅF AB, our Manager Tor Ericson, Supervisors Adrian Nadi and Stefanos Kokogias, thank you for giving us the opportunity to work in a stimu- lating and fun environment with a highly interesting research topic. Furthermore, we appreciate your help with the development platform and your feedback on our progress.

Peer Master’s Thesis colleagues at ÅF AB, thank you for all the rewarding discussions. In addition, a special thanks to Felix Caesar for letting us utilize his integrated test system to provide ground truth data and to Kristian Wahlqvist for his help with the development platform.

Hugo Skeppström Lehto & Richard Hedlund

(6)

List of Figures

2.1 Bicycle model. . . 9

2.2 Parameter visualization of the bicycle model. . . 10

2.3 Detailed view of the front wheel. . . 13

3.1 Development platform provided by ÅF AB. . . 23

3.2 System Node architecture . . . 24

3.3 Visualization of feature extraction. Light grey points are point cloud data sensed by the Lidar while the green cylinders are features determined by the feature extraction algorithm. . . 26

3.4 Demonstration of the visualization of EKF-SLAM using RVIZ. . . 31

4.1 Four snapshots from an EKF-SLAM operation (on the development platform) along with ground truth data. The EKF-SLAM pose estimate at the beginning of the run is shown in (a) after which three more snapshots are captured in (b),(c), and (d). One cell of the grid is 1x1 meter. . . . 34

4.2 Two views of test case 1. A picture of the setup (a) and the corresponding view in RVIZ (b). . . 36

5.1 Position paths of all nine logfiles for test case 1, complemented with corresponding ground truth paths in each small window. . . 41

5.2 Test case 1 results from three logfiles with intermediate aggressiveness. Each of which shows the position error, orientation error, and the slip angle. . . 42

5.3 Test case 1 results from three logfiles with high aggressiveness. Each of which shows the position error, orientation error, and the slip angle. . . 43

5.4 Position paths of all eight logfiles for test case 2, complemented with corresponding ground truth paths in each small window. . . 44

5.5 Test case 2 results from two logfiles with intermediate aggressiveness. Each of which shows the position error, orientation error, and the slip angle. . . 45

(9)

5.6 Test case 2 results from two logfiles with high aggressiveness. Each of which shows the position error, orientation error, and the slip angle . . 45 5.7 Position paths of all nine logfiles for test case 3, complemented with

corresponding ground truth paths in each small window. . . 47 5.8 Test case 3 results from three logfiles with intermediate aggressiveness.

Each of which shows the position error, orientation error, and the slip angle. . . 48 5.9 Test case 3 results from two logfiles with high aggressiveness. Each of

which shows the position error, orientation error, and the slip angle. . . 48 5.10 Position paths of all three logfiles for several laps on the track, comple-

mented with corresponding ground truth paths in each small window.

. . . 49 5.11 Track with several laps results from three logfiles with different levels

of aggressiveness. Each of which shows the position error, orientation error, and the slip angle. . . 50 5.12 Results from three different logfiles where exceptional results were dis-

covered. Two of which from test case 2 (Intermediate and High) and one from test case 3 (High). Each of the plots shows the pose error, orientation error, and the slip angle. . . 51 A.1 Test case 1 results from three logfiles with low aggressiveness. Each of

which shows the position error, orientation error, and the slip angle. . . i A.2 Test case 2 results from two logfiles with low aggressiveness. Each of

which shows the position error, orientation error, and the slip angle. . . ii A.3 Test case 3 results from three logfiles with low aggressiveness. Each of

(10)

List of Tables

3.1 Custom ROS messages . . . 25 3.2 Vehicle parameters . . . 30 4.1 Test case description and motivation. . . 35 5.1 Position NRMSE with standard deviation for the Kinematic (K) and

Dynamic (D) models compared to the ground truth, subject to three levels of aggressiveness. . . 52

(11)

Nomenclature

ADAS Advanced Driver-Assistance System.

EKF Extended Kalman Filter.

FSD Formula Student Driverless.

IMU Inertial Measurement Unit.

KF Kalman Filter.

RMSE Root Mean Square Error.

(12)

(13)

Chapter 1

Introduction

This chapter comprises relevant background information and a description of the problem. This leads to the research question and the contribution of this Master’s Thesis. In addition, the choice of method as well as ethical and sustainability considerations are covered.

1.1 Background

This Master’s Thesis was conducted at ÅF AB in Solna, Sweden. ÅF is an engineering and design company within Digital Solutions, Infrastructure, Industry, and Energy. The company is mainly situated in Sweden but is involved in many projects globally. During the past few years, the company has provided Master’s Theses in collaboration with the KTH Formula student team¹. KTH Formula student is a student driven project which aims to develop a race car to compete against other universities in international contests. The focus of recent Master’s Theses conducted within this collaboration has been towards making the race car fully autonomous in order to participate in another genre of the contest, namely the Formula Stu- dent Driverless (FSD) class. The perception procedure of FSD focuses on mapping and localization. During the first lap, the autonomous vehicle creates a map of the track which it then localizes itself within for the consecutive laps. Consequently, this yields a better performance of the trajectory planning [1]. The engineering part of these Master’s Theses has been implemented on a small-scale remote controlled car supplied by ÅF with the goal of eventually implementing the technology to the full-size formula race car.

A main requirement for a mobile robot to be qualified as autonomous is the ability to localize itself in an environment. Given a map of the environment, the problem is usually referred to as pose estimation (where in planar motion, the pose is usually given as two location coordinates and the orientation) and is considered to be the most basic perceptual problem in robotics.

1https://www.kthformulastudent.se/

(14)

CHAPTER 1. INTRODUCTION

However, in the context of FSD, no such a priori map is given. This realization brings attention to one of the most essential problems in robotics, frequently referred to as the problem of simultaneous localization and mapping (SLAM). As the name suggests, the problem arises when the robot does not have access to a map, or knows its own pose. Solving the problem requires sensor measurements to perceive the environment. By distinguishing eminent features, cones in this case, based of the measurements and using probabilistic methods (most commonly) the SLAM problem can be solved.

1.2 Problem Description

The general research of SLAM is one that is important to the community as the solution to the SLAM problem is a key functionality to make a mobile robot fully autonomous [2], [3]. Research in the automated driving field has mainly focused on three different layers; Perception, Planning, and Control [4]. Feature-based SLAM belongs to the perception layer in which there is a prediction step that models the motion of the vehicle where it predicts the pose in the current time step [5].

Most commonly, when considering vehicle models, the model often used is de- rived from trigonometric relationships, referred to as a Kinematic Model. That model is suitable for most applications in mobile robotics where the robot is moving at low speeds and consequently is not subject to high lateral accelerations. Another way of modelling the movement of the vehicle is to use a Dynamic Model. The prominent difference between the former and the latter is that the dynamic model is derived from the forces acting on the vehicle. Most importantly, the model accounts for the forces acting on the tires of the vehicle. This enables modelling of motion even during high lateral accelerations. The performance issues in regards to the model has been more frequently researched in the area of path planning where the vehicle model affects the planning and control layers [6], [7], [8], [9] [10].

1.2.1 Purpose

As stated in Section 1.2, vehicle dynamics modelling has been explored in motion planning and control for autonomous vehicles. However, to the best of our knowledge there is a gap in the research when considering the impact of vehicle models in SLAM when subject to high lateral accelerations. At high lateral accelerations the lateral tire forces will be large and therefore the tire slip angle (the difference between the tires’ orientation and their actual moving direction) will also be large which consequently will need to be accounted for in order to make an accurate pose estimation in SLAM.

Hence, the purpose of this Master’s Thesis is to fill this gap by comparing the kinematic model with the dynamic model in terms of localization performance in SLAM for racing scenarios. The two vehicle models will be incorporated and evaluated in SLAM separately. Then the results of using either model will be compared to see if the localization performance can be improved by switching vehicle

2

(15)

1.3. RELATED WORK

model during operation to the dynamic model at the point of failure of the kinematic model. Here, failure is determined by the pose estimation error from the SLAM output. This investigation will include specific tests on different areas of a track made up of plastic cones, such as straight paths and sharp turns so that the vehicle models’ performance can be discussed and to see if switching between them in real- time can have benefits to the overall localization performance.

1.2.2 Delimitations

A number of delimitations follows from the nature of this Master’s Thesis. It is conducted by two Master’s students, each of which comprises 30 Swedish credits on advanced level corresponding to 20 weeks of full-time studies. For enabling a feasible iterative implementation process of the vehicle models and SLAM, a small- scale development platform will be used. This comes with the delimitation of being able to accurately model the vehicle compared to a real size vehicle and limits which sensors are available. However, since the vehicle will be used for both vehicle models, the comparison will still be valid. The race track for the tests will be set up at the Royal Institute of Technology (KTH) and therefore the tests will only be performed indoors.

1.3 Related Work

In the field of autonomous racing, related work is often referred to the student- driven projects in FSD, where the perception of the vehicle is a postulate for autonomous performance. Among the competitors are [1], [11], and [12] which have individually developed autonomous race cars using FastSLAM [13], LOAM [14], and EKF-SLAM [15], [16] which are feature-based algorithms for localization and mapping. Here, EKF stands for the extended Kalman filter. These projects yield further research in autonomous racing as [17], which explores a method of increas- ing robustness in state estimation by sensor redundancy, combining range-bearing feature extraction and visual feature affirmation by means of computer vision.

Research covering effects of vehicle model complexity is more frequently ad- dressed in the planning and control layers of autonomous drive. There are some works exploring performance difference in motion planning as a consequence of which vehicle model is used [6], [7]. There have been research conducted in regards to Lane-keeping systems for autonomous vehicles where a kinematic and dynamic model are compared in terms of error performance. For example, [18] evaluates kinematic and dynamic vehicle models for autonomous driving systems. The paper concludes that the kinematic model is enough to model the vehicle motion. Al- though the experiments in the research was conducted at high speeds, the yaw rate and slip angle remained small. The same researcher [19] proposes a Lane-keeping system implemented with an Interacting Multiple Model (IMM) filter which includes a lateral kinematic and dynamic vehicle model. The filter is evaluated in experiments and compared with the kinematic and dynamic models separately. The

(16)

results show that the lateral kinematic model is enough to model the lateral vehicle motion at a small yaw rate but at a larger yaw rate the lateral kinematic model is no longer reliable. Additionally, the filter using the dynamic model performs poorly in terms of orientation estimation.

1.4 Research Question

The research question that enables the problem description to be answered is defined as:

• Given high lateral acceleration maneuvers that heavily affect the slip angle, what are the implications of switching to a dynamic model at the point of failure of a kinematic model? (Where point of failure is decided by the pose estimation accuracy of feature-based simultaneous localization and mapping).

1.5 Research Methodology

A suitable SLAM algorithm had to be chosen in order to compare the impact of using the kinematic or dynamic vehicle models. When given a sparse environment, such as the track (made up of cones) provided in FSD, feature-based SLAM algorithms are considered a suitable choice [20].

As mentioned in Section 1.3, some of the feature-based SLAM algorithms used in FSD are EKF-SLAM and FastSLAM. Furthermore, another feature-based SLAM algorithm is based on the unscented Kalman filter (UKF). Although FastSLAM and UKF-SLAM are superior in performance compared to EKF-SLAM [20], [21], [22], [23], [24], EKF-SLAM was chosen for two reasons. Firstly, EKF-SLAM is one of the most commonly used and one of the first feature-based SLAM algorithms.

Secondly; it is well documented, especially in [20], and thus enables a relatively fast and efficient implementation process in which the kinematic and dynamic vehicle models can separately be integrated in the prediction step.

Comparing localization performance in SLAM using the kinematic or dynamic vehicle model is subject to a need for an accurate and systematic method in order to make a valid conclusion from the results. Localization in EKF-SLAM boils down to pose estimation of the vehicle. Therefore, a localization error in relation to the actual pose due to the approximation of the given vehicle model will be present.

Here, actual pose is referred to as the ground truth which will be utilized to compare the impact of using each vehicle model. A key step of this method is to collect a dataset of ground truth pose labels for each time-synchronized vehicle state.

This method is clearly of a quantitative nature where pose estimates will be computed for each of the vehicle models from which statistical methods can be applied to evaluate the effect that the models have on the localization error. The evaluation metric that will be used is the root mean square error (RMSE) of the vehicle position which is frequently used in the field [21], [22], [25], [26]. There

4

(17)

1.6. DIVISION OF WORK

are also other types of suitable evaluation metrics such as the mean absolute error (MAE) and the normalized estimation error squared (NEES). The latter is used to compare consistency in terms of a χ² test with a stated significance level [27]

which have been used to evaluate the consistency of SLAM [28], [29], [30]. MAE and RMSE on the other hand compute the accuracy which is more applicable when evaluating localization performance. However, RMSE is more sensitive to outliers due to squaring the errors [31]. This means that more weight are put to large errors which arguable should be applied in the context of localization performance in SLAM. Hence, MAE is disregarded as an evaluation metric in this Master’s Thesis. In addition, in order to compare the RMSE on multiple datasets subject to different scales, it is appropriate to normalize the RMSE (NRMSE) by the ground truth data [32].

This leads to a metric that can be evaluated and compared between the vehicle models when integrated in SLAM for different test cases. Consequently this approach will provide answers to the research question and work as a foundation for a discussion and conclusion.

1.6 Division of Work

The work between Richard and Hugo has had a structured and collaborative approach where every decision has been made together through discussions. In terms of theory, Hugo was responsible for vehicle dynamics and feature extraction while Richard’s responsibility was the extended Kalman filter and SLAM. The practical side of the Master’s Thesis included a lot of programming in which a form of pair programming was conducted to enable deep knowledge about the system and implementation between both students. Also to critically assess each others code to spot unintentional errors. Furthermore, the pseudocode algorithms were written by Richard while the figures, and the plots in the results section were made by Hugo using L^ATEXvector graphics and Matplotlib. Finally, the rest of the report and the implementation was made together and the overall collaboration was of great success.

1.7 Ethics and Sustainability

It is evident that SLAM for autonomous drive is an essential part of making a vehicle fully autonomous and thus puts high demands on safety and redundancy of the system. This leads to ethical and sustainability aspects that need to be managed carefully to prevent accidents and hazards. As mentioned in Section 1.2, perception is the first out of three layers in autonomous drive which means that an accurate and trustworthy SLAM algorithm is crucial since the output from it is what the planning and control layers base their decisions on. Therefore, the SLAM algorithm needs to be accurate, reliable, and at the same time provide a safety system that senses any potential erroneous SLAM outputs.

(18)

The robustness of SLAM for prolonged operation is discussed in [33] where they point out two sources of potential failures: algorithmic and hardware. They argue that failures subject to the algorithm choice is mainly due to fragile data association which is sensitive to outliers that can propagate to future updates. In order for a SLAM algorithm to be robust, it requires a system that can recover its map and localization after having experienced outliers. Current SLAM algorithms struggle in such situations. When it comes to hardware failure; the problem of damaged or aging sensors need also to be detected to dynamically adapt the noise and bias for each sensor [33]. This is one of the major problems why autonomous vehicles have not yet been fully deployed on public roads. Even so, research on autonomous vehicles have led to successful technologies that have been implemented on regular vehicles as part of their advanced driver-assistance system (ADAS). Such technologies are for example lane-keeping assistance, collision avoidance, and cruise control which suggests that autonomous vehicles will increasingly be part of the future as more and more research is conducted within the field [34].

It is known that Formula 1 racing has throughout the years pushed the technological boundaries in fields such as mechanics, engines, and energy consumption which has consequently been transferred to road vehicles for obvious reasons [35].

So why shouldn’t technological developments in the field of autonomous drive be a suitable research area for racing? Intuitively this would be a great opportunity to test the SLAM algorithm in the context of high lateral accelerations within a sparse environment. During the past years, a racing competition called Roborace [36] has emerged which is trying to make advancements in the autonomous drive field by letting development teams compete against each other using an electric racing car called Robocar. The race car is the first of its kind. Their intention is ”[...] Through sport, innovations in autonomous technology will be advanced, accelerating improve- ments to road safety.” [36] which suggests that research within autonomous racing can lead to safer autonomous vehicles and potentially a faster development phase towards commercializing them.

1.8 Mathematical Notation

The mathematical notation throughout this Master’s Thesis is consistent. An arbi- trary single-dimension variable is stated as x which is made italic. A vector of sev- eral dimensions is stated as bold such as x =^[x₁, x₂^]^T whereas a multi-dimensional array is defined as a capital and bold variable to form a matrix similar to

X =





x_1,1 . . . x_1,n ... . .. ... x_n,1 . . . x_n,n



. (1.1)

There are also capital variables that are not bold, such are meant to be global variables of the coordinate axes (X, Y, Z), in the case of position and velocity of the vehicle. Constants can be small or capital depending on the convention in the

6

(19)

1.8. MATHEMATICAL NOTATION

research field. In addition, all Greek letters are made italic regardless of which type of variable it is, for instance the mean vector µ.

Since dealing with real-time observations, several variables will have a discrete time k associated with it. When in the context of algorithms there will also be for-loops in which variables will have a subscript of the iteration such as j. The consistent notation for such a variable that holds many subscripts will be defined in an order similar to x_x₁_,k,j where x₁ is the first dimension in x, k is the discrete time, and j is the iteration of an arbitrary for-loop.

(20)

(21)

Chapter 2

Theoretical Background

This chapter describes and explains the underlying theory on which this Master’s Thesis is built upon. A general explanation of how two different ways of modelling a vehicle is explained, and the equations of vehicle motion are derived. Furthermore, the theory of the algorithm solving the SLAM problem is explained.

2.1 Vehicle Dynamics

Beginning with a description of the simplified vehicle model, this section covers the equations which are used to describe the motion of the vehicle in general terms.

2.1.1 Bicycle Model

The single-track model, or the ”bicycle” model, is a widely used way of modelling vehicle behaviour. The model includes some simplifications and omissions which simplify the model by decreasing the degrees of freedom without significantly re- ducing the accuracy of the model in the linear range.

CG

Figure 2.1: Bicycle model.

(22)

CHAPTER 2. THEORETICAL BACKGROUND

The main simplifying assumption is that the vehicle’s center of gravity is at a height level to the roads surface. The consequence is that the lateral forces acting on the wheels can be assumed equal during cornering, enabling the front and rear wheel pairs to be modeled as single front and rear wheels respectively in accordance to Figure 2.1. Hence the name ”bicycle” model. As follows from the assumption, the vehicle will not experience any rolling motion during cornering due to the location of the center of gravity [37].

In the following, reference will be made to Figures 2.1 and 2.2 where the naming convention is similar to that found in [38]. The fixed vehicle coordinate system will be used to describe the movement of the vehicle in its inertial frame. The x-axis is pointed in the forward direction of the vehicle (longitudinal), the y-axis to the left (lateral), and the z-axis is perpendicular to the road surface. The inertial coordinate systems origin is placed at the center of gravity. A global coordinate system is also defined, and the respective axes are denoted X and Y . The orientation of the global system is visualized by Figure 2.2, and defined accordingly.

b ψ

δf

β v

θ_v_f v_f

x

y F_y,f

F_y,r

ℓf

ℓr

Y

X

O

R

v_r

Figure 2.2: Parameter visualization of the bicycle model.

The model pose used in this Master’s Thesis is described as planar motion in the global plane (X, Y ). It is given by the coordinates’ X and Y , and the orientation ψ which is the rotation in relation to the global X-axis about the inertial z-axis. The vehicle uses front-wheel steering (where the steering angle is denoted δ_i| i ∈ {f, r}), and thereby the rear wheels will remain parallel to the assumed rigid axis that is the vehicle frame [39]. The frame’s length is the sum of the distance from the center

10

(23)

2.1. VEHICLE DYNAMICS

of gravity to the front wheel ℓ_f, and the rear wheel ℓ_r. The vehicle slip angle β is the angle between the vehicle frame and the inertial velocity vector v at the center of the vehicle.

2.1.2 Kinematic Model

A kinematic model is commonly used when considering a mathematical description of a vehicle’s motion at low speeds. It is derived from trigonometric relationships governing the system. Thereby, there is no consideration of the forces that act on the vehicle. The major assumption of the kinematic model is that the velocity vectors (v_f, v_r in Figure 2.2) at each respective wheel are aligned with the direction of that wheel. This is the same as assuming that the tire slip angle is zero, which is a valid assumption for low lateral accelerations. The motion of the vehicle according to the kinematic model is described by

X = v cos(ψ + β)˙ (2.1a)

Y = v sin(ψ + β)˙ (2.1b)

ψ =˙ v cos(β)

ℓ_r+ ℓ_f (tan(δ_f)− tan(δr)) (2.1c) β = tan⁻¹

(ℓ_ftan(δr) + ℓrtan(δ_f) ℓ_f + ℓ_r

)

. (2.1d)

From a simple component breakdown of the vehicle’s velocity v at the center of grav- ity, the velocity in the global frame is defined according to eqs. (2.1a) and (2.1b).

The rate of change of the yaw (also referred to as the orientation, or heading angle of the vehicle) ˙ψ is derived from trigonometric relationships regarding the instanta- neous center of rotation of the vehicle, O. Because the vehicle is assumed to travel at low speeds, the radius of the vehicle’s path R is assumed to change slowly. This allows for the assumption that ˙ψ is equal to the angular velocity. Hence, there is a relation between the velocity, radius, and angular velocity according to

ψ =˙ v

R. (2.2)

The relationships between steering angle, slip angle, radius, distances to center of gravity, and velocities combined with the assumption of low speed yields the rate of change of the yaw (yaw rate), ˙ψ as eq. (2.1c). Furthermore, the slip angle β is given by eq. (2.1d). For a complete derivation of the kinematic model, refer to [38].

2.1.3 Dynamic Model

As stated in Section 2.1.2, the kinematic model is suitable for low lateral accelerations, when slip angles are small. The assumptions on which the derivation is based on will no longer be valid when considering high lateral accelerations. When doing so, the forces acting on the vehicle will affect the motion to a larger degree.

(24)

Therefore, a dynamic model which accounts for the forces should be implemented.

The equations of motion using a dynamic model are given by

X = v˙ _xcos(ψ)− vysin(ψ) (2.3a) Y = v˙ xsin(ψ) + vycos(ψ) (2.3b)

ψ =¨ 1 I_z

(

ℓ_fF_y,f − ℓrF_y,r

) (2.3c)

˙v_x= 1 m

(

F_x− Fair

) (2.3d)

˙vy = 1 m

(

Fy,f + Fy,r

)− vxψ.˙ (2.3e)

Eqs. (2.3a) and (2.3b) are derived through component breakdown as in Section 2.1.2.

Unlike the kinematic model, the velocity is separated into its respective contribu- tions in x and y to only depend on the orientation ψ. The outstanding difference in the model is as mentioned before; the forces F_y,i, where i∈ {f, r}. As shown in Figure 2.2, these are the tire forces acting in the y-direction in the inertial frame.

The derivation of these will be covered in Section 2.1.4. Furthermore, the vehicle slip angle according to the dynamic model is defined as

β = arctan (vy

v_x )

. (2.4)

To acquire an expression of the rate of change of the yaw rate ¨ψ, the conservation of momentum about the inertial z-axis is used. Solving the equation algebraically for ¨ψ yields eq. (2.3c), where I_z is the moment of inertia about the center of mass around the z-axis. Applying Newton’s second law in the inertial longitudinal and lateral direction (x-axis, y-axis), the accelerations ˙v_x and ˙v_y can be expressed as eqs. (2.3d) and (2.3e). Here m is the vehicle’s mass and the last term in (2.3d) is the centripetal acceleration while F_air is the drag [37], [38].

2.1.4 Tire Model

A vital part of modelling dynamic behaviour is the effect of the wheel forces on the motion model. The forces can be described by modelling the tire, and the equations describing the forces are derived from trigonometric relationships and tire-specific parameters. Following is a description of the brush tire model invented by E. Fiala 1952 [40]. The model is also known as the Fiala tire model and consists of bristles (hence brush tire model), or tread elements, that deflect in a direction parallel to the road surface and represents the elasticity of the real tire. When the wheel is rolling freely without braking, turning, or subject to side slip, the bristles remain vertical (i.e perpendicular to the road surface) and there will be no side force generated. When the velocity vector creates an angle with the direction of the tire, side slip occurs and the bristles deflect from the leading edge of the tire to the trailing edge as shown in Figure 2.3b. Thereby generating a side force.

12

(25)

2.1. VEHICLE DYNAMICS

The leading edge of the contact patch will adhere to the ground as follows from the allowed friction, and will not slide. Depending on the tire slip angle α_i, the deflection γ(x) throughout the contact line of the tire will differ. The tire model used is a model of lateral tire force, presented in [38] with an approximated parabolic normal pressure distribution on the contact patch. The contact patch is in turn approximated to be rectangular, defined by the patch length 2a, and width 2b.

With a small tire slip angle, the deflection varies linearly over the contact line from the leading edge to the near end of the contact patch. This contact line is parallel to the wheel velocity vector. When the contact line intersects the parabola (from the assumption of uniform parabolic pressure and consequently, the deflection) sliding will start and the contact line will coincide with the parabola (maximum deflection) for the remainder of the contact line. The resulting deflection along the contact line for this case is depicted in the graph in Figure 2.3b. For the extreme case of pure sliding, the deflection will be maximized along the parabola from leading to following edge. In the extreme case, the maximum side force is generated [41].

δ

_f

v

_f

θ

_v_f

(a) Tire slip angle (front wheel)

2b 2a

α

v

Cα

µ kc

x γ(x)

γ(x)

xs

(b) Trigonometric and tire-specific parameters depicted in the case of tire deflection due to α̸= 0

Figure 2.3: Detailed view of the front wheel.

The tire slip angle α_i = δ_i− θvi| i ∈ {f, r} is the angle differential between the steering angle δ_i, and the angle between the longitudinal direction of the vehicle and the wheel velocity v_i. It is denoted θ_vi, visualized in Figure 2.3a, and defined

(26)

as

θ_vf = arctan

(v_y+ ℓ_fψ˙ vx

)

(2.5a)

θvr = arctan

(v_y− ℓrψ˙ vx

)

(2.5b)

for front (f ) and rear (r) wheel respectively. As stated in [38], the lateral tire force according to the brush-tire model is equal to

F_y =



 µFz

[

3θ|S| − 3θ²|S|²+ θ³|S|³^]sgn(S) if|S| ≤ ¹_θ

µFzsgn(S), if|S| > ¹_θ.

(2.6)

Fz is the normal force on the tire and µ is the tire–road friction coefficient. Eq. (2.6) is derived from the lateral deflection profile through lateral force equilibrium. The expression is bounded due to the limit of slip angle where full slip occurs. Eq. (2.6) can be re-written into

F_y,i=



 [

Cα|tan(αi)|−_3µFz^C2^α |tan(αi)|²+ ^C3^α

27µ2F 2z|tan(αi)|³

]

sgn(tan(αi)) if _|αi|≤arctan

(

3µFz Cα

)

µFzsgn(tan(αi)) otherwise

(2.7) to more clearly reflect the parameter dependencies of the lateral force. Mainly the cornering stiffness C_α, friction µ, and tire slip angle α_i. The notation S in eq. (2.6) is simply related to the tire slip angle as S = tan(α_i), and θ (not to be confused with θ_vi) is defined as

θ = 4a²bk 3µFz

. (2.8)

As seen in [42], the cornering stiffness can also be written on a radian basis as

Cα = 2ca². (2.9)

Here k and c are the lateral stiffness per unit area, and unit length, respectively.

They are tire specific and relate through the contact patch width 2b as k = c

2b. (2.10)

Eqs. (2.8)–(2.10) are needed to re-write eq. (2.6) into eq. (2.7).

2.2 Extended Kalman Filter

One of the most extensively used filtering techniques for SLAM is the Kalman filter (KF) [43], [44] which was first invented during the mid 20th century [45]. KF is used to estimate the state vector x_k at the discrete time step k for linear Gaussian

14

(27)

2.2. EXTENDED KALMAN FILTER

systems by using moments parameterization. That is to define the state beliefs by the mean µ_k and the covariance matrix Σ_k which creates a multivariate normal distribution according to

p(x_k) = 1

√2π|Σk|exp^{−1

2(x_k− µk)^TΣ_k⁻¹(x_k− µk)^}. (2.11) Important to note is the fact that the KF uses the Markov assumption which states that the next state x_k+1 is independent of the prior state x_k₋₁ given the present state x_k. Along with this assumption, the goal of the KF is to update the posterior beliefs of the state vector by using the prior beliefs (the posterior from the previous time step) in two steps; the prediction step that based on the state model predicts the current state, and the correction step that updates the predicted state by incorporating new measurements [20]. Most of the mathematical notations in the following Sections are similar to the one used in [20].

One major limitation of the KF is that it only handles linear systems. Hence, the successor called (EKF) handles this problem by making it possible to apply this filter to nonlinear systems with the use of linearization.

2.2.1 Prediction Step

In the prediction step of EKF, the state vector model is described by a nonlinear function g according to

xk= g(xk−1, uk) + ϵk (2.12) where ϵ_k is a Gaussian additive noise vector with zero mean and covariance R_k while u_kis the control input. Hence, the state probability distribution is defined as p(x_k|xk−1, u_k)∼ N (g(xk−1, u_k), R_k). (2.13) Since one of the key characteristics of the KF is that the posterior beliefs are always Gaussian, it enforces the nonlinear function g to be linearized in order to compute it on a closed form such that the EKF can keep the same characteristics. The linearized approximation is done using a first order Taylor expansion such that

g(xk−1, uk)≈ g(µ_k₋₁, uk) + Gk(xk−1− µ_k₋₁) (2.14) which is linearized around the mean of the previous state µ_k₋₁ since it is the most probable state, and where G_k is the Jacobian given by

G_k= ∂g(x_k₋₁, u_k)

∂x_k₋₁ =







∂g1

∂x1 . . . _∂x^∂g¹ .. n

. . .. ...

∂gn

∂x1 . . . ^∂g_∂xⁿ

n





 (2.15)

where x_k₋₁ = µ_k₋₁. The predicted posterior beliefs of the present time step can then be computed using the posterior beliefs p(x_k₋₁) from the previous time step, such that

p(xk) = ^∑

x_k−1

p(xk|xk−1, uk)p(xk−1)∼ N (µ_k, Σk) (2.16)

(28)

where the predicted first and second moments are governed by

µ_k= g(µ_k₋₁, uk) (2.17a)

Σk= GkΣk−1G^T_k + Rk. (2.17b) 2.2.2 Correction Step

The prediction step only predicts the state at the present time step based on the state model. This state prediction needs to be corrected by measurements as well as an expectation of the measurements. The model of the measurement vector is analogous to the nonlinear approach made in the prediction step, specifically given by

zk= h(µ_k) + ζ_k (2.18)

where h is the nonlinear function and ζ_k the Gaussian additive noise from the measurements. The measurement probability then becomes

p(z_k|xk)∼ N (h(µk), Q_k) (2.19) when incorporating the state prediction x_k and the additive observation noise Q_k which is Gaussian with zero mean.

A key terminology in EKF is the innovation which is defined as the measurement residual error between the expected measurements and the actual measurements according to

˜

y_k= z_k− h(µk) (2.20)

where µ_k is the prediction of the state vector at the present time step. This is used later on to update the mean of the state.

The nonlinear function h applies the same linearization procedure with the Ja- cobian H of the measurement model as the one made in the prediction step. The slight change in this case is that it instead linearizes around the predicted mean µ_k computed from the prediction step with G_k. By using Taylor expansion the linearization of the measurement model turns into

h(xk)≈ h(µ_k) + Hk(xk− µ_k) (2.21) where H_k has the same structure as G_k from eq. (2.15). From this, the innovation matrix S_k is computed by a linear transformation of the predicted covariance such that

S_k= H_kΣ_kH^T_k + Q_k. (2.22) Another important parameter is the Kalman gain K defined according to

K_k= Σ_kH^T_kS⁻¹_k (2.23)

which holds information about how much the state predictions needs to be corrected by incorporating the measurements. This depends on how noisy the measurements

16

(29)

2.2. EXTENDED KALMAN FILTER

are and how much the predicted states expects to change between iterations, which is represented in Σ_k. Intuitively if the prediction covariance Σ_k and the measurement noise Q_k are small, then the measurements are considered to be more accurate, hence they should be considered in favor to the predicted estimate. However, if the prediction is highly accurate, that is if Σ_k approaches zero, then the Kalman gain also approaches zero which consequently means that the measurements have little effect on the state estimate [46].

The final step of EKF is to correct the predicted first and second moments of the Gaussian distribution which is done by computing

µ_k= µ_k+ K_k˜y_k (2.24a)

Σk= (I− KkHk)Σk (2.24b)

which results in the pose estimate at time step k.

2.2.3 EKF Algorithm

This Section presents the algorithm for EKF by combining the previously stated equations. The algorithm in pseudocode is stated in Algorithm 1 which is based on the one presented in [20]. The EKF implementation has a recursive structure in which its inputs are the previously estimated first and second moments as well as the present control signal and measurements at each time step k. Lines 1 through 4 represent the prediction step where N (µk, Σ_k) is computed.

Algorithm 1 Extended Kalman Filter

1: Input: µ_k₋₁, Σ_k₋₁, u_k, z_k 2: µ_k← g(µk−1, u_k)

3: G_k← ^∂g(µ_∂µ^k⁻¹^,u^k⁾

k−1

4: Σk← GkΣk−1G^T_k + Rk

5:

6: ˜yk ← zk− h(µk) 7: Hk← ^∂h(µ_∂µ^k⁾

k

8: Sk← HkΣkH^T_k + Qk

9: K_k← ΣkH^T_kS⁻¹_k 10:

11: µ_k← µk+ K_ky˜_k 12: Σ_k← (I − KkH_k)Σ_k 13: Output: µ_k, Σk

(30)

The correction step is done between lines 6 and 12 which begin with computing the Kalman gain. This is done by incorporating the measurements (lines 6 through 9) after which the correction of the first and second moments are computed to form the final Gaussian distributionN (µk, Σ_k) (lines 11 through 12). This outputs the beliefs of the present state.

2.3 Feature Extraction

In regards to the perception layer of autonomous drive, features in the environment play an important role to the performance when considering localization based on peripheral measurements. The aim of feature extraction is to remove redundant data and make it better for human interpretation as well as to reduce the number of data points for more efficient data processing. There are different methods to extract features. Mainly the choice of method depends on which data is obtainable, and the data availability further depends on the available sensors in the system. For example, if visual camera data is obtainable, feature regions may be distinguished by shapes, intensities, and/or sizes [47]. The data could also be obtained from a range-bearing sensor (e.g a laser scanner) whose features can be extracted from geometric shapes [48], [49], [50]. The extraction relies on segmentation of the data based on the geometric shape. The method described in the rest of this section relies on segmentation of data based on Euclidean clustering of data points.

2.3.1 Euclidean Clustering using KD-Trees

A peripheral sensor gathers spatial data which needs to be pre-processed efficiently in a real-time application. A relatively complex, yet efficient, algorithm that is able to extract features of various sizes, is the one presented in [51] which makes use of a kd-tree to find the approximately nearest neighbours. A kd-tree is a commonly used data structure for finding nearest neighbours [52], it makes orthogonal splits of k-dimensional spatial data into sub partitions in which the approximately nearest neighbours can efficiently be computed since kd-trees have a time complexity of O(log n) on average [53]. Here, n is the number of data points.

The kd-tree is similar to a binary tree because it partitions the data with the median as the pivot for one dimension every level of the tree. For a two dimensional case, it starts with partitioning the data in the x-dimension as the root node. On the second level, it then partitions the data in the y-dimension consecutively, after which it again partitions the data in the x-dimension on level 3. It does this recursively for every dimension. After the kd-tree has been created, the approximately nearest neighbours are applied within the boundaries for the given data point. The search for nearest neighbours is based on a radius r within a threshold d_th where the distance between the given data point p_i and for each of its neighbours p_j is computed by d^∗(p_i, p_j) where d^∗is the Euclidean metric. Until termination when all data points have been processed and assigned to a cluster, it does this analogously for each of the neighbours if not already processed.

18

(31)

2.4. SIMULTANEOUS LOCALIZATION AND MAPPING

2.4 Simultaneous Localization and Mapping

SLAM is the process of simultaneously creating a map of the vehicle’s surrounding and estimating its pose in that map without having any a priori knowledge of either.

This is a difficult task since the map building depends on reliable pose information whereas the pose of the vehicle relies on how accurate the map is [20], [54]. There are various SLAM approaches that are governed by different kinds of sensors. One of the most famous and influential solutions to SLAM is EKF-SLAM [15], [16]

which uses a feature-based approach to build the map in an online fashion, that is to update the SLAM posterior using Recursive Bayesian Estimation (RBE) that is based on Bayes’ Theorem such that

p(xk, m|z1:k, u1:k) =p(z_k|xk, m)Σx_k−1p(x_k|xk−1, u_k)p(x_k−1, m|z1:k−1, u_1:k−1)

p(zk) (2.25)

at time step k [55]. Here m is the map containing all landmarks, which are observed features that have been added to the map (by the SLAM algorithm). The SLAM posterior in online SLAM as opposed to full SLAM marginalizes over all past pose and landmark estimates to only compute the present µ_k which is an augmented mean from EKF which besides the pose of the vehicle also contains the position of each landmark in a concatenated vector. The reason for using marginalization is due to impractical computational demands to estimate all the past poses and landmarks at every given step in time. This marginalization is done recursively in every iter- ation by taking the past SLAM posterior distribution p(x_k₋₁, m|z1:k−1, u_1:k₋₁) to be considered as a prior in the present time step multiplied by the transition proba- bility p(x_k|xk−1, uk) to form a conjugate prior. This is multiplied with p(zk|xk, m) which is the likelihood that the present state and map have generated the measure- ments z_k. A key assumption is that the measurements are considered conditionally independent. The marginal likelihood p(z_k) is a normalization factor [56].

In analogy to EKF, EKF-SLAM uses this Bayesian approach to first predict the vehicle’s pose based on the vehicle model in the form of a Gaussian distribution N (µ_k, Σk). Since the vehicle model is a model of the real world, it does not give the full solution to the vehicle’s pose. Therefore the localization needs to be corrected based on the range and bearing values of the features at time k using a measurement model which is then associated with the current set of landmarks in the map.

EKF-SLAM is an efficient algorithm when subject to a sparse environment, that is when the number of landmarks is small. However if the environment contains many features or if the features are difficult to extract, EKF-SLAM gets heavily affected since it has a time complexity ofO(n²) where n is the number of landmarks in the map [20].

2.4.1 Data Association

In the correction step, all potentially new landmarks called features are iterated one by one to determine if they should be added to the map or if they already correspond

A Comparative Study of the Kinematic and Dynamic Vehicle Models

Impact of Vehicle Dynamics

Modelling on Feature Based SLAM for Autonomous Racing

A Comparative Study of the Kinematic and Dynamic Vehicle Models

HUGO SKEPPSTRÖM LEHTO RICHARD HEDLUND

Acknowledgements

Contents

List of Figures

List of Tables

Nomenclature

Chapter 1

Introduction

1.1 Background

1.2 Problem Description

1.3 Related Work

1.4 Research Question

1.5 Research Methodology

1.6 Division of Work

1.7 Ethics and Sustainability

1.8 Mathematical Notation

Chapter 2

Theoretical Background

2.1 Vehicle Dynamics

δ

v

θ

v

2.2 Extended Kalman Filter

2.3 Feature Extraction

2.4 Simultaneous Localization and Mapping