STOCKHOLM SWEDEN 2016,
State-Estimator Design for the KTH Research Concept Vehicle
FAN GAO
KTH ROYAL INSTITUTE OF TECHNOLOGY SCHOOL OF ENGINEERING SCIENCES
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.
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 exper- imental 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 solv- ing 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.
Notations
Symbol Description
λ longitude in the geodetic system ϕ latitude in the geodetic system
h height between the measured point and reference ellipsoid along the direction of its normal
xe, ye, ze coordinates in x/y/z direction in ECEF frame uu, vu, wu velocities in x/y/z direction in local ENU frame ax, ay, az accelerations in x/y/z dirction in body-fixed frame
Rue transformation matrix from the ECEF frame to the local ENU frame
Rub transformation matrix from the local ENU frame to the body-fixed frame
s longitudinal slip
reff effective rolling radius rr static tyre radius
ω wheel angular velocity
Vxwheel real longitudinal velocity at the axle of the wheel α slip angle of tyres
vXb, vYb
projections of wheel center velocity on the x and y axis in body- fixed frame
Sv, Sh vertical and horizontal shift in the Magic Formula Xk the state of the system at the time step k
Yk the measurement at the time step k Q process noise covariance
R measurement noise covariance
υ sensor noise
η dynamic coefficient for velocity estimation
Fyf, Fyr lateral forces on front and rear tyre respectively in the bicycle model ωz
angular velocity of the vehicle body around z axis in the bicycle model
m vehicle total mass with two persons mj equivalent mass of rotating parts
w track width
Jz body inertial around z axis
Cf, Cr cornering stiffness of front and rear tyres acory corrected measurement of lateral acceleration ψ˙m measurement of yawrate of the vehicle
Φbank measurement of bank angle of the road
xlocal, ylocal displacement measurements in x and y direction of Local frame x, y estimations of displacement in x and y direction
bx, by bias of x and y
ω1,2,3,4 angular velocity of each wheel δ1,2,3,4 steering angle of each wheel
vx, vy longitudinal velocity and lateral velocity in body-fixed frame ψ, ˙ψ yaw angle and yawrate of the vehicle
Abbreviations
Symbol Description
RCV Research Concept Vehicle
ITRL Integrated Transport Research Lab ADAS Advanced Driver Assistance Systems IMU Inertia Measurement Unit
GPS Global Positioning System ECEF Earth-Centered Earth-Fixed
ENU East-North-Up
CoG Center of gravity
KF Kalman Filter
UKF Unscented Kalman Filter EKF Extended Kalman Filter
RTK Real Time Kinematics
CEP Circular Error Probability
1 Introduction 1
1.1 Background . . . 1
1.2 Problem Description . . . 2
1.3 Aim . . . 2
1.4 Project Limitation . . . 2
2 Theoretical Background 3 2.1 Coordinate Systems . . . 3
2.1.1 Geodetic Coordinate System . . . 4
2.1.2 Earth-Centered Earth-Fixed Coordinate System . . . 4
2.1.3 Local East-North-Up Coordinate System . . . 5
2.1.4 Vehicle-Fixed East-North-Up Coordinate System . . . 5
2.1.5 Body-Fixed Coordinate System . . . 6
2.1.6 Coordinate Transformations . . . 6
2.2 Vehicle Modeling . . . 8
2.2.1 Tyre Models . . . 8
2.2.2 Single-track Model . . . 12
2.2.3 Double-track Model . . . 14
2.3 State Estimation Techniques . . . 15
2.3.1 The Kalman Filter . . . 15
2.3.2 The Unscented Kalman Filter . . . 16
2.4 Available Sensors . . . 17
2.4.1 Wheel Speed Sensor . . . 18
2.4.2 IMU and Steering angle sensor . . . 18
2.4.3 GPS Receiver . . . 19
2.5 Evaluation Method . . . 20
3.1 RCV Platform . . . 21
3.2 Test Setup . . . 22
3.3 Filter Design . . . 24
3.3.1 Processing Input Signals . . . 24
3.3.2 Dynamics Block . . . 25
3.3.3 Navigation Block . . . 29
4 Results 31 4.1 Results for Dynamics Block . . . 31
4.1.1 Straight Line and U-turn Test . . . 31
4.1.2 Slalom Test . . . 34
4.1.3 Circle Test . . . 37
4.2 Results for Navigation Block . . . 40
4.2.1 Straight Line and U-turn Test . . . 40
4.2.2 Slalom Test . . . 44
4.2.3 Circle Test . . . 46
5 Discussion 51 6 Conclusion and Future Work 55 6.1 Conclusion . . . 55
6.2 Future Work . . . 56
6.2.1 More Data for Tuning and Validation . . . 56
6.2.2 Testing in Real-Time system . . . 56
6.2.3 More Complex Vehicle Model . . . 56
6.2.4 A Virtual Simulation Platform . . . 56
6.2.5 More sensors . . . 56
References 57
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 Anti- lock 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 ve- hicles 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 elim- inating 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.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.
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.
Ze
Xe
Ye Zu(up) Yu(North)
Xu(East)
Oe
Ou
λ φ
φ
{
h
Equator Prime Meridian
e:ECEF u:ENU
Figure 2.1: Geodetic, ECEF, and ENU coordinate systems
2.1.1 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 (λ) mea- sures 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
Pg =
λ ϕ h
(2.1)
2.1.2 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
with the earth. Therefore one fixed point on the earth has its unique coordinates (xe, ye, ze). The origin (denoted by Oe) (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
Pe =
xe ye
ze
(2.2)
2.1.3 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 Ou) 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
Pu =
xu yu zu
, Vu =
uu vu wu
(2.3)
2.1.4 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.
Yvu Zvu
Xvu
Xb Yb
Zb
θ
φ
ψ
b: Body coordinate vu: Vehicle-fixed ENU
vx
vy
Ovu/Ob
Figure 2.2: Vehicle-fixed ENU, and body-fixed coordinate systems
2.1.5 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 longitu- dinal 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
Pb =
xb yb zb
, Vb =
vx vy vz
, ab =
ax ay az
(2.4)
The rigid-body dynamics of vehicles and the output of inertial sensors are described in this frame.
2.1.6 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
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.
2.1.6.1 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 Pg (see Equation 2.1), its coordinate in the ECEF system is given by [6]
Pe=
xe
ye ze
=
(N + h) cos ϕ cos λ (N + h) cos ϕ sin λ (N (1 − e2) + h) sin ϕ
(2.5)
where
N = a2
p(a cos ϕ)2+ (b sin ϕ)2 (2.6)
a = 6, 378, 137.0 m (2.7)
b ≈ 6, 356, 752.314245 m (2.8)
e = 0.08181919 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 (x0, y0, z0) in ECEF frame and (λ0, ϕ0, h0) in geodetic frame, then the point Pe can be expressed in the local ENU system as
Pu =
xu yu zu
= Rue
xe− x0 ye− y0 ze− z0
(2.10)
In equation 2.10, Rue is the transformation matrix from the ECEF frame to the local ENU frame, and it is given by
Rue =
− sin λ0 cos λ0 0
− cos λ0sin ϕ0 − sin λ0sin ϕ0 cos ϕ0 cos λ0cos ϕ0 sin λ0cos ϕ0 sin ϕ0
(2.11)
2.1.6.2 Local ENU and Body-Fixed Coordinate Systems
This transformation is for converting the position coordinates calculated in the body- fixed 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
Pu =
xu yu
zu
= Rub
xb yb
zb
(2.12)
Rub =
cos θ cos ψ sin ϕ sin θ cos ψ − cos ϕ sin ψ sin ϕ sin ψ + cos ϕ sin θ cos ψ 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
Rue =
cos ψ − sin ψ 0 sin ψ cos ψ 0
0 0 1
(2.14)
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.
2.2.1 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 com- plexity 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
model is more suitable for analysis in theory rather than for implementing in real- time experiments. In the following three common tyre models will be introduced, which include the linear model, the Magic Formula and the brush model. And cam- ber angle is assumed to be zero for all. Before the tyre models are presented, a key concept called slip will be explained.
2.2.1.1 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 ωreff of the tire and the real longitudinal velocity at the axle of the wheel Vxwheel is called the longitudinal slip. And the longitudinal slip ratio is define by
s = ωreff − Vxwheel
max(ωreff, Vxwheel) (2.15)
where reff is the effective rolling radius defined as the ratio of the wheel center velocity in the longitudinal direction vxwheel of a free rolling tyre (no traction or braking force applied) to the angular velocity of the free rolling wheel ω by
reff = vxwheel
ω (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 vXb and vYb which are the projections of velocity at the wheel center in the body frame and the steering angle δ. The expression is given by
α = vYb
vXb − δ (2.17)
y x v
Xb
α12/34 δ1 2 / 34
wheel plane Fy1 2 / 3 4
Figure 2.3: slip angle at front/rear tyre
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 direc- tion 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.
2.2.1.2 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
Fy ≈ −Cαα (2.18)
where the cornering stiffness Cα of the tyre is defined by Cα = − ∂Fy
∂α
α=0
(2.19)
Figure 2.4: An example of lateral force versus slip angle [8]
2.2.1.3 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]
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 Fy as a function of the input slip angle α is given by
Fy = D sin(C arctan(Bx − E(Bx − arctan(Bx)))) + Sv
x = α + Sh (2.20)
where
• Sh is the horizontal shift and Sv 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α
α
Slip (angle) F , F
y xM
zM
zF
xF
yFigure 2.5: Steady-state tyre characteristics [10]
2.2.1.4 The Brush Model
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
thesis, the derivation of expressions of two forces and aligning torque will not be explained here. Please refer to [7] and [11].
Fx
Fz
vx
ω
x
z
Figure 2.6: Bristle deformation during accelerating
2.2.2 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 parametriza- tion. 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 trans- fer are negligible too,
• no aerodynamic effect,
• tyres remain in the linear region (ay 6 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 bi- cycle 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.
Yu
Xu Fy 1 2
Fy 3 4 vx,ax
vy,ay
ψ δ1 2
δ3 4
Xb
Yb
f
b l
ωz
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 ro- tational 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:
Fx = max = m( ˙vx− ωzvy) (2.21) Fy = may = m( ˙vy+ ωzvx) (2.22) Mz = Jzω˙z = Fyff − Fyrb (2.23) where Fx is the projection of Fyi (i = 12, 34) onto the x-axis (i.e. Xb) in body-fixed coordinate frame. Fy is the projection of Fyi (i = 12, 34) onto the y-axis (i.e. Yb) in body-fixed coordinate frame. Fyf and Fyr are components of Fy at the front and rear tyre respectively. The inertial acceleration ax/y consists of two terms: the ac- celeration ˙vx/y which is caused by the motion along the x/y-axis and the centripetal acceleration ωzvy/x.
In the simple bicycle model the longitudinal velocity is assumed constant, so Equa- tion 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 2.18. Combining with small angle assumptions, the matrix expression of the simple bicycle model is
˙vy ψ¨
= −C12mv+C34
x −vx− C12f −Cmv 34b
x
−C12Jf −C34b
zvx −C12fJ2+C34b2
zvx
! vy ψ˙
+
C12
m
C34
C12f m
Jz −CJ34b
z
δ12 δ34
(2.24)
2.2.3 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 Xb direction. The representations of the dynamics are shown as
Fy = may = m( ˙vy+ ωzvx) = Fy1cos δ1+ Fy2cos δ2+ Fy3cos δ3+ Fy4cos δ4 (2.25)
Mz = Jzω˙z = Fy1cos δ1f + Fy1sin δ1wf
2 + Fy2cos δ2f
− Fy2sin δ2wf
2 + Fy3sin δ3wr
2 − Fy3cos δ3b − Fy4cos δ4b − Fy4sin δ4wr
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.
Yu
Xu Fy 1
Fy 4 vx,ax
vy,ay
ψ δ1
δ3
Xb
Yb
f
b l
ωz
Fy 2
Fy 3
δ2
δ4
wf
wr
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.
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.
2.3.1 The Kalman Filter
A linear system will be presented by its state space model having state vector X ∈ Rn and observation vector Y ∈ Rm in Equation 2.27.
Xk= Ak−1Xk−1+ Bk−1Uk−1+ qk−1
Yk = HkXk+ rk (2.27)
• Xkis the state of the system at the time step k.
• Yk is the measurement at the time step k.
• qk−1∼ N (1, Qk−1) is the process noise at the time step k − 1.
• rk∼ N (0, Rk) is the measurement noise at the time step k.
• Ak−1 is the transition matrix of the dynamic model.
• Bk−1 is the input matrix.
• Uk−1 is the control input vector to the dynamic model.
• Hk is the observation model matrix.
Two elements that propagate through the iteration of the Kalman filter are the pos- teriori state estimate Xk|k and the posteriori estimate covariance Pk|k. The whole algorithm is initialized with X0|0 and P0|0 which can be guessed if the initial values are unknown and in most cases the results will converge. But sometimes a good
estimation of initial values can influence the behavior of the Kalman filter, for ex- ample the convergence time. Since the Kalman filter follows a recursive way, the posteriori Xk−1|k−1, Pk−1|k−1 from last time step will be used to calculate the priori Xk|k−1, Pk|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) Yk at the current time step resulting in the posteriori Xk|k, Pk|k. The whole process is shown below [15].
Algorithm 1 The Kalman Filter Predict
Xˆk|k−1= Ak−1Xˆk−1|k−1+ Bk−1Uk−1 Pk|k−1= Ak−1Pk−1|k−1ATk−1+ Qk−1 Update
Vk= Yk− HkXˆk|k−1 Innovation
Sk = HkPk|k−1HkT + Rk Measurement Prediction Covariance Kk = Pk|k−1HkTSk−1 Kalman Gain
Xˆk|k = ˆXk|k−1+ KkVk Pˆk|k = Pk|k−1− KkHkPk|k−1
2.3.2 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
xk = f (xk−1, uk−1) + qk−1 yk= h(xk) + rk
(2.28) where x ∈ Rn is the state vector and y ∈ Rm is the measurement vector, qk−1 ∼ N (1, Qk−1) is the process noise, rk ∼ N (0, Rk) is the measurement noise, f is the dynamic model function and h is the measurement model function. Before imple- menting 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 Px, there will be 2n + 1 sigma points χi and their corresponding weights Wi:
χ0 = ¯x χi = ¯x + (p
(n + λ)Px)i i = 1, ..., n χi = ¯x − (p
(n + λ)Px)i−n i = n + 1, ..., 2n
(2.29)
W0m = λ/(n + λ)
W0c= λ/(n + λ) + (1 − α2+ β) Wim = Wic = 1/2(n + λ)
(2.30)
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
ˆ
x0 = E[x0]
P0 = E[(x0− ˆx0)(x0− ˆx0)T] Predict
χk−1|k−1= [ˆxk−1|k−1 ... xˆk−1|k−1] + [0 p(n + λ)Pk−1|k−1 −p(n + λ)Pk−1|k−1] ˆ
χk|k−1= f (χk−1|k−1, uk−1) ˆ
xk|k−1=
2n
X
i=0
Wimχˆi,k|k−1
Pk|k−1=
2n
X
i=0
Wic[ ˆχi,k|k−1− ˆxk|k−1][ ˆχi,k|k−1 − ˆxk|k−1]T + Qk−1
χk|k−1= [ˆxk|k−1 ... xˆk|k−1] + [0 p(n + λ)Pk|k−1 −p(n + λ)Pk|k−1] Yˆk|k−1= h(χk|k−1)
ˆ
yk|k−1 =
2n
X
i=0
WimYˆi,k|k−1 Update
Sk=
2n
X
i=0
Wic[ ˆYi,k|k−1− ˆyk|k−1][ ˆYi,k|k−1− ˆyk|k−1]T + Rk
Ck =
2n
X
i=0
Wic[χi,k|k−1− ˆxk|k−1][ ˆYi,k|k−1− ˆyk|k−1]T Kk= CkSk−1
ˆ
xk|k = ˆxk|k−1+ Kk[yk− ˆyk|k−1] Pk|k = Pk|k−1− KkSkKkT
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.
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.
2.4.1 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 (rr) since the velocity of the wheel is vwheel = ωrr. 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 (reff) which is different from static radius when the vehicle stands still. Then the wheel velocity can be modeled as
Vˆwheel = ωreff + υv (2.31)
where ˆVwheel is the estimated real velocity, ω represents measurements from the speed sensor and υv is the measurement noise of the sensor.
2.4.2 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
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
am = a + ba+ υ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, ba 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)
Figure 2.9: Xsens MTi-G-700 motion tracker [17]
2.4.3 GPS Receiver
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
Ym = Y + bY + υY (2.35)
where Ym is the measurement and Y is the actual value. bY is the bias but not always existing, which depends on the quality of output signal from the GPS receiver. υY
is the measurement noise.
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.
σ = v u u t
1 N
N
X
i=1
(xi− µ)2 (2.36)
where N is the number of all points, µ is the mean of xi and in our case it represents the ground truth.
Finally the CEP can be calculated using the standard deviation.
CEP = 0.62σy + 0.56σx (2.37)
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 indi- vidual 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
Table 3.1: The RCV Specifications
Name Value Unit
m Vehicle total mass with two persons 608 kg mj 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
rr Tyre radius 0.31 m
Jz Inertial around z-axis 1500 kgm2
Cf/Cr 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] Figure 3.3: Trimble GPS receiver[21]
3.2 Test Setup
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.
• 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 l1 and l2. The road profile is measured by Mohammad Mehdi Davari using a Trimble S5 and the information is given in Table 3.2.
A B C
l
1l
2l
1
l
2bank slope
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
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 3.3.1 describes the processing of raw signals before they can be fed to the main algorithm. Section 3.3.2 involves details of the estimation algorithm and logic for dynamical states. Section 3.3.3 comprises state estimation of positions.
3.3.1 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 cor- rection 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 cor- rupted by the bank angle of the test track. Based on Equation 2.32, the corrected lateral acceralation acory is
acory = amy − bay = amy − gsin(φbank) (3.1) where amy is the measurement from IMU, φbank is the bank angle of the track, g is the gravitational acceleration and equals to 9.81m/s2 (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 Qk 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 2.1.6.1.
Then this local ENU frame rotates around Zu 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 2.1.6.2, 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
xu = (λ − λ0) · 56448.05
yu = (ϕ − ϕ0) · 111405.66 (3.2)
xlocal = xu· cos ν0+ yu· sin ν0 ylocal = yu· cos ν0− xu· sin ν0
ψm = ν − ν0
(3.3)
where the variables with subscript 0 belong to the initial point, xlocal and ylocal 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
acory corrected lateral acceleration m/s2 ψ˙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 xlocal displacement in x direction of Local frame m ylocal 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
3.3.2 Dynamics Block
In this block four states will be estimated i.e. longitudinal velocity vx, lateral ve- locity vy, 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 er- rors. 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 model- based estimator is chosen here. As described in Section 2.2.3 the vehicle model can
be simplified to a 2-DoF model. So the longitudinal velocity needs to be estimated before implementing the double-track model.
3.3.2.1 Longitudinal Velocity Estimation
The longitudinal velocity vx is the velocity component in x-direction of a body-fixed coordinate system. A general way to estimate the longitudinal velocity is to build a dynamic model in x-direction, which requires propulsion force, rolling resistance, aerodynamic force and a tyre model and information in y-direction if necessary. And in order to calculate these forces accurately more information like friction coefficient and load transfer need to be estimated. In addition, the online tuning of a tyre model like the Magic Formula are computationally heavy for the algorithm. All above discussed make the estimation complicated and computationally inefficient and will not be adopted.
A simple method is created for this specific problem. There are two reasons to do this, one is that accurate wheel angular velocity can be measured by speed sensors on each wheel and the other is that the implementation of this algorithm is mostly for not severe driving conditions which minimize the uncertainties. This method is inspired by the development of empirical tyre models. The very basic empirical tyre model is just a regression model which fits the measurement data best. And semi-empirical models like the Magic Formula not only are able to perfectly fit the curves but also features coefficients which have clear relationships with typical shape and magnitude factors of the curves to be fitted [7]. For our specific use, the simple regression model of the longitudinal velocity is implemented. Similar to Equation 2.31, this regression model has a coefficient η representing both the dynamic rolling radius reff and some uncertainties in high dynamic cases. The mathematical form of this regression model is
vm = η · vx
η = 1
8(eψ˙− 1) + 0.995 (3.4)
where vm is calculated by
vm = (ω1+ ω2+ ω3+ ω4)rr
4 (3.5)
Finding the regression relation is straight-forward, as η should include high dynamic uncertainties, the high dynamic behavior is distinct when the car is turning. And yawrate ˙ψ is the right parameter that correlates with η and the actual longitudinal velocity. Figure 3.5 shows the fitted exponential regression curve. And the coef- ficients of the exponential function are derived by averaging results from ten tests including low and high dynamics. The whole process was finished offline with logged data.
