State-Estimator Design for the KTH Research Concept Vehicle

Transcription

1 DEGREE PROJECT IN VEHICLE ENGINEERING, SECOND CYCLE, 30 CREDITS STOCKHOLM, SWEDEN 2016 State-Estimator Design for the KTH Research Concept Vehicle FAN GAO KTH ROYAL INSTITUTE OF TECHNOLOGY SCHOOL OF ENGINEERING SCIENCES

2 Acknowledgement First of all, I would like to express my appreciation to my supervisor Mikael Nybacka, without his continuous support and help this thesis would not have been possible. I also would like to say that I really enjoyed the time when I was working at the Integrated Transport Research Lab and it was a great pleasure for me to work with so many nice people and I would like to thank you all. And I specifically thank my friends Karl Kruzer, Andreas Högger, Stefanos Kokogias, Petter Tomner, Lars Svensson and Rui Oliveira who helped me whenever I needed help and also brought a lot of fun to our office. I really learnt a lot from you. Finally I owe my most important acknowledgements to my family, whose love and dedication carried me through this thesis project.

3 Abstract The Research Concept Vehicle (RCV) is a pure electric vehicle with four in-wheel motors and individual steering as well as camber actuators. It serves as an experimental research vehicle which is built by the Integrated Transport Research Lab (ITRL). The development of the RCV s functionality never stops after the platform started running. In order to involve the advanced driver assistance systems and realize autonomous driving in the RCV, accurate information of vehicle dynamic states and the environment is required. In this case, based on the sensors we have on the RCV, sensor fusion and state estimation are critical to be adopted for solving this problem. The purpose of this thesis is to find appropriate estimators, define the specifications and design the corresponding logics to estimate vehicle dynamic parameters and the navigation information. The classic Kalman Filter (KF) and its extension for nonlinear systems Unscented Kalman Filter (UKF) are explained and used for solving the problem. A double-track vehicle model is implemented in the estimator for current use and further development. The results of all estimations are shown, and the mathematical evaluation of position estimates indicate that they outperform the original signals which are inputs to the sensor fusion algorithm. At last, some suggestions for further improvement are presented.

4 Nomenclature Notations Symbol λ ϕ h x e, y e, z e u u, v u, w u a x, a y, a z Re u Rb u s r eff r r ω V xwheel α v Xb, v Yb S v, S h X k Y k Q R υ η F yf, F yr ω z m m j w Description longitude in the geodetic system latitude in the geodetic system height between the measured point and reference ellipsoid along the direction of its normal coordinates in x/y/z direction in ECEF frame velocities in x/y/z direction in local ENU frame accelerations in x/y/z dirction in body-fixed frame transformation matrix from the ECEF frame to the local ENU frame transformation matrix from the local ENU frame to the body-fixed frame longitudinal slip effective rolling radius static tyre radius wheel angular velocity real longitudinal velocity at the axle of the wheel slip angle of tyres projections of wheel center velocity on the x and y axis in bodyfixed frame vertical and horizontal shift in the Magic Formula the state of the system at the time step k the measurement at the time step k process noise covariance measurement noise covariance sensor noise dynamic coefficient for velocity estimation lateral forces on front and rear tyre respectively in the bicycle model angular velocity of the vehicle body around z axis in the bicycle model vehicle total mass with two persons equivalent mass of rotating parts track width

5 l wheel base f distance from front axle to CoG J z body inertial around z axis C f, C r cornering stiffness of front and rear tyres a cor y corrected measurement of lateral acceleration ψ m measurement of yawrate of the vehicle Φ bank measurement of bank angle of the road x local, y local displacement measurements in x and y direction of Local frame x, y estimations of displacement in x and y direction b x, b y bias of x and y ω 1,2,3,4 angular velocity of each wheel δ 1,2,3,4 steering angle of each wheel v x, v y longitudinal velocity and lateral velocity in body-fixed frame ψ, ψ yaw angle and yawrate of the vehicle Abbreviations Symbol RCV ITRL ADAS IMU GPS ECEF ENU CoG KF UKF EKF RTK CEP Description Research Concept Vehicle Integrated Transport Research Lab Advanced Driver Assistance Systems Inertia Measurement Unit Global Positioning System Earth-Centered Earth-Fixed East-North-Up Center of gravity Kalman Filter Unscented Kalman Filter Extended Kalman Filter Real Time Kinematics Circular Error Probability VI

6 Contents 1 Introduction Background Problem Description Aim Project Limitation Theoretical Background Coordinate Systems Geodetic Coordinate System Earth-Centered Earth-Fixed Coordinate System Local East-North-Up Coordinate System Vehicle-Fixed East-North-Up Coordinate System Body-Fixed Coordinate System Coordinate Transformations Vehicle Modeling Tyre Models Single-track Model Double-track Model State Estimation Techniques The Kalman Filter The Unscented Kalman Filter Available Sensors Wheel Speed Sensor IMU and Steering angle sensor GPS Receiver Evaluation Method

7 3 Methodology RCV Platform Test Setup Filter Design Processing Input Signals Dynamics Block Navigation Block Results Results for Dynamics Block Straight Line and U-turn Test Slalom Test Circle Test Results for Navigation Block Straight Line and U-turn Test Slalom Test Circle Test Discussion 51 6 Conclusion and Future Work Conclusion Future Work More Data for Tuning and Validation Testing in Real-Time system More Complex Vehicle Model A Virtual Simulation Platform More sensors References 57

8 Chapter 1 Introduction 1.1 Background In the study of vehicle dynamics the estimation of vehicle states is crucial, which determines parameters, such as the longitudinal velocity, lateral velocity, yawrate. By taking these estimated parameters as inputs, controllers can be designed to keep a high level of handling performance as well as safety. For example, the Antilock Brake System relies on accurate estimation of longitudinal velocity, and the Electronic Stability Control system requires both accurate longitudinal velocity and yawrate. On the other hand, vehicle state estimation is also an important technique for the fault diagnosis on board. By comparing the estimates with corresponding measurements from the sensors, one can find faults in sensors in time and thus guarantee the system s robustness [1]. Since March 1995, when the first vehicle dynamic control system was introduced in the market, many car manufactures and institutes have been developing advanced driver assistance systems (ADAS) for contemporary internal combustion engine vehicles to improve handling and safety performances [2] [3]. At the same time, the concept of autonomous driving was brought which can improve the safety by eliminating human errors in driving. In the last decade, electric vehicles have become the focus of research as another solution to future transportation. And Tesla even released their electric vehicle with an autopilot functionality. The Research Concept Vehicle (RCV), which has been built by KTH Integrated Transport Research Lab (ITRL), is a pure electric vehicle with four in-wheel motors and individual steering as well as camber actuators, and serves as an experimental research and concept demonstration platform [4]. To equip the RCV with different ADAS and realize autonomous driving functionality in the future, building a state estimation algorithm is the essential as the first step. 1

9 CHAPTER 1. INTRODUCTION 1.2 Problem Description Vehicle state estimation is one of the key techniques in active vehicle dynamic control system which requires accurate estimations of vehicle parameters like longitudinal and lateral velocity, yaw rate and slip angle. On the other hand, good estimations of positions and heading are necessary for autonomous driving, specifically for path planning in our case. In order to fulfil the tasks mentioned above, measurements of different signals like wheel speed, acceleration, angular velocity and position will be combined together with the estimation algorithm. And the main problems involve choosing a suitable physical model, designing an estimation algorithm and tuning the estimator. 1.3 Aim The main purpose of this degree project consists in implementing a state estimation algorithm to provide accurate vehicle dynamic parameters (longitudinal and lateral velocity, yawrate) and navigation information (position and heading). Different sensors are utilized including the wheel speed sensor, the steering angle sensor (the position sensor), the integrated inertial measurement unit (IMU) and the global positioning system (GPS). Signals from these sensors serve as inputs to the estimator which generates the final estimation results. And the outcome of this project will be further used as inputs for path planning and vehicle dynamic control system design. 1.4 Project Limitation The parameters for calculating air drag are not available, such as frontal area and air drag coefficient. And the tyre specifications are not fully validated either, which limits the accuracy of the vehicle model. Since the magnetometer cannot be used because its signal will be corrupted by the electromagnetic field. The heading accuracy becomes lower. The GPS signals are preprocessed by Xsens MTi-G-700 which leads to different levels of accuracy depending on the vehicle state. This will influence the tuning of the estimator. 2

10 Chapter 2 Theoretical Background 2.1 Coordinate Systems Several different coordinate systems are used in modelling, data processing and analysis with regards to navigation studies in this thesis. Therefore a summary of adopted coordinate frames and coordinate transformations is given, which include [5] the geodetic coordinate system, the earth-centered earth-fixed (ECEF) coordinate system, the local east-north-up (ENU) coordinate system, the vehicle-fixed ENU coordinate system, and the body-fixed coordinate system. It is necessary to point out that our experimental vehicle normally runs on test tracks, which means all tests are carried out in small regions (track length less than 100 meters). And the vehicle s inherent design and power limitation make it run at a relatively low speed (lower than 10 m/s). Therefore some simplifications are made in the coordinate transformation to reduce the computational cost. 3

11 CHAPTER 2. THEORETICAL BACKGROUND Z e Y u (North) Z u (up) e:ecef u:enu X u (East) Prime Meridian O u O e φ Y e X e λ Equator h{ φ Figure 2.1: Geodetic, ECEF, and ENU coordinate systems Geodetic Coordinate System The current geodetic coordinate system (see Figure 2.1) is called WGS-84 (World Geodetic System 84) and it is normally used in GPS-based navigation. It uses a reference ellipsoid which is a mathematical surface model that approximates the geoid and serves as a basis for a coordinate system of longitude, latitude and height (elevation or altitude), which are denoted by λ, ϕ and h respectively. Then one measured point on earth can be represented by (λ, ϕ, h). The longitude (λ) measures the rotational angle between the Prime Meridian and the measured point, which ranges from -180 to 180. The latitude (ϕ) measures the angle between the equatorial plane and the normal of the reference ellipsoid that passes through the measured point, which is represented from -90 to 90. The height (h) measures the distance between the measured point and reference ellipsoid along the direction of its normal. Coordinate vectors expressed in this frame are denoted with a subscript g, i.e., the position is denoted by λ P g = ϕ (2.1) h Earth-Centered Earth-Fixed Coordinate System The ECEF coordinate system (see Figure 2.1) is a cartesian coordinate system and also known as earth centered rotational coordinate system, which implies it rotates 4

12 CHAPTER 2. THEORETICAL BACKGROUND with the earth. Therefore one fixed point on the earth has its unique coordinates (x e, y e, z e ). The origin (denoted by O e ) (0, 0, 0) is defined as the center mass of the earth. The X-axis intersects the sphere of the earth at 0 longitude and 0 latitude. The Z-axis is orthogonal to the equatorial plane and points towards the north pole. The Y-axis follows the right-hand rule and completes the coordinate system. Coordinate vectors expressed in this frame are denoted with a subscript e, i.e., the position is denoted by x e P e = y e (2.2) z e Local East-North-Up Coordinate System The ENU coordinate system (see Figure 2.1), also known as a ground coordinate system, is location dependent since its origin (denoted by O u ) is arbitrarily fixed to a point on the surface of the earth. The X-axis and Y-axis point towards the true east (geodetic east) and true north (geodetic north) respectively, and the Z-axis points upwards along the ellipsoid normal. This ground-fixed coordinate system is an inertial system and the navigation of our vehicle is carried out within this frame. The starting point is usually selected in each test as the origin of the local ENU frame. Coordinate vectors expressed in this frame are denoted with a subscript u, i.e., the position and velocity are denoted by x u u u P u = y u, V u = v u (2.3) z u w u Vehicle-Fixed East-North-Up Coordinate System The vehicle-fixed ENU coordinate system (see Figure 2.2) is attached to the vehicle with its origin locating at the vehicle s center of gravity (CoG). The X-axis and Y-axis point towards the ellipsoid east (geodetic east) and ellipsoid north (geodetic north) respectively, and the Z-axis points upwards along the ellipsoid normal. In general, the axis directions of the vehicle-fixed ENU frame are not aligned with those of the local ENU frame since the origin of the vehicle-fixed ENU frame varies while the vehicle moves. However due to the limitation of the test track as well as the speed of the vehicle, as mentioned earlier, the directional difference between the axes is negligible. Therefore the directions of the vehicle-fixed ENU coordinate system are assumed to coincide with those of the local ENU frame, and this assumption holds true throughout the thesis. 5

13 CHAPTER 2. THEORETICAL BACKGROUND Z vu Z b ψ Y vu b: Body coordinate vu: Vehicle-fixed ENU O vu /O b v y θ Y b X vu v x φ X b Figure 2.2: Vehicle-fixed ENU, and body-fixed coordinate systems Body-Fixed Coordinate System The body-fixed coordinate system (see Figure 2.2) is also a vehicle-carried frame. The origin is at the CoG of the body. The x-axis points forward along the longitudinal direction of the body and the z-axis is positively upwards. The y-axis points laterally with the right-hand rule. The orientations of this frame with respect to the local ENU frame are expressed by (ϕ, θ, ψ), which are also known as Euler angles. ϕ, θ and ψ are called roll angle, pitch angle and yaw angle respectively. Coordinate vectors expressed in this frame are denoted with a subscript b, i.e., the position, velocity and acceleration are denoted by x b v x P b = y b, V b = v y, a b = a y (2.4) z b v z a z The rigid-body dynamics of vehicles and the output of inertial sensors are described in this frame. a x Coordinate Transformations The purpose of coordinate transformations is to fuse the data measured by different sensors in different reference frames. For example, one way to gain the information of positions is to integrate the vehicle velocities in both longitudinal and lateral directions, which are measured in body-fixed coordinate system. Another way is to use GPS which can provide positional signals in geodetic coordinate system. And for the navigation study, the sensor fusion algorithm needs to combine these 6

14 CHAPTER 2. THEORETICAL BACKGROUND two sources of signals from different coordinate systems, which makes coordinate transformations necessary. Two coordinate transformations for navigation purpose are introduced, which include geodetic to local east-north-up (ENU) coordinate systems, local east-north-up (ENU) to body-fixed coordinate systems Geodetic and Local ENU Coordinate Systems This transformation is necessary for converting the GPS position measurements to the local ENU coordinate system and needs two steps to complete. The first step is from geodetic frame to ECEF frame. For a point expressed in geodetic system as P g (see Equation 2.1), its coordinate in the ECEF system is given by [6] x e (N + h) cos ϕ cos λ P e = y e = z e (N + h) cos ϕ sin λ (N(1 e 2 ) + h) sin ϕ (2.5) where N = a 2 (a cos ϕ)2 + (b sin ϕ) 2 (2.6) a = 6, 378, m (2.7) b 6, 356, m (2.8) e = m (2.9) In above equations, N is the radius of curvature in the prime vertical; a and b are the semi-major axis and semi-minor axis of the ellipsoid reference respectively, which are defined by WGS-84. e is the first eccentricity. The second step is the transformation from the ECEF frame to the local ENU frame. For this purpose, the origin of the local ENU coordinate system must be known as a premise. Define the position of the origin is represented by (x 0, y 0, z 0 ) in ECEF frame and (λ 0, ϕ 0, h 0 ) in geodetic frame, then the point P e can be expressed in the local ENU system as x u x e x 0 P u = y u = Re u y e y 0 (2.10) z u z e z 0 In equation 2.10, Re u is the transformation matrix from the ECEF frame to the local ENU frame, and it is given by sin λ 0 cos λ 0 0 Re u = cos λ 0 sin ϕ 0 sin λ 0 sin ϕ 0 cos ϕ 0 (2.11) cos λ 0 cos ϕ 0 sin λ 0 cos ϕ 0 sin ϕ 0 7

15 CHAPTER 2. THEORETICAL BACKGROUND Local ENU and Body-Fixed Coordinate Systems This transformation is for converting the position coordinates calculated in the bodyfixed frame to the local ENU frame, which is described as Euler rotations. Use the position for illustration, the relation and the transformation matrix are given by x u P u = y u = R u b y b z u z b (2.12) cos θ cos ψ sin ϕ sin θ cos ψ cos ϕ sin ψ sin ϕ sin ψ + cos ϕ sin θ cos ψ Rb u = cos θ sin ψ cos ϕ cos ψ + sin ϕ sin θ sin ψ cos ϕ sin θ sin ψ sin ϕ cos ψ sin θ sin ϕ cos θ cos ϕ cos θ (2.13) Since the incline of the test track is small, it is reasonable to assume small angles approximate to zero. Therefore in this transformation, the pitch angle θ and roll angle ϕ are assumed to be zero. And the new transformation matrix is simplified as cos ψ sin ψ 0 Re u = sin ψ cos ψ 0 (2.14) x b 2.2 Vehicle Modeling Vehicle modeling is a crucial part in the analysis of dynamics. The more precise the model is, the closer to the reality the simulation results will be. In this section, two dynamical models with different complexities are presented, i.e., single-track model and double-track model. And as an essential and pivotal part of them, different tyre models will be introduced as well Tyre Models Tyres contribute greatly to the dynamics of the vehicle by generating forces and moments in the tyre-road contact, which can alter the speed and orientation of the vehicle. To quantify the forces and moments for analyzing dynamic behaviour of the vehicle, several types of mathematical models of the tyre have been developed. And for different specific utilization, there are models with different levels of complexity and accuracy. These models can be divided into mainly two groups, namely empirical models and physical models. The empirical model relies on full scale tyre experiments and describes the measured characteristics through look-up tables or formulas statistically, i.e. the model fits the measurement data best. The physical model is more based on the physical mechanisms of the tyre behaviour, and it is more complex and accurate [7]. However, because of the complexity the physical 8

16 CHAPTER 2. THEORETICAL BACKGROUND model is more suitable for analysis in theory rather than for implementing in realtime experiments. In the following three common tyre models will be introduced, which include the linear model, the Magic Formula and the brush model. And camber angle is assumed to be zero for all. Before the tyre models are presented, a key concept called slip will be explained Slip Slip occurs when external forces act on the tyre, which can be either in longitudinal or lateral direction. Longitudinal Slip The difference between the circumferential velocity ωr eff of the tire and the real longitudinal velocity at the axle of the wheel V xwheel is called the longitudinal slip. And the longitudinal slip ratio is define by s = ωr eff V xwheel max(ωr eff, V xwheel ) (2.15) where r eff is the effective rolling radius defined as the ratio of the wheel center velocity in the longitudinal direction v xwheel of a free rolling tyre (no traction or braking force applied) to the angular velocity of the free rolling wheel ω by r eff = v xwheel ω (2.16) Lateral Slip The slip angle α is defined as the angle between the orientation of the velocity vector of the wheel and the orientation of the wheel plane (see Figure 2.3). One way to calculate the slip angle is to use v Xb and v Yb which are the projections of velocity at the wheel center in the body frame and the steering angle δ. The expression is given by α = v Y b v Xb δ (2.17) v x α 12/34 δ12/34 X b y F y12/34 wheel plane Figure 2.3: slip angle at front/rear tyre 9

17 CHAPTER 2. THEORETICAL BACKGROUND It is necessary to point out that the pure slip in each direction can be combined together when more than one type of slip occurs. The combined slip case is more complex since the limitations appear not only on the maximum forces in x/y direction but also on the available maximum resultant force. Assume longitudinal slip is zero since no severe driving is expected in this study. So the combined slip case is not considered and only slip angle in lateral direction will be discussed in the following part The Linear Model When the slip angle is small, the lateral force is approximately proportional to the slip angle (see Figure 2.4). Assume that all tyres work in the elastic region, then the lateral force generated by the tyre deformation can be represented as F y C α α (2.18) where the cornering stiffness C α of the tyre is defined by ( ) Fy C α = α α=0 (2.19) Figure 2.4: An example of lateral force versus slip angle [8] The Magic Formula In the preceding section the simple linear model has been applied in models with low dynamic cases when the slip angle is small. However for large slip angles, more sophisticated models are required to keep the accuracy. The Magic Formula [9] 10

18 CHAPTER 2. THEORETICAL BACKGROUND provides a method that can fit the tyre characteristics well in a wide range including large slip angles. It can calculate aligning moment, longitudinal and lateral tyre forces as well as combined force of these two. The expression for longitudinal force and lateral force is similar with only different inputs which are slip ratio s and slip angle α respectively. The lateral force F y as a function of the input slip angle α is given by F y = D sin(c arctan(bx E(Bx arctan(bx)))) + S v x = α + S h (2.20) where S h is the horizontal shift and S v is the vertical shift B is the stiffness factor C is the shape factor D is the peak factor E is the curvature factor the product BCD in this formula equals the cornering stiffness C α F y, Fx Mz F x M z α F y The Brush Model Slip (angle) Figure 2.5: Steady-state tyre characteristics [10] Unlike the two empirical models above, the brush model is more physically based. It can provide a better understanding of tyre working principles in relation to its physical construction. The model assumes the tread rubber as flexible bristles which can deflect in a direction parallel to the road surface. When slip occurs in either longitudinal or lateral direction, the bristles deflect in the corresponding direction and thus generate force in the contact zone. Figure 2.6 depicts the generation of longitudinal force in the brush model. Since the brush model is not used in this 11

19 CHAPTER 2. THEORETICAL BACKGROUND thesis, the derivation of expressions of two forces and aligning torque will not be explained here. Please refer to [7] and [11]. z v x ω x F x F z Figure 2.6: Bristle deformation during accelerating Single-track Model The single-track model, also known as the bicycle model, is the simplest dynamic vehicle model, of which two wheels per axle are replaced with one single wheel. The main advantage is that it can describe different dynamic behaviors of the vehicle reasonably under various conditions without complicated modeling and parametrization. A few assumptions are made to meet the simplification: the vehicle is y-axis symmetric and the height of its center of gravity is zero, which means it moves in the xy-plane, roll, pitch and jump motions are negligible, longitudinal and lateral load transfer are negligible too, no aerodynamic effect, tyres remain in the linear region (a y 0.4g), which means small slip angles and steering angles, constant longitudinal velocity. Front-wheel-steering is adopted in almost every production car and the typical bicycle model is based on this particular configuration. For the purpose of this thesis, I will introduce a modified bicycle model (see Figure 2.7) with four-wheel-steering capability since the RCV is able to steer with all four wheels. 12

20 CHAPTER 2. THEORETICAL BACKGROUND Y u X b δ 12 F y12 f v x, a x l v y, a y δ 34 ω z b Y b F y34 ψ Figure 2.7: Single-track bicycle model with front/rear-wheel-steering As mentioned above, the vehicle moves in xy-plane and has a planar motion with three degrees of freedom, i.e., translational motions in x and y directions and rotational motion around the z direction. According to Newton s second law, the equations of motion attached to the CoG are expressed in the body-fixed coordinate system by: F x = ma x = m( v x ω z v y ) (2.21) F y = ma y = m( v y + ω z v x ) (2.22) M z = J z ω z = F yf f F yr b (2.23) where F x is the projection of F yi (i = 12, 34) onto the x-axis (i.e. X b ) in body-fixed coordinate frame. F y is the projection of F yi (i = 12, 34) onto the y-axis (i.e. Y b ) in body-fixed coordinate frame. F yf and F yr are components of F y at the front and rear tyre respectively. The inertial acceleration a x/y consists of two terms: the acceleration v x/y which is caused by the motion along the x/y-axis and the centripetal acceleration ω z v y/x. In the simple bicycle model the longitudinal velocity is assumed constant, so Equation 2.21 will not be considered here. According to the assumption in regard to tyres of bicycle model, the linear tyre model is adopted and the lateral forces can be calculated based on Equation 2.17 and Equation Combining with small angle assumptions, the matrix expression of the simple bicycle model is ( vÿ ψ ) = ( C 12 +C 34 mv x v x C 12f C 34 b mv x C 12f C 34 b J zv x C 12f 2 +C 34 b 2 J zv x ) ( vy ψ ) ( C12 + m C 12 f J z C 34 m C 34b J z X u ) ( ) δ12 (2.24) δ 34 13

21 CHAPTER 2. THEORETICAL BACKGROUND Double-track Model Double-track model aims to model the whole vehicle with high complexity to improve the accuracy of the model. In order to incorporate the effects of tyre forces on each wheel and have the potential to involve the roll and pitch dynamics in the future model, the double-track model (see Figure 2.8) will be implemented in the state estimation algorithm. Assuming constant longitudinal velocity results in zero resultant force in X b direction. The representations of the dynamics are shown as F y = ma y = m( v y + ω z v x ) = F y1 cos δ 1 + F y2 cos δ 2 + F y3 cos δ 3 + F y4 cos δ 4 (2.25) M z = J z ω z = F y1 cos δ 1 f + F y1 sin δ 1 w f 2 + F y2 cos δ 2 f w f F y2 sin δ F w r y3 sin δ 3 2 F w r y3 cos δ 3 b F y4 cos δ 4 b F y4 sin δ 4 2 (2.26) The tyre model is chosen according to the application and normally the linear tyre model works fine for low dynamic cases. And the magic formula can be implemented for high dynamic scenarios. The expressions of tyre forces will be listed in the next chapter. Y u X b δ 2 w f δ 1 F y2 v x, a x f l δ 4 F y1 v y, a y ω z b Y b δ 3 F y4 w r ψ F y3 Figure 2.8: Double-track model with front/rear-wheel-steering In both vehicle models above, the assumption of constant longitudinal velocity holds and decreases the degree of freedom to 2 thus simplifying the problem. However, one can always model the longitudinal dynamics and achieve higher accuracy in high dynamic cases with large slip angles. But in this specific application it is difficult to reach this goal since some forces like the aerodynamic force are hard to estimate as well as the rolling friction due to the lack of related parameters. And the speed sensor that samples at 33Hz makes it applicable to assume quasi static at each sample point, i.e. the longitudinal speed is constant. 14 X u

22 CHAPTER 2. THEORETICAL BACKGROUND 2.3 State Estimation Techniques The Kalman filter [12], named after Rudolf E. Kalman, is the best possible linear filter. It s a recursive estimator that can minimize the covariance and estimate the states as unbiased as possible. Applications of Kalman filters range from navigation and dynamic state estimation to general signal processing, but with the limitation of linear systems. To tackle the problems with non-linearity, several approaches can be applied. One way is to use the Taylor series expansion which yields a Gaussian approximation to the true belief. This method is called extended Kalman filter (EKF). Another way is based on unscented transform that linearizes the system through the use of some weighted points called sigma points. This method is called unscented Kalman filter (UKF) [13]. Compared to EKF, UKF benefits from the unscented transform for linearization and has the same high efficiency and better estimations in general. While the performance of EKF is influenced by spread of the prior state uncertainty and nonlinearities which might also make the computation of Jacobians difficult [14]. In this section, Kalman filter and unscented Kalman filter will be briefly introduced The Kalman Filter A linear system will be presented by its state space model having state vector X R n and observation vector Y R m in Equation X k = A k 1 X k 1 + B k 1 U k 1 + q k 1 Y k = H k X k + r k (2.27) X k is the state of the system at the time step k. Y k is the measurement at the time step k. q k 1 N(1, Q k 1 ) is the process noise at the time step k 1. r k N(0, R k ) is the measurement noise at the time step k. A k 1 is the transition matrix of the dynamic model. B k 1 is the input matrix. U k 1 is the control input vector to the dynamic model. H k is the observation model matrix. Two elements that propagate through the iteration of the Kalman filter are the posteriori state estimate X k k and the posteriori estimate covariance P k k. The whole algorithm is initialized with X 0 0 and P 0 0 which can be guessed if the initial values are unknown and in most cases the results will converge. But sometimes a good 15

23 CHAPTER 2. THEORETICAL BACKGROUND estimation of initial values can influence the behavior of the Kalman filter, for example the convergence time. Since the Kalman filter follows a recursive way, the posteriori X k 1 k 1, P k 1 k 1 from last time step will be used to calculate the priori X k k 1, P k k 1 at the current time step, which is the first step called Predict. In the second step Update, the predicted state estimates and covariances are corrected based on the measurements (or observation) Y k at the current time step resulting in the posteriori X k k, P k k. The whole process is shown below [15]. Algorithm 1 The Kalman Filter Predict ˆX k k 1 = A k 1 ˆXk 1 k 1 + B k 1 U k 1 P k k 1 = A k 1 P k 1 k 1 A T k 1 + Q k 1 Update V k = Y k H k ˆXk k 1 Innovation S k = H k P k k 1 Hk T + R k Measurement Prediction Covariance K k = P k k 1 Hk T S 1 k Kalman Gain ˆX k k = ˆX k k 1 + K k V k ˆP k k = P k k 1 K k H k P k k The Unscented Kalman Filter When the system is nonlinear, the Kalman filter is not directly applicable any more. Either the process or the measurement model or both must be nonlinear in this case and have the form x k = f(x k 1, u k 1 ) + q k 1 y k = h(x k ) + r k (2.28) where x R n is the state vector and y R m is the measurement vector, q k 1 N(1, Q k 1 ) is the process noise, r k N(0, R k ) is the measurement noise, f is the dynamic model function and h is the measurement model function. Before implementing the Kalman filter linearization of the system is required. The UKF, as mentioned above, can achieve this through unscented transform. It only takes a minimal set of sample points (called sigma points) around the mean of a Gaussian. The sigma points are propagated through the nonlinear functions and the estimated mean and covariance can be recovered from the propagation. For a random variable x (dimension n) with a mean x and a covariance P x, there will be 2n + 1 sigma points χ i and their corresponding weights W i : χ 0 = x χ i = x + ( (n + λ)p x ) i χ i = x ( (n + λ)p x ) i n W m 0 = λ/(n + λ) i = 1,..., n i = n + 1,..., 2n W c 0 = λ/(n + λ) + (1 α 2 + β) W m i = W c i = 1/2(n + λ) 16 (2.29) (2.30)

24 CHAPTER 2. THEORETICAL BACKGROUND where λ = α 2 (n + κ) n is a scaling parameter. α determines the spread of the sigma points around the mean value x, κ is another scaling parameter and β indicates prior knowledge of the distribution of x. Empirical values are adopted here: α = 0.001, β = 2, κ = 0. The whole algorithm is shown below [16]. Algorithm 2 The Unscented Kalman Filter Initialized with ˆx 0 = E[x 0 ] P 0 = E[(x 0 ˆx 0 )(x 0 ˆx 0 ) T ] Predict χ k 1 k 1 = [ˆx k 1 k 1... ˆx k 1 k 1 ] + [0 (n + λ)p k 1 k 1 (n + λ)p k 1 k 1 ] ˆχ k k 1 = f(χ k 1 k 1, u k 1 ) ˆx k k 1 = P k k 1 = 2n i=0 2n i=0 W m i ˆχ i,k k 1 W c i [ˆχ i,k k 1 ˆx k k 1 ][ˆχ i,k k 1 ˆx k k 1 ] T + Q k 1 χ k k 1 = [ˆx k k 1... ˆx k k 1 ] + [0 (n + λ)p k k 1 (n + λ)p k k 1 ] Ŷ k k 1 = h(χ k k 1 ) ŷ k k 1 = Update S k = C k = 2n i=0 2n i=0 2n i=0 W m i Ŷ i,k k 1 W c i [Ŷi,k k 1 ŷ k k 1 ][Ŷi,k k 1 ŷ k k 1 ] T + R k W c i [χ i,k k 1 ˆx k k 1 ][Ŷi,k k 1 ŷ k k 1 ] T K k = C k S 1 k ˆx k k = ˆx k k 1 + K k [y k ŷ k k 1 ] P k k = P k k 1 K k S k K T k 2.4 Available Sensors Sensors to robots are like eyes to animals. The number and the quality determine the visual field and the clarity respectively, which means more high-quality sensors can provide more accurate information. However, the selection of sensor is always cost dependent. One has to make a trade-off between the cost and the estimation results. A proper selection of sensors should be the most cost-effective among those that are able to collect enough information for estimation algorithms. On the other hand, the selection of an estimation algorithm also influences the set-up of sensors. Less signals from sensors requires more complex algorithms including more tuning parameters, which can decrease the feasibility of the algorithm in real-time system [1]. So the feasibility of algorithms is the other factor when choosing sensors. 17

25 CHAPTER 2. THEORETICAL BACKGROUND Given the purpose, some specific sensors will be used to provide the corresponding signals. There are mainly two kinds of challenges to be solved in this thesis, one is about vehicle dynamics with regards to longitudinal and lateral velocities and yaw motion, the other one is navigation which involves finding the positions and heading of the car. For vehicle dynamic analysis the common sensors are wheel speed sensor, steering angle sensor, accelerometer, gyroscope, throttle and brake pedal sensor. Some other sensors can even provide the moment of each wheel and the moment of the steering wheel. Sensors used in navigation can be quite expensive and accurate as well, like a GPS sensor with very high accuracy. An accurate GPS receiver with real time kinematics (RTK) functionality has centimeter level positioning accuracy but also cost over 100,000 Swedish kronor. Cheap GPS receivers with much lower accuracy are always combined with other sensors and algorithms to provide better results. Based on requirements and constrains mentioned above, a wheel speed sensor, an inertial measurement unit (IMU) which includes accelerometers and gyroscopes for X/Y/Z direction, a steering angle sensor and a GPS receiver are selected. The wheel speed sensor is used to estimate the longitudinal velocity. The IMU combines with the steering angle sensor to analyze lateral velocity and yaw rate. The signals of heading and positions are received by the GPS receiver. More details of sensors are given below Wheel Speed Sensor The wheel speed sensor is very common in modern cars as an essential component in ABS. It is an encoder (consisting of a Hall sensor) attached to the wheel axle measuring the angular velocity of the wheel. Due to this mechanism, error and noise are generated during the process of electromagnetic conversion. Another error source comes from the variation in wheel radius (r r ) since the velocity of the wheel is v wheel = ωr r. There are some reasons causing the change of wheel radius like tyre pressure and wear and tear. And one of these becomes significant when the vehicle is moving, especially in high dynamic scenario. The wheel radius in this case is called dynamic rolling radius or effective rolling radius (r eff ) which is different from static radius when the vehicle stands still. Then the wheel velocity can be modeled as ˆV wheel = ωr eff + υ v (2.31) where ˆV wheel is the estimated real velocity, ω represents measurements from the speed sensor and υ v is the measurement noise of the sensor IMU and Steering angle sensor The IMU adopted here is embedded in a motion tracker called Xsens MTi-G-700 (see Figure 2.9). It can measure the acceleration along X/Y/Z axis and the change of 18

26 CHAPTER 2. THEORETICAL BACKGROUND angle around these three axes by using accelerometers and gyroscopes respectively. Measurements from both instruments have inherited bias and noise because of the imprecision in manufacturing and installation. Particularly for accelerometer the bias of the measurement may come from the tilted plane where it is installed on. For example if the accelerometer is attached to a vehicle which stands still on an inclined road, the measurement will have a constant offset from 0. This is due to the physical design of the accelerometer and can be eliminated if the tilt angle is known. The gyroscope does not have this source of error and the bias can always be omitted so it is normally easier to have a more accurate gyroscope than accelerometer. The mathematical models of them are given below a m = a + b a + υ a (2.32) ψ m = ψ + υ ψ (2.33) where on the left side of the equations are the measurements from IMU. Acceleration a and angular velocity ψ are actual values, b a is the acceleration bias caused by the inclined road, υ a and υ ψ are noise signals. The steering angle δ is measured by the position sensor in the steering actuator, and normally the signal can be used directly. So it is modeled as δ m = δ (2.34) GPS Receiver Figure 2.9: Xsens MTi-G-700 motion tracker [17] The GPS receiver can receive signals from GPS satellites for navigation purposes. The GPS basically provides information of location and time, based on these the velocity and course angle of one object can also be obtained. In this study, a 4Hz GPS receiver which is embedded in the Xsens MTi-G-700 will be adopted. The positioning accuracy is 2.5m CEP and no heading accuracy is given. The GPS measurement is expressed by Y m = Y + b Y + υ Y (2.35) where Y m is the measurement and Y is the actual value. b Y is the bias but not always existing, which depends on the quality of output signal from the GPS receiver. υ Y is the measurement noise. 19

27 CHAPTER 2. THEORETICAL BACKGROUND 2.5 Evaluation Method The evaluation method is to describe the positioning error of the estimation. To be consistent with what is used by the Xsens and Trimble, the circular error probability (CEP) [18] is introduced. The CEP is a measure of positioning accuracy which is defined as the radius of a circle centered at the mean, whose boundary is expected to include 50% of all the estimated points. Obtaining the CEP in 2-D, first the standard deviations of points in x and y direction need to be calculated. σ = 1 N (x i µ) N 2 (2.36) i=1 where N is the number of all points, µ is the mean of x i and in our case it represents the ground truth. Finally the CEP can be calculated using the standard deviation. CEP = 0.62σ y σ x (2.37) 20

28 Chapter 3 Methodology 3.1 RCV Platform The Research Concept Vehicle (RCV) (see Figure 3.1) is a prototype vehicle built by KTH Integrated Transport Research Lab for experimental research and concept demonstration. The RCV is a steer-by-wire electric vehicle with individual hub motor and steering actuator in each wheel corner module which also includes individual camber actuator. Because of this high level of actuation, the RCV is capable of four-wheel drive and four-wheel steering as well as controlling the translational (not fully controlled in lateral direction) and yaw motion. The specifications are listed in Table 3.1. Figure 3.1: The Research Concept Vehicle 21

29 CHAPTER 3. METHODOLOGY Table 3.1: The RCV Specifications Name Value Unit m Vehicle total mass with two persons 608 kg m j Equivalent mass of rotating parts 40 kg w Track width 1.5 m l Wheel base 2 m f Distance from front axle to CoG 1.16 m r r Tyre radius 0.31 m J z Inertial around z-axis 1500 kgm 2 C f /C r Cornering stiffness 23 kn/rad δ f /δ r Steering angle interval [ 0.24, 0.24] rad ɛ Camber angle interval [ 15, 10] deg The RCV is equipped with a dspace 1401/1501 MicroAutoBox computer which can process all signals from actuators, motors, sensors and driver inputs and act accordingly by sending orders to different parts of the system [19]. All signals are transmitted through Controller Area Network (CAN). The angular velocity of the wheel is measured by an encoder in the motor, and the steering angle signal is measured by a sensor in the actuator. Besides these two build-in sensors, an external sensor Xsens MTi-G-700 motion tracker is attached to the vehicle to provide signals of accelerations, rotational velocities and GPS for normal dynamics study. To validate the estimation results, a VBOX 3i data logger (see Figure 3.2) and a Trimble GPS receiver SPS852 (see Figure 3.3) which can provide more accurate measurements will be used. The VBOX with the IMU integration functionality can measure both longitudinal and lateral velocity, and the accuracy of velocity is 0.1km/h. The RTK enabled Trimble receives correction signals from a local base station and can provide the position information with the accuracy of 10cm CEP. Figure 3.2: VBOX 3i data logger[20] 3.2 Test Setup Figure 3.3: Trimble GPS receiver[21] The purpose of performing tests is to record real world data for validating the estimation algorithm. The tests involve both low and high dynamic scenarios which cover all normal driving situations. Since the position estimation in this study is built for autonomous driving and the driving needs to be smooth and has low speed, the tests for this purpose can merge with the tests for low-dynamic study. The different scenarios are listed below. 22

30 CHAPTER 3. METHODOLOGY Straight line test U-turn test Slalom test Circle test The test track (see Figure 3.4) is located near Arlanda airport in Stockholm. Two parts of the track tilting in opposite direction along x direction (from A to B to C in Figure 3.4). And the whole track is also inclined in the direction which is perpendicular to l 1 and l 2. The road profile is measured by Mohammad Mehdi Davari using a Trimble S5 and the information is given in Table 3.2. l 2 l 1 A B C l 1 bank slope l 2 Figure 3.4: The Arlanda Test Track Table 3.2: The Test Track Information Section AB Section BC slope 0.010rad 0.050rad bank 0.021rad 0.024rad 23

31 CHAPTER 3. METHODOLOGY 3.3 Filter Design In this section implementations of the algorithm will be explained in detail. The content follows the logic order of the model. Section describes the processing of raw signals before they can be fed to the main algorithm. Section involves details of the estimation algorithm and logic for dynamical states. Section comprises state estimation of positions Processing Input Signals Some raw signals from the measurement cannot be used directly and need to be processed to fit the model. There are two types of preprocessing, one is data correction and the other is data transformation. Data correction removes known bias from measurements, and data transformation indicates converting the measurements from their original coordinates to the coordinate that every parameter is expressed in. Only the lateral acceleration signal is required to be corrected for being corrupted by the bank angle of the test track. Based on Equation 2.32, the corrected lateral acceralation a cor y is a cor y = a m y b ay = a m y gsin(φ bank ) (3.1) where a m y is the measurement from IMU, φ bank is the bank angle of the track, g is the gravitational acceleration and equals to 9.81m/s 2 (This value is applied for all cases in this thesis). As noticed the sensor noise is not included in Equation 3.1 and all other measurement noise in the following part will not be shown in similar case as well. Instead, all sensor noises will be comprised in the noise covariance matrix Q k of unscented Kalman filter/kalman filter. Coordinate transformation is taken for GPS signals, i.e. heading ν, longitude λ and latitude ϕ, since these signals are measured in the geodetic coordinate system. First a local ENU frame is defined which has its origin fixed at the CoG of the vehicle when it just starts to move. Knowing the position (in longitude and latitude) of the origin, it is easy to do the transformation based on the method in Section Then this local ENU frame rotates around Z u to coincide with the body fixed frame (omitting the road tilt angle ) and the rotational angle is just the initial heading of the vehicle. This step can be done using the transformation matrix in Section , and small angle approximation will hold as explained there. From Equation 2.5 and Equation 2.10, one can calculate the length of one degree of longitude and latitude which will decrease the computation in the model. Note that the body-fixed frame when the vehicle just about to start is defined as the Local frame. In this study all variables will be expressed in this Local frame, and the altitude is always assumed constant. The whole process of the transformation is expressed mathematically as x u = (λ λ 0 ) y u = (ϕ ϕ 0 ) (3.2) 24

32 CHAPTER 3. METHODOLOGY x local = x u cos ν 0 + y u sin ν 0 y local = y u cos ν 0 x u sin ν 0 (3.3) ψ m = ν ν 0 where the variables with subscript 0 belong to the initial point, x local and y local are the displacements in the Local frame. ψ m is the heading in the Local frame and its initial value should be 0. The initial heading ν 0 needs to be measured very accurately every time before tests, otherwise the local ENU frame cannot coincide with the Local frame perfectly. Due to the limitation of current instruments, the alignment cannot be performed very well. This becomes one source of error. Except the signals mentioned above, other signals will be input to the algorithm directly. All inputs are shown in the table below. Table 3.3: List of Input Signals to Main Algorithm Symbol Name Unit a cor y corrected lateral acceleration m/s 2 ψ m measurement of yaw-rate of the vehicle rad/s φ bank measurement of bank angle of the road rad ψ m heading measurement in Local frame rad x local displacement in x direction of Local frame m y local displacement in y direction of Local frame m ω 1,2,3,4 angular velocity of each wheel rad/s δ 1,2,3,4 steering angle of each wheel rad Dynamics Block In this block four states will be estimated i.e. longitudinal velocity v x, lateral velocity v y, yawrate ψ and yaw angle ψ. There are mainly two approaches to solve this problem, one is to use the kinematic model-based estimator and the other one is dynamic model-based estimator [22]. The kinematic model is sensitive to the accuracy of the sensor signals which is influenced by the installation and calibration of the sensor. In our case the acceleration signals are too noisy and the installation requirement cannot be fulfilled really well, which potentially results in large drift errors. On the contrary, the dynamical model does not have high standard for sensors but is sensitive to model parameters and not modeled dynamics. But the drawbacks of dynamical model can be compensated by the advantages of unscented Kalman filter. The process noise covariance matrix can be modeled as uncertainties of the vehicle model and the measurement in Update step can give additional information of the dynamic behavior especially in high dynamic case. So the dynamic modelbased estimator is chosen here. As described in Section the vehicle model can 25