GROUND VEHICLE ATTITUDE ESTIMATION THROUGH MAGNETIC-INERTIAL SENSOR FUSION

Size: px

Start display at page:

Download "GROUND VEHICLE ATTITUDE ESTIMATION THROUGH MAGNETIC-INERTIAL SENSOR FUSION"

Sabina McLaughlin
6 years ago
Views:

1 F16-VDCF-4 GROUND VEHICLE ATTITUDE ESTIMATION THROUGH MAGNETIC-INERTIAL SENSOR FUSION 1 Hwang, Yoonjin * ; 1 Seibu, Choi 1 Korea Advanced Institute of Science and Technology, S. Korea KEYWORDS Vehicle Dynaics, Attitude Estiation, Sideslip, Euler Angle Estiation, Sensor Fusion ABSTRACT The ain goal of this paper is to design accurate and odel-independent vehicle attitudes estiator. The proposed estiator uses low cost 3 axis agnetoeter and vehicle yaw rate and lateral acceleration sensors. The estiation ski can be divided into parts: Euler angle estiation and sideslip estiation. An extended Kalan filter cobined with a deterinistic Euler angle estiator is utilized for robust and fast converging Euler angle estiation. Using the result of Euler angle estiator, a sideslip observer based on copensated planar kineatic odel is proposed. The feasibility of proposed algorith is verified with the coercial siulation tool, Carsi. Siulation includes possible noises of sensor based on anufacturer datasheets. TECHNICAL PAPER 1. INTRODUCTION An Electronic Stability Syste (ESC) is one of the successful control strategy to prevent unintended otion of the vehicle. According to the recent research[1], the ESC can reduce single-vehicle crashes up to 36% and rollover crashes up to 7%. The ESC syste requires accurate inforation about the vehicle otion, which contains steering wheel angle, yaw rate, lateral acceleration and sideslip angle. Especially, the sideslip angle is an iportant factor to controllability. Tyre grip varies with the sideslip angle. The cornering stiffness increases along the sideslip, but once it hits its peak value, the tyre cannot ake ore force. That eans when the sideslip exceed certain level, the driver cannot control it with steering. However, the sideslip angle cannot be easured directly while other paraeters are given by sensors. Since the sideslip angle plays a key role to judge how uch vehicle can react on driver s control, various estiation skis have been introduced for decades: a) estiation based on a dynaic odel, which is called bicycle odel, b) estiation by integrating a kineatic odel for the plane otion, c) alternative sensors to easure the sideslip directly, and d) experient-based ethods. First two odel based approaches are ost coon ways to deterine how uch the vehicle slips. The estiation based on the bicycle odel uses a reduced order odel of a vehicle. It uses nuber of vehicle paraeters, and only requires yaw rate and acceleration data. The accuracy of it is quite precise if the vehicle paraeters are correct. However, the paraeters are not easily deterined and can be changed fro their original value due to various reasons. To handle this probles, adaptive estiation ethods have been introduced[], but it still requires precise paraeter adjustent and high coputing power. The kineatic odel observer doesn t need the accurate odel paraeters, but the Euler angle deterination and the drifting issue are the ain obstacles to design it[3]. Methods using GPS receivers to get the sideslip are introduced as an alternative[4; 5]. Multiple GPS receivers can provide the sideslip, but ost of production vehicles are not

2 equipped with ultiple GPS receivers. Moreover, the sapling rate of low cost GPS receivers is 1Hz to 1Hz, which is too low to use as a reference. Yoon proposed a ethod with a agnetoeter, IMU, and GPS for robust sideslip estiation. This ethod can stand on a wide range of road condition such as frictions, bank angle or inclination. The Kalan filter integrates Euler angles with IMU data, and checks the angles by coparing the rotation of the geoagnetic vector. This effectively eliinate a drift issue when using an IMU integration. With those three Euler angles and GPS velocity, the copensated kineatic odel is used to get the sideslip angle of the vehicle. However, in this research, 6-DOF IMU and very high perforance GPS receiver are used to estiate the vehicle state. Both those type of high perforance sensors are not coon equipent in production vehicles due to the cost of the. This paper proposes a new ethod to obtain the sideslip angle with factory-equipped and low cost sensors. The proposed ethod only requires yaw rate and lateral acceleration, standard equipent of ESC systes, and three-axis agnetoeter (TAM), used for E- copass. The estiation consists of steps: Euler angle estiator and sideslip angle estiator. The Euler angle estiator is based on a sensor fusion through Kalan filtering of agnetic sensor and yaw rate sensor. Then, using the estiated Euler angle, kineatic odel based sideslip estiation is perfored. This proposed estiator doesn t need any vehicle odel paraeter, so it can be adopted various vehicle without individualization. Since this ethod also gives the data of roll, pitch, and yaw angle, it can be used for the roll over protection, suspension control, or even navigation systes. This paper is organized as follows. Section describes how easureent vectors react with arbitrary rotation and a ethodology to find corresponding rotational atrix with vector easureents. Section 3 deals with the design of the Euler angle estiator by using a Kalan filter through a agnetic-inertial sensor fusion and the sideslip estiator with a copensated planar kineatic odel. Section 4 verify the perforance of proposed estiators on siulations and experients.. THEORETICAL BASIS.1 3-Axis Magnetoeter A agnetoeter easures the geoagnetic field which can be assued as a constant vector in local area. The geoagnetic field has its direction to the agnetic north pole and its agnitude about 5G. Since the geoagnetic field is fixed in Earth-fixed reference frae (R-frae), the easureent in body-fixed frae (-frae) is varying with the rotation of the vehicle. There are several ways to represent a body rotation with Euler angles; this paper use ZYX successive rotation with yaw angle ψ, pitch angle θ, and roll angle φ. Figure 1: Successive ZYX rotation

3 The easured geoagnetic vector in -frae can be expressed as (.1). B cosθcosψ cosθsinψ sinθ cosφsinψ sinφsinθcosψ cosφcosψ sinφsinθsinψ sinφcosθ = + + H sinφsinψ + cosφsinθcosψ sinφcosψ + cosφsinθsinψ cosφcosθ H is the geoagnetic field easured in the R-frae, and B is the geoagnetic field easured in -frae. Since the rotation odel has coponents with Euler angles, the differentiation of (.1) with can provide an equation with angular rates. (.1) r q Bx B= ω B= r p B y q p B z (.) However, the unique solution of (.) cannot be obtained by itself. There are 3 equations and 3 unknown variables, p, q and r, but considering the constraint of the agnitude of B, one of the equations can be represented by others. Therefore, to obtain the unique solution, there should be at least one ore condition. Then, yaw rate r can plays a role. By easuring r fro a sensor, (.) becoes solvable, and the result is as follows. p = ( B + rb )/ B y x z q = ( B rb )/ B x y z (.3). Tri-axial Attitude Deterination Tri-axial attitude deterination (TRIAD)[6-8] is a ethod to deterine the attitude of body with ultiple vector easureents. As entioned in previous section, the attitude cannot be obtained as unique solution with one vector easureent. Figure : (a) Any vector on shaded plane can be the rotation axis fro V to V (b) The rotation axis can be uniquely defined with two vector set (V, W) However, with ultiple easureent, so called Wahba s proble[9], there are ore constraints than required, so the syste is overdeterined. TRIAD algorith provides the

4 optial solution that iniize certain weight for each vector. First, the triad vectors fro easureent and reference vectors V and W are set as follows. R V = RV R R W = RW R v1 = V, v = V W, v3 = v1 v w = V, w = V W, w = w w 1 R R R 3 1 Fro that orthogonal triad basis, the rotational relationship fro R-frae to -frae can be obtained. R, Triad (.4) [ ˆ ˆ ˆ ][ ˆ ˆ ˆ T R = v v v w w w ] (.5) R is the rotation atrix calculated fro R-frae to -frae with sae weight for each R, Triad easureent, where v ˆi and w ˆ i are the noralized vectors of i 3. GROUND VEHICLE ATTITUDE ESTIMATON 3.1 EKF with Pseudo Angular Velocity v and To cobining the easureent odel given in (.1) and the transient odel with the angular velocity obtained by (.3), the extended Kalan filter[1] is chosen to solve severe nonlinearity on easureent odel. The transient odel is[11] w i. φ = p+ ( qsinφ+ rcos φ) tanθ θ = qcosφ rsinφ ψ = pcosφ+ qsinφsinθ + rcosφsinθ (3.1) and the easureent odel is exactly sae as (.1). With the transient odel and easureent odel, the Kalan filter is applied to estiate the orientation of the vehicle. xˆ = F xˆ + k k 1 k 1 P = F P F + Q + T k k 1 k 1 k 1 k 1 K = PH[ HPH + R] T T 1 k k k k k k xˆ = xˆ + K ( z H xˆ ) k k k k k k P = ( I KH) P + k k k k (3.) x ˆ z where, F =, H =, x = xˆ xˆ [ φ θ ψ ] T The covariance Q and R are assued that they only have diagonal coponents, and treated

5 as tuning paraeters since their distribution is unknown. 3. TRIAD with Pseudo Gravity Vector EKF introduced in previous section is quite accurate without the easureent noise or isalignent. However, in the real world, the easureent noise is not avoidable, and the sall error in easureent can be stacked and ake the syste diverge, called drifting. To prevent the drifting issue, a deterinistic estiation can be an effective alternative. While TRIAD ethod requires least vector easureents, the supposed syste easures only 1 vector, which is the geoagnetic field. To deal with this proble, a virtual vector easureent can be introduced. g = [ ay 1 ay ] (3.3) g The vector is a pseudo gravity vector, assuing the vehicle otion doesn t ake inertial force and the road doesn t have any inclination angle. To restrict the effect of TRIAD estiation when (.8) is not valid, a varying gain, TF (transient factor) is introduced. TF has its value fro to 1. A low TF eans the vehicle otion is quiet steady longitudinal otion, whereas a high TF indicates the dynaic otion of vehicle. TF is defined as EKF and TRIAD is cobined through TF as follows. where xˆ [ φ θ ψ] T = and L is a tunable gain. 3.3 Sideslip Estiation xˆ = xˆ + (1 TF) L( xˆ xˆ ) (3.4) EKF EKF TRIAD The velocity kineatics[1] of the vehicle on the plane is used widely to estiate the longitudinal and lateral velocity of vehicle. v sin ˆ x r vx axw θ g v = y r v + + y a y sin ˆ φcos ˆ θ (3.5) The corresponding Luenberger for observer is as follows. vˆ ( ) ˆ sin ˆ x rt vx a xw θ vˆ = = g Lt () y vˆ rt () vˆ + y a + + y sin ˆcos ˆ y φ θ vˆ x yˆ = vˆ y (3.6) Since the transient atrix varies with, observer gain L(t) is selected by using frozen pole-placeent to stand for the varying syste.[13]

6 x L r x I A LC = λ + L r = + L + r r + L 1 = L r 1 det( λ ( )) det λ 1λ ( ) L + r λ (3.7) Then, choose L 1 and L as L = κ r L 1 = κ ( 1) r (3.8) The syste eets the stability condition by LaSalle s theore, with the Lyapunov function candidate as follows. 1 ( κ x y ) V = v + v V = 3 κ rvx (3.9) 4. SIMULATION RESULTS 4.1 Siulation Results The verification of proposed Euler angle estiator is perfored using a coercial vehicle dynaics siulation tool, Carsi. The ain focus of the test is to verify the robustness of the estiator with angled road which is hard to deal with the conventional ethods, and with various states consist of quasi-static, steady and dynaic condition. BCase 1BDriver control BBank 3BIncline 4BCase I 5BDouble Lane Change 6BFlat 7BFlat 8BCase II 9BStraight 1B 11BFlat 1BCase III 13BStraight 14BFlat 15BHill 16BCase IV 17BDouble Lane Change 18B 1 19BHill Table 1: Test scenarios Case I is a standard double lane change test. As shown is figure 3 the single TRIAD ethods cannot follow the angles during the turn, whereas EKF effectively estiates the Euler angles including the yaw angle during the severe turning anoeuver. The cobined estiator shows no difference with standard EKF, since right after the turn ended TRIAD is stabilized rapidly and it has no effect at dynaic region due to the conditional gain SF. Case II is about constant bank angle. There is soe peaks at the very first of anoeuver due to the liitation of road creating on siulation tool. For roll and pitch angle 3 of the all show quiet good estiation perforance, but in the case of yaw, the cobined estiator can hold the steady state error uch less than the standard EKF. Case III, the vehicle goes through the hill. Since the inclination is not stiff, TRIAD also has

7 low disturbance. The cobined estiator and the standard EKF shows siilar levels of error. Case IV is the coplicate anoeuver, which covers all previous cases at once. As shown in figure 6, each estiators has predictable perforance until 7 sec. However, after 7 second, the drift issue occurs with the standard EKF and the cobined estiator both. The error of EKF is getting larger slowly with, but the cobined estiator effectively restrict the divergence of error with TRIAD ethod. 1 (a) Estiated roll angle Ref. TRIAD EKF EKF + TRIAD (b) Estiated pitch angle (a) Estiated roll angle (b) Estiated pitch angle (c) Estiated yaw angle (c) Estiated yaw angle Figure 3: Case I, Double Lane Change, Flat Figure 4: Case II, Straight, Bank 5 (a) Estiated roll angle 1 (a) Estiated roll angle (b) Estiated pitch angle (b) Estiated pitch angle (c) Estiated yaw angle (c) Estiated yaw angle Figure 5: Case III, Straight, Hill Figure 6: Case IV, Double Lane Change, Cobined The results of sideslip estiation is copared with siple integration of uncopensated planar kineatic odel. There is unknown bias in the yaw rate and acceleration easureent, those leads the integration to diverge. The proposed sideslip observer with the odel copensated with the effect of gravity and varying gain eliinate the divergence issue fro the integration, and at the sae it keeps the estiation error under

8 acceptable level (a) Estiated sideslip Ref. Int. Cop (a) Estiated sideslip Ref. Int. Cop (b) Estiation error of sideslip 1.5 Int. Cop (a) Estiation error of sideslip 8 Int. 6 Cop Figure 7 Straight, Flat Figure 8 DLC, Cobined 4. CONCLUSION Without using the vehicle odel paraeter, such as oent of inertia, ass or cornering stiffness, it has been proven the ideal perforance and feasibility of the low cost vehicle attitude estiator. The proposed estiator follows the reference values whether the vehicle is under dynaic condition or not. Since it does not contain any vehicle odel paraeters, the proposed estiator can be applied to any vehicle directly only with the initial calibration process. The originalities distinguished fro the previous researches are the following: iproveent of the converging and reducing the steady state error with a cobination of the deterinistic estiation(triad) and the integrational estiation(ekf), and robustness of the vehicle attitude estiation against to the road angles. However, according to the previously reported papers, since the geoagnetic field is very weak copared with the agnetic field generated by electric devices, using the agnetoeter on the ground can be affected by the environent nearby. The hard and soft iron effect also could be a proble. Therefore, handling the agnetic noise and distinguish it fro the geoagnetic field, and on-board calibration ethod is required. A real-car-based experient is on preparation.

9 REFERENCES [1] NHTSA. "Statistical Analysis of the Effectiveness of Electronic Stability Control (ESC) Systes - Final Report" [J]. 7, [] Oh, J. J., & Choi, S. B. "Vehicle velocity observer design using 6-d iu and ultipleobserver approach" [J]. 1, 13(4): [3] Ki, H., & Ryu, J. "Sideslip angle estiation considering short-duration longitudinal velocity variation" [J]. 11, 1(4): [4] Bevly, D. M., Ryu, J., & Gerde, J. C. "Integrating INS sensors with GPS easureents for continuous estiation of vehicle sideslip, roll, and tire cornering stiffness" [J]. 6, 7(4): [5] Yoon, J.-H., & Peng, H. "Vehicle sideslip angle estiation using two single-antenna GPS receivers" [J]. Aerican Society of Mechanical Engineers, 1, [6] Bar-Itzhack, I. Y., & Haran, R. R. "Optiized TRIAD algorith for attitude deterination" [J]. 1997, (1): [7] Markley, F. L. "Attitude deterination using vector observations: a fast optial atrix algorith" [J]. 1993, [8] Shuster, M. D. "The TRIAD algorith as axiu likelihood estiation" [J]. 6, 54(1): [9] Wahba, G. "A least squares estiate of satellite attitude" [J]. 1965, 7(3): [1] Haykin, S. S., Haykin, S. S., & Haykin, S. S. 1. Kalan filtering and neural networks: Wiley Online Library. [11] Oh, J., & Choi, S. B. "Vehicle roll and pitch angle estiation using a cost-effective six-diensional inertial easureent unit" [J]. 13, 7(4): [1] Farrelly, J., & Wellstead, P Estiation of vehicle lateral velocity. Paper presented at the Control Applications, 1996., Proceedings of the 1996 IEEE International Conference on. [13] Son, H., & Choi, S. 8. A study on design of a robust vehicle side-slip angle observer using an integration of kineatic and bicycle odel. Paper presented at the KSAE 8 Annual Conf., Daejeon, Korea.

The optimization design of microphone array layout for wideband noise sources

The optimization design of microphone array layout for wideband noise sources PROCEEDINGS of the 22 nd International Congress on Acoustics Acoustic Array Systes: Paper ICA2016-903 The optiization design of icrophone array layout for wideband noise sources Pengxiao Teng (a), Jun