# Vibration Information Acquisition and Representation Control Strategy for Construction Vehicle Driver Seat

Authors: Mingde Gong; Baoqiang Zhao; Xin Wang
Journal: Shock and Vibration
 (2019)

---

## Abstract

Vibration information acquisition and representation control strategy in the experiment of construction of vehicle driver seat are investigated in this study. An improved adaptive algorithm of multisensor attitude data fusion is proposed. The nonlinear equations of vehicle attitude quaternions are constructed by using the test information of accelerometers and magnetometers. Then, a complementary filter is used to fuse the obtained attitude with gyroscope information to obtain attitude information during vehicle travel. Weight matrix and filter coefficients in the algorithm are adjusted with vehicle motions to ensure accuracy of attitude measurement under static, low-, and high-acceleration conditions. The vehicle vibration displacement information is reconstructed by the low-frequency attenuation integration method on the basis of attitude measurement to inhibit integral trend items and reduce vibration displacement reconstruction errors. The three-state controller is designed to control a six-degree-of-freedom hydraulic simulated vibration test bed, and the obtained vibration signal is used as an excitation signal to perform a vibration representation control test. Finally, results verify that the proposed method demonstrates favourable data fusion and representation accuracy.


---

## Body

## 1. Introduction

When construction vehicles drive on complex roads, various parts of a driver seat are constantly affected by vibration and impact of uneven road, which may be a huge risk for vehicle safety and cause damage in various degrees [1–3]. Accelerating the development of vehicle seat products and ensuring the reliability of the product are effective approaches to avoid these damages by using a real vehicle to collect vehicle vibration information of different road conditions and then adopt a vibration test system to simulate a complex vibration environment [4–7]. At present, a single degree-of-freedom (DOF) seat vibration fatigue test bed has a single-vibration form, making it difficult to simulate complex vibration environments [8, 9]. Although the multi-DOF vibration test bed can simulate a complex vibration environment, researches on the multiaxis vibration test bed mostly focus on the vibration control algorithm [10–17]. Considerable researches on road excitation information acquisition have been conducted. However, most of these studies use road roughness information generated by a power spectral density function of a standard road level [18, 19] or a rigid road [20, 21], and studies on obtaining actual driving vibration information are limited.The basis of vibration environment simulation is to acquire vibration information during vehicle driving. Vibration information includes vehicle attitude and vibration displacement [22–24]. The calculating algorithm for an attitude angle is the core to obtain vehicle attitude, an important factor that affects the record accuracy of vibration information. The design and use of a reasonable attitude-calculating algorithm are the most important issues in an attitude measurement system [25].The vehicle attitude measurement system in this study is composed of gyroscope, accelerometer, and magnetometer. These components are suitable for various complex environments [26]. In accordance with the respective characteristics of sensors, the collected data are fused to enable a complementary sensor performance and obtain high-precision attitude measurement data. Researchers propose two kinds of filtering algorithms for the multisensor information fusion problem. One is Kalman filtering and various algorithms, such as Kalman filter (KF), extended KF, unscented KF, and federated KF, are derived on the basis of Kalman filtering [27–29]. The other is complementary filtering (CF), and various improved filtering algorithms, such as classical CF, linear CF, and CF based on the gradient descent method, are derived on the basis of CF [30, 31].The vehicle vibration displacement information is difficult to be detected directly, and an indirect measurement method is used to obtain the actual test. At present, the indirect measurement method of the accelerometer is extensively used [32, 33]. The signals of velocity and displacement are obtained by single and double integrals of acceleration signals, respectively. The use of the acceleration signal to reconstruct velocity and displacement information is an effective method for obtaining a displacement signal. Three main methods are previously mentioned. The first method considers the integral of the acceleration signal in a time domain. This method is simple and has a minimal amount of calculation. It has been applied to the field of aircraft line planning and navigation [34]. However, this method has limited processing capability for trend terms, and the obtained displacement signal has serious trend-term errors occasionally. The second method is decomposing a signal in accordance with its characteristics. The empirical mode decomposition method is used to correct the displacement resulted from an integral of the acceleration signal [35]. The third method is using Fourier transform to convert acceleration, velocity, and displacement [36, 37]. This method is effective in handling trend items; however, it has numerous calculations and is unsuitable for real-time data processing.To improve the control accuracy of the multisensor data fusion, the adaptive control algorithms are developed to guarantee the boundedness of adaptive estimation [38, 39] and have integrated adaptive control to achieve the online identification of component parameters of the PV system. Moreover, fuzzy control is used to approximate the unmodeled part of system. In [40], the fault is accommodated by reconfiguring the formation structure through the novel notion of the weighted absolute measurement formation digraph and then a robust controller for the partially low-level recovered vehicle is activated.This study aims to increase accuracy of vehicle attitude measurement at the static, low-, and high-acceleration states and propose an improved adaptive algorithm of multisensor attitude fusion. Vehicle attitude quaternion nonlinear equations by using the test information of the accelerometer and magnetometer are developed in this paper. Then, the complementary filter is used to fuse the attitude and gyroscope information to obtain the attitude information during vehicle driving. Moreover, according to the weight matrix and filtering coefficient of the vehicle motion state adjustment algorithm, the vehicle attitude measurement under the static, low-, and high-acceleration states achieves good results. Finally, a six-degree-of-freedom hydraulic simulation vibration test bed is designed, and the collected vibration signals are used as the excitation signals to carry out the vibration reproduction control experiment.


## 2. Compositions and Principle of the System

### 2.1. Vibration Representation Test Bed

The vibration test bed is based on the Stewart structure and consists of an upper platform, a lower platform, and six branched chains. Each chain is composed of a hydraulic cylinder, an upper base, a lower base, and a universal joint. The top of the upper base is fixed to the lower surface of the upper platform. The lower end is fixed to the upper end of the universal joint, and the lower end of the lower base is fixed to the upper surface of the lower platform. The driver seat of a vehicle is fixed to the upper end of the upper platform. The upper platform has six DOFs, and its structure is illustrated in Figure1. Table 1 displays the technical index of the vibration test bed.Figure 1
Structure of the vibration representation platform.Table 1
Technical index of the vibration representation platform.
Bearing capacity
Range of motion
Working frequency band
α
β
γ
a
b
c
1t
±15°
±15°
±15°
±150 mm
±150 mm
±130 mm
0.1–20 HzIn Table1, α denotes the roll angle around the x-axis, β denotes the pitch angle around the y-axis, γ denotes the yaw angle around the z-axis, a denotes a displacement range along the x-direction, b denotes a displacement range along the y-direction, and c denotes a displacement range along the z-direction.


### 2.2. Electrohydraulic Servo Control System

A control system of a hydraulic cylinder comprises a computer processing unit, a controller, an A/D converter, a D/A converter, an electrohydraulic servo valve amplifier, and a displacement sensor.The collected acceleration signal is transmitted to the computer processing unit after being processed using a filter. A digital signal that controls the expected displacement of the hydraulic cylinder is compared with the actual displacement signal of the hydraulic cylinder fed back by a vibration representation device. Subsequently, the difference is transmitted to the central processing unit to be calculated by the control algorithm and then outputs a control result. The current signal in the electromagnetic coil of the electrohydraulic servo valve is converted by using a D/A converter to control the displacement of the electrohydraulic servo valve spool and the extension of the hydraulic cylinder and drives the motion representation of the 6-DOF test bed. The displacement sensor in the extended end of the hydraulic cylinder detects the displacement information and transmits this information to the A/D converter to convert it into a digital signal. The displacement signal is compared with the displacement signal calculated by using the obtained data to determine the difference which forms a closed-loop control of the hydraulic cylinder displacement.Figure2 depicts the structure of the hydraulic servo control system. In Figure 2, ui (i = 1–6) is the control quantity which is the output of the D/A converter to the servo amplifier and yi (i = 1–6) is the piston displacement of the hydraulic cylinder. Moreover, x is the displacement of the servo spool; pb and qb are pressure and flow of the rod cavity of the hydraulic cylinder, respectively; pa and qa correspond to pressure and flow of a rodless cavity of the hydraulic cylinder; and ps is the working pressure of the system.Figure 2
Principle of a closed-loop control of the hydraulic servo control system.


## 2.1. Vibration Representation Test Bed

The vibration test bed is based on the Stewart structure and consists of an upper platform, a lower platform, and six branched chains. Each chain is composed of a hydraulic cylinder, an upper base, a lower base, and a universal joint. The top of the upper base is fixed to the lower surface of the upper platform. The lower end is fixed to the upper end of the universal joint, and the lower end of the lower base is fixed to the upper surface of the lower platform. The driver seat of a vehicle is fixed to the upper end of the upper platform. The upper platform has six DOFs, and its structure is illustrated in Figure1. Table 1 displays the technical index of the vibration test bed.Figure 1
Structure of the vibration representation platform.Table 1
Technical index of the vibration representation platform.
Bearing capacity
Range of motion
Working frequency band
α
β
γ
a
b
c
1t
±15°
±15°
±15°
±150 mm
±150 mm
±130 mm
0.1–20 HzIn Table1, α denotes the roll angle around the x-axis, β denotes the pitch angle around the y-axis, γ denotes the yaw angle around the z-axis, a denotes a displacement range along the x-direction, b denotes a displacement range along the y-direction, and c denotes a displacement range along the z-direction.


## 2.2. Electrohydraulic Servo Control System

A control system of a hydraulic cylinder comprises a computer processing unit, a controller, an A/D converter, a D/A converter, an electrohydraulic servo valve amplifier, and a displacement sensor.The collected acceleration signal is transmitted to the computer processing unit after being processed using a filter. A digital signal that controls the expected displacement of the hydraulic cylinder is compared with the actual displacement signal of the hydraulic cylinder fed back by a vibration representation device. Subsequently, the difference is transmitted to the central processing unit to be calculated by the control algorithm and then outputs a control result. The current signal in the electromagnetic coil of the electrohydraulic servo valve is converted by using a D/A converter to control the displacement of the electrohydraulic servo valve spool and the extension of the hydraulic cylinder and drives the motion representation of the 6-DOF test bed. The displacement sensor in the extended end of the hydraulic cylinder detects the displacement information and transmits this information to the A/D converter to convert it into a digital signal. The displacement signal is compared with the displacement signal calculated by using the obtained data to determine the difference which forms a closed-loop control of the hydraulic cylinder displacement.Figure2 depicts the structure of the hydraulic servo control system. In Figure 2, ui (i = 1–6) is the control quantity which is the output of the D/A converter to the servo amplifier and yi (i = 1–6) is the piston displacement of the hydraulic cylinder. Moreover, x is the displacement of the servo spool; pb and qb are pressure and flow of the rod cavity of the hydraulic cylinder, respectively; pa and qa correspond to pressure and flow of a rodless cavity of the hydraulic cylinder; and ps is the working pressure of the system.Figure 2
Principle of a closed-loop control of the hydraulic servo control system.


## 3. Driving Vibration Information

### 3.1. Attitude Calculation

#### 3.1.1. Attitude Measurement Algorithm

A gyroscope has a remarkable dynamic performance, but an accumulative error during the calculating process results in a time lapse. Accelerometer and magnetometer use gravity and geomagnetic fields for attitude measurement, respectively. No accumulative error has been observed. However, the dynamic performance is poor, and the measurement accuracy is affected by motion acceleration and electromagnetic interference. Different sensor characteristics are used in the frequency domain to obtain accurate attitude information, and the CF scheme is used to fuse the measurement data obtained by using the gyroscope, accelerometer, and magnetometer. Figure3 demonstrates the principle of the fusion algorithm. In Figure 3, wb=wxbwybwzbT is the body motion angular velocity measured by the gyroscope, ab=axbaybazbT is the observed value of the accelerometer, mb=mxbmybmzbT is the observed value of the geomagnetic field of the magnetometer, Q is the attitude of the vehicle, Qg is the attitude obtained by using the integral of the gyroscope-measured value, Qa is the attitude quaternion obtained by fusing the accelerometer- and magnetometer-measured values, Q^ is the optimal estimated attitude which is the output of the complementary filter, βf is the parameter of the complementary filter, and W is the weight matrix.Figure 3
Principle of the attitude calculation algorithm.Firstly, the vehicle motion state is judged by the accelerometer-measured values to determine the weight matrixW, and the interference of vehicle motion acceleration is limited by adjusting the weight matrix. Secondly, nonlinear attitude quaternion equations are constructed by using the accelerometer and magnetometer information and solved by using the Gauss–Newton method to fuse the accelerometer- and magnetometer-measured values. Thirdly, filter parameter βf is determined by the vehicle motion state to achieve the adaptive cut-off frequency of the complementary filter. Finally, the measurement data obtained by using the gyroscope, accelerometer, and magnetometer are fused to ensure the accuracy of attitude measurement under static and dynamic conditions.(1) Attitude Update Algorithm of the Gyroscope. Runge–Kutta method is used to solve quaternion differential equations numerically in accordance with the measured angular velocity of a body. The discrete quaternion update equation is(1)Qg=Q^k−1+ΔT6K1+2K2+2K3+K4,where Qg denotes the output attitude of the gyroscope, Q^k−1 indicates the last estimated attitude of the complementary filter, ΔT represents the sampling interval, and K1,K2,K3,andK4 are obtained by the following equation:(2)K1=12ΩkQ^k−1,K2=12ΩkQ^k−1+ΔT2K1,K3=12ΩkQ^k−1+ΔT2K2,K4=12ΩkQ^k−1+ΔTK3,where Ωk is the output angular velocity matrix of the gyroscope at the current time and Ωk is determined by the following equation:(3)Ωk=0−wxkb−wykb−wzkbwxkb0wzkb−wykbwykb−wzkb0wxkbwzkbwykb−wxkb0,where wxkb, wykb, and wzkb denote the output values of the three axes of the gyroscope at the current time.(2) Attitude Information Fusion of the Accelerometer and Magnetometer. The estimated gravitational acceleration vector in the vehicle coordinate system is(4)g^b=Rgb00G=RTbg00G=2Gq1q3−q0q22Gq2q3+q0q1Gq02−q12−q22+q32.The gravitational acceleration vectorab measured by the accelerometer should be equal to the estimated gravitational acceleration vector g^b when a vehicle is in a static state. Thus, equation (5) can be obtained:(5)fa1=2Gq1q3−q0q2−axb=0,fa2=2Gq2q3+q0q1−ayb=0,fa3=Gq02−q12−q22+q32−azb=0.The geomagnetic vector measured by the magnetometer in the initial attitude of the vehicle ismx0,my0,mz0T. During the motion of vehicle, the estimated geomagnetic field vector in the vehicle coordinate system can be obtained using the coordinate transformation matrix Rgb from the geodetic coordinate system to the vehicle coordinate system:(6)m^b=RgbMxMyMz=Mxq02+q12−q22−q32+2Myq1q2+q0q3+2Mzq1q3−q0q22Mxq1q2−q0q3+Myq02−q12+q22−q32+2Mzq2q3+q0q12Mxq1q3+q0q2+2Myq2q3−q0q1+Mzq02−q12−q22+q32.The geomagnetic field vectormb measured by the magnetometer must be equal to the estimated geomagnetic field vector m^b. Thus, equation (7) can be obtained:(7)fa4=Mxq02+q12−q22−q32+2Myq1q2+q0q3+2Mzq1q3−q0q2−mxb=0,fa5=2Mxq1q2−q0q3+Myq02−q12+q22−q32+2Mzq2q3+q0q1−myb=0,fa6=2Mxq1q3+q0q2+2Myq2q3−q0q1+Mzq02−q12−q22+q32−mzb=0.Normalised quaternion is used to represent an attitude, that is,Q=1. Thus, the quaternion also satisfies the following equation:(8)fa7=q02+q12+q22+q32−1=0.Letfaq0,q1,q2,q3=fa1,fa2,fa3,fa4,fa5,fa6,fa7T; the overdetermined nonlinear equations for q0,q1,q2,andq3 are obtained from equations (5), (7), and (8):(9)fa=fa1,fa2,fa3,fa4,fa5,fa6,fa7T=0.Quaternion attitude information can be obtained by solving equation (9) using the accelerometer-and magnetometer-measured values at the current moment. However, the equations disregard the weight of each equation in the data fusion and can obtain accurate results under static and uniform motions. Acceleration interference information is obtained from the accelerometer-measured values when the vehicle has a moving acceleration, and errors are identified in the test result. To limit the influence of the vehicle moving acceleration on data fusion results, this study introduces the weight matrix W to modify equations. W is a diagonal matrix that contains three parameters, namely, λa,λm,andλQ, which is defined as follows:(10)W=λ1λ2⋱λ7=λaI3×3λmI3×3λQ,where I3×3 is the unit matrix, λa denotes the weight of the accelerometer-measured values, λm denotes the weight of the magnetometer-measured values, and λQ denotes the weight of the normalised quaternion equation.After considering the weight of each equation, the nonlinear equation forq0,q1,q2,andq3 is(11)Fax=Wfa=λ1fa1,λ2fa2,…,λ7fa7T=0,where x=q0,q1,q2,q3.The solutions of equations are transformed into a weighted least square problem:(12)min∑i=17λifaix2=minWfax2.The aforementioned equation is solved to obtain the attitude quaternionQa from the accelerometer- and magnetometer-measured values through the Gauss–Newton iteration method.(3) Design of the Complementary Filter. The digital implementation of the complementary filter is(13)Q^=βfQg+1−βfQa,where Q is the optimal estimated attitude which is the output of the complementary filter and βf is the parameter of the complementary filter. The relationship between βf and filter cut-off frequency fT is(14)βf=11+2πΔTfT.The attitude data are dominated by the gyroscope when the signal frequency is greater thanfT, whereas the attitude fusion results of the accelerometer and magnetometer and the low- and high-frequency interferences are separated by the complementary filter effectively.(4) Adaptive Adjustment Scheme of Filter Parameters and Weight Matrix. The accelerometer-measured value is the vector sum of the gravitational and moving acceleration when a vehicle has a moving acceleration. At this time, the mode of accelerometer measurement is no longer the magnitude of the gravitational acceleration. Therefore, a vehicle motion state can be judged by the modulus ab of the accelerometer-measured value.The error of the upper limitσ0 is given (σ0 is a relatively small positive real number) because of the measurement error. The vehicle is considered in a static or a uniform motion state when the magnitude of accelerometer-measured value satisfies the following equation:(15)axb2+ayb2+azb2−G≤σ0.The error is small when a vehicle has low moving acceleration although the accelerometer introduces an attitude measurement error. At this time, the accelerometer-measured value still plays a beneficial role in the attitude fusion and cannot be completely ignored. Owing to a positive real numberσ1, the vehicle is considered in a low acceleration state when the accelerometer-measured value satisfies the following equation:(16)σ0<axb2+ayb2+azb2−G≤σ1.A vehicle is considered in a high-acceleration state when the accelerometer-measured value satisfies equation (17). At this time, the accelerometer-measured value is irrelevant to attitude measurement:(17)axb2+ayb2+azb2−G>σ1.The adjustment scheme of the weight matrix and filter parameters is formulated by the motion state of a vehicle. The parameters are listed in Table2.Table 2
Adjustment scheme of the parameters.
Vehicle status
λ
a
λ
m
λ
Q
β
f
Static
1
/
G
1/H
1
0.61
Low acceleration
σ
1
−
δ
G
/
σ
1
G
1/H
1
0.88
High acceleration
0
1/H
1
0.94


#### 3.1.2. Experiment for the Attitude Measurement

The comparison experiment for attitude measurement is conducted by using the VG440 model gyroscope of Memsic Semiconductor Company® to verify the effectiveness of the proposed algorithm. Vertical gyroscope and sensing device are fixed on the test carrier plate to ensure that they perform a random rotation motion in each direction. Subsequently, the attitude information obtained from these materials is recorded and compared.Figures4–6 exhibit the comparison of the measured data and the error of roll, pitch, and yaw angles, correspondingly. In the random rotation, the attitude angle error is less than 1°, thereby satisfying the design requirements.Figure 4
Roll angle: (a) comparison of the measured values; (b) errors.
(a)
(b)Figure 5
Pitch angle: (a) comparison of the measured values; (b) errors.
(a)
(b)Figure 6
Yaw angle: (a) comparison of the measured values; (b) errors.
(a)
(b)


### 3.2. Displacement Reconstruction

#### 3.2.1. Vibration Displacement Reconstruction Algorithm

The implementation of the vibration displacement reconstruction algorithm is as follows: Firstly, the accelerometer is compensated by the gravitational acceleration to obtain the moving acceleration that is indicated in its own coordinate system. Secondly, the moving acceleration of the vehicle coordinate system is transformed into a geodetic coordinate system by using an attitude transformation matrix, and the time history of vehicle acceleration is obtained in the geodetic coordinate system. Finally, low-frequency attenuation integration algorithm is used to reconstruct the vibration displacement information from acceleration information. The principle of the vehicle vibration displacement reconstruction algorithm is displayed in Figure7, where ϕ, θ, and ψ denote the roll, pitch, and yaw angles of the vehicle.Figure 7
Principle of the vehicle vibration displacement reconstruction algorithm.(1) Gravitational Acceleration Compensation. Whether a vehicle is still or moving, the output of the accelerometer contains components of the gravitational acceleration on each axis. In fact, the output value of accelerometer is the vector sum of the moving and gravitational accelerations. Therefore, gravitational acceleration compensation must be performed on the output value of the accelerometer to obtain the moving acceleration of the vehicle. The specific method calculates as follows:(18)amb=ab−Rgbg,where amb is the moving acceleration of the vehicle, ab is the output value of the accelerometer, Rgb is the attitude transformation matrix from the vehicle coordinate system to the geodetic coordinate system, and g=00GT represents the local gravitational acceleration vector in the geodetic coordinate system.(2) Conversion of Motion Acceleration to the Geodetic Coordinate System. The obtained moving acceleration amb after gravity compensation is expressed in the vehicle coordinate system and must be converted into the geodetic coordinate system to obtain the moving acceleration information in space. The representation of the moving acceleration amg of a vehicle in the geodetic coordinate system is as follows:(19)amg=Rbgamb,where Rbg is the attitude transformation matrix from the vehicle coordinate system to the geodetic coordinate system.(3) Displacement Is Reconstructed by Acceleration. The three-axis acceleration is processed by using the low-frequency attenuation algorithm to determine displacement after obtaining the moving acceleration information in the geodetic coordinate system. The integral displacement Smg is obtained as follows:(20)Smg=−F−1ω2ω4+β2Famgt,where F and F−1 represent the Fourier transform and inverse Fourier transform, correspondingly.A vibration of the test carrier is implemented in all directions, and the vibration attitude of the test carrier is reconstructed by using this algorithm. Figure8 presents the 6-DOF motion information.Figure 8
6-DOF motion information of the test carrier: (a)X displacement; (b) Y displacement; (c) Z displacement; (d) roll angle; (e) pitch angle; (f) yaw angle.
(a)
(b)
(c)
(d)
(e)
(f)


### 3.3. Design of a Representation System Controller

The three-state feedbacks are position, velocity, and acceleration feedback. The three-state control includes three-state feedback and feedforward control. The introduction of acceleration feedback can improve the damping ratio of system under the stable condition of the inner loop. The introduction of velocity feedback reduces the position closed-loop damping ratio slightly but extends the bandwidth of the closed-loop system which is beneficial. The three-state feedforward control can offset the pole which is close to the virtual axis in the closed-loop system to extend the system bandwidth further.


#### 3.3.1. Design of the Three-State Feedback Controller

A mathematical model of the single hydraulic cylinder electrohydraulic servo system is established, and the open-loop transfer function of system is(21)Gks=kkss2/ωk2+2ξks/ωk+1,where kk is the open-loop gain of a system, ωk is the natural frequency of the hydraulic power mechanism, and ξk is the damping ratio of the hydraulic power mechanism.Figure9 illustrates the block diagram of a system after adding three-state feedback. In Figure 9, kdf,kvf,andkaf are the calculated parameters and represent the position, velocity, and acceleration feedback coefficients of the controller.Figure 9
Block diagram of the three-state feedback control system.In accordance with the system block diagram, the closed-loop transfer function of the three-state feedback system can be obtained as follows:(22)Gfs=1s3/kdfkkωk2+2ξk/kdfkkωk+kaf/kdfs2+kvfkk+1/kdfkks+1.The closed-loop transfer function after setting the expected three-state feedback is(23)Ws=1s/ωr+1s2/ωc2+2ξcs/ωc+1,where ωr is the expected final bandwidth of a system (system bandwidth after correction of three-state feedback and feedforward system) to achieve the final dynamic characteristic requirements. ωr=157rad/s (approximately 25 Hz) is selected. The value of ωc is generally 1.05–1.20 times that of the hydraulic natural frequency. In this study, ωc=1.2ωk and ξc=0.7 are selected to ensure that the system has a sufficient stability margin after the three-state feedback.The system transfer functionGfs is the same as the expected transfer function Ws after the three-state feedback. Equations (22) and (23) can be used to solve the three-state feedback coefficients:(24)kdf=ωrωc2kkωk2,kvf=kdf2ξcωc+1ωr−1kk,kaf=kdf2ξcωrωc+1ωc2−2ξkkkωk.


#### 3.3.2. Design of the Three-State Feedforward Controller

The three-state feedforward controller is added before a system completes the three-state feedback regulation, thereby eliminating the pole which is close to the virtual axis in the closed-loop transfer function. Thus, the system bandwidth can be extended. Let the transfer function of the three-state feedforward controller be(25)Bs=kaps2+kvps+kdp,where kap,kvp, and kdp are the determined controller parameters.In equation (23), the three-state feedforward controller satisfies equation (26) to offset the pole of the three-state feedback control system, which pole is close to the virtual axis:(26)Bs=s2ωc2+2ξcsωc+1.Equation (27) can be obtained by integrating equations (25) and (26).The parameters of the three-state feedback controller can be obtained by substituting the selected parameter values ofξc and ωc into the following equation:(27)kdp=1,kvp=2ξcωc,kap=1ωc2.


## 3.1. Attitude Calculation

### 3.1.1. Attitude Measurement Algorithm

A gyroscope has a remarkable dynamic performance, but an accumulative error during the calculating process results in a time lapse. Accelerometer and magnetometer use gravity and geomagnetic fields for attitude measurement, respectively. No accumulative error has been observed. However, the dynamic performance is poor, and the measurement accuracy is affected by motion acceleration and electromagnetic interference. Different sensor characteristics are used in the frequency domain to obtain accurate attitude information, and the CF scheme is used to fuse the measurement data obtained by using the gyroscope, accelerometer, and magnetometer. Figure3 demonstrates the principle of the fusion algorithm. In Figure 3, wb=wxbwybwzbT is the body motion angular velocity measured by the gyroscope, ab=axbaybazbT is the observed value of the accelerometer, mb=mxbmybmzbT is the observed value of the geomagnetic field of the magnetometer, Q is the attitude of the vehicle, Qg is the attitude obtained by using the integral of the gyroscope-measured value, Qa is the attitude quaternion obtained by fusing the accelerometer- and magnetometer-measured values, Q^ is the optimal estimated attitude which is the output of the complementary filter, βf is the parameter of the complementary filter, and W is the weight matrix.Figure 3
Principle of the attitude calculation algorithm.Firstly, the vehicle motion state is judged by the accelerometer-measured values to determine the weight matrixW, and the interference of vehicle motion acceleration is limited by adjusting the weight matrix. Secondly, nonlinear attitude quaternion equations are constructed by using the accelerometer and magnetometer information and solved by using the Gauss–Newton method to fuse the accelerometer- and magnetometer-measured values. Thirdly, filter parameter βf is determined by the vehicle motion state to achieve the adaptive cut-off frequency of the complementary filter. Finally, the measurement data obtained by using the gyroscope, accelerometer, and magnetometer are fused to ensure the accuracy of attitude measurement under static and dynamic conditions.(1) Attitude Update Algorithm of the Gyroscope. Runge–Kutta method is used to solve quaternion differential equations numerically in accordance with the measured angular velocity of a body. The discrete quaternion update equation is(1)Qg=Q^k−1+ΔT6K1+2K2+2K3+K4,where Qg denotes the output attitude of the gyroscope, Q^k−1 indicates the last estimated attitude of the complementary filter, ΔT represents the sampling interval, and K1,K2,K3,andK4 are obtained by the following equation:(2)K1=12ΩkQ^k−1,K2=12ΩkQ^k−1+ΔT2K1,K3=12ΩkQ^k−1+ΔT2K2,K4=12ΩkQ^k−1+ΔTK3,where Ωk is the output angular velocity matrix of the gyroscope at the current time and Ωk is determined by the following equation:(3)Ωk=0−wxkb−wykb−wzkbwxkb0wzkb−wykbwykb−wzkb0wxkbwzkbwykb−wxkb0,where wxkb, wykb, and wzkb denote the output values of the three axes of the gyroscope at the current time.(2) Attitude Information Fusion of the Accelerometer and Magnetometer. The estimated gravitational acceleration vector in the vehicle coordinate system is(4)g^b=Rgb00G=RTbg00G=2Gq1q3−q0q22Gq2q3+q0q1Gq02−q12−q22+q32.The gravitational acceleration vectorab measured by the accelerometer should be equal to the estimated gravitational acceleration vector g^b when a vehicle is in a static state. Thus, equation (5) can be obtained:(5)fa1=2Gq1q3−q0q2−axb=0,fa2=2Gq2q3+q0q1−ayb=0,fa3=Gq02−q12−q22+q32−azb=0.The geomagnetic vector measured by the magnetometer in the initial attitude of the vehicle ismx0,my0,mz0T. During the motion of vehicle, the estimated geomagnetic field vector in the vehicle coordinate system can be obtained using the coordinate transformation matrix Rgb from the geodetic coordinate system to the vehicle coordinate system:(6)m^b=RgbMxMyMz=Mxq02+q12−q22−q32+2Myq1q2+q0q3+2Mzq1q3−q0q22Mxq1q2−q0q3+Myq02−q12+q22−q32+2Mzq2q3+q0q12Mxq1q3+q0q2+2Myq2q3−q0q1+Mzq02−q12−q22+q32.The geomagnetic field vectormb measured by the magnetometer must be equal to the estimated geomagnetic field vector m^b. Thus, equation (7) can be obtained:(7)fa4=Mxq02+q12−q22−q32+2Myq1q2+q0q3+2Mzq1q3−q0q2−mxb=0,fa5=2Mxq1q2−q0q3+Myq02−q12+q22−q32+2Mzq2q3+q0q1−myb=0,fa6=2Mxq1q3+q0q2+2Myq2q3−q0q1+Mzq02−q12−q22+q32−mzb=0.Normalised quaternion is used to represent an attitude, that is,Q=1. Thus, the quaternion also satisfies the following equation:(8)fa7=q02+q12+q22+q32−1=0.Letfaq0,q1,q2,q3=fa1,fa2,fa3,fa4,fa5,fa6,fa7T; the overdetermined nonlinear equations for q0,q1,q2,andq3 are obtained from equations (5), (7), and (8):(9)fa=fa1,fa2,fa3,fa4,fa5,fa6,fa7T=0.Quaternion attitude information can be obtained by solving equation (9) using the accelerometer-and magnetometer-measured values at the current moment. However, the equations disregard the weight of each equation in the data fusion and can obtain accurate results under static and uniform motions. Acceleration interference information is obtained from the accelerometer-measured values when the vehicle has a moving acceleration, and errors are identified in the test result. To limit the influence of the vehicle moving acceleration on data fusion results, this study introduces the weight matrix W to modify equations. W is a diagonal matrix that contains three parameters, namely, λa,λm,andλQ, which is defined as follows:(10)W=λ1λ2⋱λ7=λaI3×3λmI3×3λQ,where I3×3 is the unit matrix, λa denotes the weight of the accelerometer-measured values, λm denotes the weight of the magnetometer-measured values, and λQ denotes the weight of the normalised quaternion equation.After considering the weight of each equation, the nonlinear equation forq0,q1,q2,andq3 is(11)Fax=Wfa=λ1fa1,λ2fa2,…,λ7fa7T=0,where x=q0,q1,q2,q3.The solutions of equations are transformed into a weighted least square problem:(12)min∑i=17λifaix2=minWfax2.The aforementioned equation is solved to obtain the attitude quaternionQa from the accelerometer- and magnetometer-measured values through the Gauss–Newton iteration method.(3) Design of the Complementary Filter. The digital implementation of the complementary filter is(13)Q^=βfQg+1−βfQa,where Q is the optimal estimated attitude which is the output of the complementary filter and βf is the parameter of the complementary filter. The relationship between βf and filter cut-off frequency fT is(14)βf=11+2πΔTfT.The attitude data are dominated by the gyroscope when the signal frequency is greater thanfT, whereas the attitude fusion results of the accelerometer and magnetometer and the low- and high-frequency interferences are separated by the complementary filter effectively.(4) Adaptive Adjustment Scheme of Filter Parameters and Weight Matrix. The accelerometer-measured value is the vector sum of the gravitational and moving acceleration when a vehicle has a moving acceleration. At this time, the mode of accelerometer measurement is no longer the magnitude of the gravitational acceleration. Therefore, a vehicle motion state can be judged by the modulus ab of the accelerometer-measured value.The error of the upper limitσ0 is given (σ0 is a relatively small positive real number) because of the measurement error. The vehicle is considered in a static or a uniform motion state when the magnitude of accelerometer-measured value satisfies the following equation:(15)axb2+ayb2+azb2−G≤σ0.The error is small when a vehicle has low moving acceleration although the accelerometer introduces an attitude measurement error. At this time, the accelerometer-measured value still plays a beneficial role in the attitude fusion and cannot be completely ignored. Owing to a positive real numberσ1, the vehicle is considered in a low acceleration state when the accelerometer-measured value satisfies the following equation:(16)σ0<axb2+ayb2+azb2−G≤σ1.A vehicle is considered in a high-acceleration state when the accelerometer-measured value satisfies equation (17). At this time, the accelerometer-measured value is irrelevant to attitude measurement:(17)axb2+ayb2+azb2−G>σ1.The adjustment scheme of the weight matrix and filter parameters is formulated by the motion state of a vehicle. The parameters are listed in Table2.Table 2
Adjustment scheme of the parameters.
Vehicle status
λ
a
λ
m
λ
Q
β
f
Static
1
/
G
1/H
1
0.61
Low acceleration
σ
1
−
δ
G
/
σ
1
G
1/H
1
0.88
High acceleration
0
1/H
1
0.94


### 3.1.2. Experiment for the Attitude Measurement

The comparison experiment for attitude measurement is conducted by using the VG440 model gyroscope of Memsic Semiconductor Company® to verify the effectiveness of the proposed algorithm. Vertical gyroscope and sensing device are fixed on the test carrier plate to ensure that they perform a random rotation motion in each direction. Subsequently, the attitude information obtained from these materials is recorded and compared.Figures4–6 exhibit the comparison of the measured data and the error of roll, pitch, and yaw angles, correspondingly. In the random rotation, the attitude angle error is less than 1°, thereby satisfying the design requirements.Figure 4
Roll angle: (a) comparison of the measured values; (b) errors.
(a)
(b)Figure 5
Pitch angle: (a) comparison of the measured values; (b) errors.
(a)
(b)Figure 6
Yaw angle: (a) comparison of the measured values; (b) errors.
(a)
(b)


## 3.1.1. Attitude Measurement Algorithm

A gyroscope has a remarkable dynamic performance, but an accumulative error during the calculating process results in a time lapse. Accelerometer and magnetometer use gravity and geomagnetic fields for attitude measurement, respectively. No accumulative error has been observed. However, the dynamic performance is poor, and the measurement accuracy is affected by motion acceleration and electromagnetic interference. Different sensor characteristics are used in the frequency domain to obtain accurate attitude information, and the CF scheme is used to fuse the measurement data obtained by using the gyroscope, accelerometer, and magnetometer. Figure3 demonstrates the principle of the fusion algorithm. In Figure 3, wb=wxbwybwzbT is the body motion angular velocity measured by the gyroscope, ab=axbaybazbT is the observed value of the accelerometer, mb=mxbmybmzbT is the observed value of the geomagnetic field of the magnetometer, Q is the attitude of the vehicle, Qg is the attitude obtained by using the integral of the gyroscope-measured value, Qa is the attitude quaternion obtained by fusing the accelerometer- and magnetometer-measured values, Q^ is the optimal estimated attitude which is the output of the complementary filter, βf is the parameter of the complementary filter, and W is the weight matrix.Figure 3
Principle of the attitude calculation algorithm.Firstly, the vehicle motion state is judged by the accelerometer-measured values to determine the weight matrixW, and the interference of vehicle motion acceleration is limited by adjusting the weight matrix. Secondly, nonlinear attitude quaternion equations are constructed by using the accelerometer and magnetometer information and solved by using the Gauss–Newton method to fuse the accelerometer- and magnetometer-measured values. Thirdly, filter parameter βf is determined by the vehicle motion state to achieve the adaptive cut-off frequency of the complementary filter. Finally, the measurement data obtained by using the gyroscope, accelerometer, and magnetometer are fused to ensure the accuracy of attitude measurement under static and dynamic conditions.(1) Attitude Update Algorithm of the Gyroscope. Runge–Kutta method is used to solve quaternion differential equations numerically in accordance with the measured angular velocity of a body. The discrete quaternion update equation is(1)Qg=Q^k−1+ΔT6K1+2K2+2K3+K4,where Qg denotes the output attitude of the gyroscope, Q^k−1 indicates the last estimated attitude of the complementary filter, ΔT represents the sampling interval, and K1,K2,K3,andK4 are obtained by the following equation:(2)K1=12ΩkQ^k−1,K2=12ΩkQ^k−1+ΔT2K1,K3=12ΩkQ^k−1+ΔT2K2,K4=12ΩkQ^k−1+ΔTK3,where Ωk is the output angular velocity matrix of the gyroscope at the current time and Ωk is determined by the following equation:(3)Ωk=0−wxkb−wykb−wzkbwxkb0wzkb−wykbwykb−wzkb0wxkbwzkbwykb−wxkb0,where wxkb, wykb, and wzkb denote the output values of the three axes of the gyroscope at the current time.(2) Attitude Information Fusion of the Accelerometer and Magnetometer. The estimated gravitational acceleration vector in the vehicle coordinate system is(4)g^b=Rgb00G=RTbg00G=2Gq1q3−q0q22Gq2q3+q0q1Gq02−q12−q22+q32.The gravitational acceleration vectorab measured by the accelerometer should be equal to the estimated gravitational acceleration vector g^b when a vehicle is in a static state. Thus, equation (5) can be obtained:(5)fa1=2Gq1q3−q0q2−axb=0,fa2=2Gq2q3+q0q1−ayb=0,fa3=Gq02−q12−q22+q32−azb=0.The geomagnetic vector measured by the magnetometer in the initial attitude of the vehicle ismx0,my0,mz0T. During the motion of vehicle, the estimated geomagnetic field vector in the vehicle coordinate system can be obtained using the coordinate transformation matrix Rgb from the geodetic coordinate system to the vehicle coordinate system:(6)m^b=RgbMxMyMz=Mxq02+q12−q22−q32+2Myq1q2+q0q3+2Mzq1q3−q0q22Mxq1q2−q0q3+Myq02−q12+q22−q32+2Mzq2q3+q0q12Mxq1q3+q0q2+2Myq2q3−q0q1+Mzq02−q12−q22+q32.The geomagnetic field vectormb measured by the magnetometer must be equal to the estimated geomagnetic field vector m^b. Thus, equation (7) can be obtained:(7)fa4=Mxq02+q12−q22−q32+2Myq1q2+q0q3+2Mzq1q3−q0q2−mxb=0,fa5=2Mxq1q2−q0q3+Myq02−q12+q22−q32+2Mzq2q3+q0q1−myb=0,fa6=2Mxq1q3+q0q2+2Myq2q3−q0q1+Mzq02−q12−q22+q32−mzb=0.Normalised quaternion is used to represent an attitude, that is,Q=1. Thus, the quaternion also satisfies the following equation:(8)fa7=q02+q12+q22+q32−1=0.Letfaq0,q1,q2,q3=fa1,fa2,fa3,fa4,fa5,fa6,fa7T; the overdetermined nonlinear equations for q0,q1,q2,andq3 are obtained from equations (5), (7), and (8):(9)fa=fa1,fa2,fa3,fa4,fa5,fa6,fa7T=0.Quaternion attitude information can be obtained by solving equation (9) using the accelerometer-and magnetometer-measured values at the current moment. However, the equations disregard the weight of each equation in the data fusion and can obtain accurate results under static and uniform motions. Acceleration interference information is obtained from the accelerometer-measured values when the vehicle has a moving acceleration, and errors are identified in the test result. To limit the influence of the vehicle moving acceleration on data fusion results, this study introduces the weight matrix W to modify equations. W is a diagonal matrix that contains three parameters, namely, λa,λm,andλQ, which is defined as follows:(10)W=λ1λ2⋱λ7=λaI3×3λmI3×3λQ,where I3×3 is the unit matrix, λa denotes the weight of the accelerometer-measured values, λm denotes the weight of the magnetometer-measured values, and λQ denotes the weight of the normalised quaternion equation.After considering the weight of each equation, the nonlinear equation forq0,q1,q2,andq3 is(11)Fax=Wfa=λ1fa1,λ2fa2,…,λ7fa7T=0,where x=q0,q1,q2,q3.The solutions of equations are transformed into a weighted least square problem:(12)min∑i=17λifaix2=minWfax2.The aforementioned equation is solved to obtain the attitude quaternionQa from the accelerometer- and magnetometer-measured values through the Gauss–Newton iteration method.(3) Design of the Complementary Filter. The digital implementation of the complementary filter is(13)Q^=βfQg+1−βfQa,where Q is the optimal estimated attitude which is the output of the complementary filter and βf is the parameter of the complementary filter. The relationship between βf and filter cut-off frequency fT is(14)βf=11+2πΔTfT.The attitude data are dominated by the gyroscope when the signal frequency is greater thanfT, whereas the attitude fusion results of the accelerometer and magnetometer and the low- and high-frequency interferences are separated by the complementary filter effectively.(4) Adaptive Adjustment Scheme of Filter Parameters and Weight Matrix. The accelerometer-measured value is the vector sum of the gravitational and moving acceleration when a vehicle has a moving acceleration. At this time, the mode of accelerometer measurement is no longer the magnitude of the gravitational acceleration. Therefore, a vehicle motion state can be judged by the modulus ab of the accelerometer-measured value.The error of the upper limitσ0 is given (σ0 is a relatively small positive real number) because of the measurement error. The vehicle is considered in a static or a uniform motion state when the magnitude of accelerometer-measured value satisfies the following equation:(15)axb2+ayb2+azb2−G≤σ0.The error is small when a vehicle has low moving acceleration although the accelerometer introduces an attitude measurement error. At this time, the accelerometer-measured value still plays a beneficial role in the attitude fusion and cannot be completely ignored. Owing to a positive real numberσ1, the vehicle is considered in a low acceleration state when the accelerometer-measured value satisfies the following equation:(16)σ0<axb2+ayb2+azb2−G≤σ1.A vehicle is considered in a high-acceleration state when the accelerometer-measured value satisfies equation (17). At this time, the accelerometer-measured value is irrelevant to attitude measurement:(17)axb2+ayb2+azb2−G>σ1.The adjustment scheme of the weight matrix and filter parameters is formulated by the motion state of a vehicle. The parameters are listed in Table2.Table 2
Adjustment scheme of the parameters.
Vehicle status
λ
a
λ
m
λ
Q
β
f
Static
1
/
G
1/H
1
0.61
Low acceleration
σ
1
−
δ
G
/
σ
1
G
1/H
1
0.88
High acceleration
0
1/H
1
0.94


## 3.1.2. Experiment for the Attitude Measurement

The comparison experiment for attitude measurement is conducted by using the VG440 model gyroscope of Memsic Semiconductor Company® to verify the effectiveness of the proposed algorithm. Vertical gyroscope and sensing device are fixed on the test carrier plate to ensure that they perform a random rotation motion in each direction. Subsequently, the attitude information obtained from these materials is recorded and compared.Figures4–6 exhibit the comparison of the measured data and the error of roll, pitch, and yaw angles, correspondingly. In the random rotation, the attitude angle error is less than 1°, thereby satisfying the design requirements.Figure 4
Roll angle: (a) comparison of the measured values; (b) errors.
(a)
(b)Figure 5
Pitch angle: (a) comparison of the measured values; (b) errors.
(a)
(b)Figure 6
Yaw angle: (a) comparison of the measured values; (b) errors.
(a)
(b)


## 3.2. Displacement Reconstruction

### 3.2.1. Vibration Displacement Reconstruction Algorithm

The implementation of the vibration displacement reconstruction algorithm is as follows: Firstly, the accelerometer is compensated by the gravitational acceleration to obtain the moving acceleration that is indicated in its own coordinate system. Secondly, the moving acceleration of the vehicle coordinate system is transformed into a geodetic coordinate system by using an attitude transformation matrix, and the time history of vehicle acceleration is obtained in the geodetic coordinate system. Finally, low-frequency attenuation integration algorithm is used to reconstruct the vibration displacement information from acceleration information. The principle of the vehicle vibration displacement reconstruction algorithm is displayed in Figure7, where ϕ, θ, and ψ denote the roll, pitch, and yaw angles of the vehicle.Figure 7
Principle of the vehicle vibration displacement reconstruction algorithm.(1) Gravitational Acceleration Compensation. Whether a vehicle is still or moving, the output of the accelerometer contains components of the gravitational acceleration on each axis. In fact, the output value of accelerometer is the vector sum of the moving and gravitational accelerations. Therefore, gravitational acceleration compensation must be performed on the output value of the accelerometer to obtain the moving acceleration of the vehicle. The specific method calculates as follows:(18)amb=ab−Rgbg,where amb is the moving acceleration of the vehicle, ab is the output value of the accelerometer, Rgb is the attitude transformation matrix from the vehicle coordinate system to the geodetic coordinate system, and g=00GT represents the local gravitational acceleration vector in the geodetic coordinate system.(2) Conversion of Motion Acceleration to the Geodetic Coordinate System. The obtained moving acceleration amb after gravity compensation is expressed in the vehicle coordinate system and must be converted into the geodetic coordinate system to obtain the moving acceleration information in space. The representation of the moving acceleration amg of a vehicle in the geodetic coordinate system is as follows:(19)amg=Rbgamb,where Rbg is the attitude transformation matrix from the vehicle coordinate system to the geodetic coordinate system.(3) Displacement Is Reconstructed by Acceleration. The three-axis acceleration is processed by using the low-frequency attenuation algorithm to determine displacement after obtaining the moving acceleration information in the geodetic coordinate system. The integral displacement Smg is obtained as follows:(20)Smg=−F−1ω2ω4+β2Famgt,where F and F−1 represent the Fourier transform and inverse Fourier transform, correspondingly.A vibration of the test carrier is implemented in all directions, and the vibration attitude of the test carrier is reconstructed by using this algorithm. Figure8 presents the 6-DOF motion information.Figure 8
6-DOF motion information of the test carrier: (a)X displacement; (b) Y displacement; (c) Z displacement; (d) roll angle; (e) pitch angle; (f) yaw angle.
(a)
(b)
(c)
(d)
(e)
(f)


## 3.2.1. Vibration Displacement Reconstruction Algorithm

The implementation of the vibration displacement reconstruction algorithm is as follows: Firstly, the accelerometer is compensated by the gravitational acceleration to obtain the moving acceleration that is indicated in its own coordinate system. Secondly, the moving acceleration of the vehicle coordinate system is transformed into a geodetic coordinate system by using an attitude transformation matrix, and the time history of vehicle acceleration is obtained in the geodetic coordinate system. Finally, low-frequency attenuation integration algorithm is used to reconstruct the vibration displacement information from acceleration information. The principle of the vehicle vibration displacement reconstruction algorithm is displayed in Figure7, where ϕ, θ, and ψ denote the roll, pitch, and yaw angles of the vehicle.Figure 7
Principle of the vehicle vibration displacement reconstruction algorithm.(1) Gravitational Acceleration Compensation. Whether a vehicle is still or moving, the output of the accelerometer contains components of the gravitational acceleration on each axis. In fact, the output value of accelerometer is the vector sum of the moving and gravitational accelerations. Therefore, gravitational acceleration compensation must be performed on the output value of the accelerometer to obtain the moving acceleration of the vehicle. The specific method calculates as follows:(18)amb=ab−Rgbg,where amb is the moving acceleration of the vehicle, ab is the output value of the accelerometer, Rgb is the attitude transformation matrix from the vehicle coordinate system to the geodetic coordinate system, and g=00GT represents the local gravitational acceleration vector in the geodetic coordinate system.(2) Conversion of Motion Acceleration to the Geodetic Coordinate System. The obtained moving acceleration amb after gravity compensation is expressed in the vehicle coordinate system and must be converted into the geodetic coordinate system to obtain the moving acceleration information in space. The representation of the moving acceleration amg of a vehicle in the geodetic coordinate system is as follows:(19)amg=Rbgamb,where Rbg is the attitude transformation matrix from the vehicle coordinate system to the geodetic coordinate system.(3) Displacement Is Reconstructed by Acceleration. The three-axis acceleration is processed by using the low-frequency attenuation algorithm to determine displacement after obtaining the moving acceleration information in the geodetic coordinate system. The integral displacement Smg is obtained as follows:(20)Smg=−F−1ω2ω4+β2Famgt,where F and F−1 represent the Fourier transform and inverse Fourier transform, correspondingly.A vibration of the test carrier is implemented in all directions, and the vibration attitude of the test carrier is reconstructed by using this algorithm. Figure8 presents the 6-DOF motion information.Figure 8
6-DOF motion information of the test carrier: (a)X displacement; (b) Y displacement; (c) Z displacement; (d) roll angle; (e) pitch angle; (f) yaw angle.
(a)
(b)
(c)
(d)
(e)
(f)


## 3.3. Design of a Representation System Controller

The three-state feedbacks are position, velocity, and acceleration feedback. The three-state control includes three-state feedback and feedforward control. The introduction of acceleration feedback can improve the damping ratio of system under the stable condition of the inner loop. The introduction of velocity feedback reduces the position closed-loop damping ratio slightly but extends the bandwidth of the closed-loop system which is beneficial. The three-state feedforward control can offset the pole which is close to the virtual axis in the closed-loop system to extend the system bandwidth further.


### 3.3.1. Design of the Three-State Feedback Controller

A mathematical model of the single hydraulic cylinder electrohydraulic servo system is established, and the open-loop transfer function of system is(21)Gks=kkss2/ωk2+2ξks/ωk+1,where kk is the open-loop gain of a system, ωk is the natural frequency of the hydraulic power mechanism, and ξk is the damping ratio of the hydraulic power mechanism.Figure9 illustrates the block diagram of a system after adding three-state feedback. In Figure 9, kdf,kvf,andkaf are the calculated parameters and represent the position, velocity, and acceleration feedback coefficients of the controller.Figure 9
Block diagram of the three-state feedback control system.In accordance with the system block diagram, the closed-loop transfer function of the three-state feedback system can be obtained as follows:(22)Gfs=1s3/kdfkkωk2+2ξk/kdfkkωk+kaf/kdfs2+kvfkk+1/kdfkks+1.The closed-loop transfer function after setting the expected three-state feedback is(23)Ws=1s/ωr+1s2/ωc2+2ξcs/ωc+1,where ωr is the expected final bandwidth of a system (system bandwidth after correction of three-state feedback and feedforward system) to achieve the final dynamic characteristic requirements. ωr=157rad/s (approximately 25 Hz) is selected. The value of ωc is generally 1.05–1.20 times that of the hydraulic natural frequency. In this study, ωc=1.2ωk and ξc=0.7 are selected to ensure that the system has a sufficient stability margin after the three-state feedback.The system transfer functionGfs is the same as the expected transfer function Ws after the three-state feedback. Equations (22) and (23) can be used to solve the three-state feedback coefficients:(24)kdf=ωrωc2kkωk2,kvf=kdf2ξcωc+1ωr−1kk,kaf=kdf2ξcωrωc+1ωc2−2ξkkkωk.


### 3.3.2. Design of the Three-State Feedforward Controller

The three-state feedforward controller is added before a system completes the three-state feedback regulation, thereby eliminating the pole which is close to the virtual axis in the closed-loop transfer function. Thus, the system bandwidth can be extended. Let the transfer function of the three-state feedforward controller be(25)Bs=kaps2+kvps+kdp,where kap,kvp, and kdp are the determined controller parameters.In equation (23), the three-state feedforward controller satisfies equation (26) to offset the pole of the three-state feedback control system, which pole is close to the virtual axis:(26)Bs=s2ωc2+2ξcsωc+1.Equation (27) can be obtained by integrating equations (25) and (26).The parameters of the three-state feedback controller can be obtained by substituting the selected parameter values ofξc and ωc into the following equation:(27)kdp=1,kvp=2ξcωc,kap=1ωc2.


## 3.3.1. Design of the Three-State Feedback Controller

A mathematical model of the single hydraulic cylinder electrohydraulic servo system is established, and the open-loop transfer function of system is(21)Gks=kkss2/ωk2+2ξks/ωk+1,where kk is the open-loop gain of a system, ωk is the natural frequency of the hydraulic power mechanism, and ξk is the damping ratio of the hydraulic power mechanism.Figure9 illustrates the block diagram of a system after adding three-state feedback. In Figure 9, kdf,kvf,andkaf are the calculated parameters and represent the position, velocity, and acceleration feedback coefficients of the controller.Figure 9
Block diagram of the three-state feedback control system.In accordance with the system block diagram, the closed-loop transfer function of the three-state feedback system can be obtained as follows:(22)Gfs=1s3/kdfkkωk2+2ξk/kdfkkωk+kaf/kdfs2+kvfkk+1/kdfkks+1.The closed-loop transfer function after setting the expected three-state feedback is(23)Ws=1s/ωr+1s2/ωc2+2ξcs/ωc+1,where ωr is the expected final bandwidth of a system (system bandwidth after correction of three-state feedback and feedforward system) to achieve the final dynamic characteristic requirements. ωr=157rad/s (approximately 25 Hz) is selected. The value of ωc is generally 1.05–1.20 times that of the hydraulic natural frequency. In this study, ωc=1.2ωk and ξc=0.7 are selected to ensure that the system has a sufficient stability margin after the three-state feedback.The system transfer functionGfs is the same as the expected transfer function Ws after the three-state feedback. Equations (22) and (23) can be used to solve the three-state feedback coefficients:(24)kdf=ωrωc2kkωk2,kvf=kdf2ξcωc+1ωr−1kk,kaf=kdf2ξcωrωc+1ωc2−2ξkkkωk.


## 3.3.2. Design of the Three-State Feedforward Controller

The three-state feedforward controller is added before a system completes the three-state feedback regulation, thereby eliminating the pole which is close to the virtual axis in the closed-loop transfer function. Thus, the system bandwidth can be extended. Let the transfer function of the three-state feedforward controller be(25)Bs=kaps2+kvps+kdp,where kap,kvp, and kdp are the determined controller parameters.In equation (23), the three-state feedforward controller satisfies equation (26) to offset the pole of the three-state feedback control system, which pole is close to the virtual axis:(26)Bs=s2ωc2+2ξcsωc+1.Equation (27) can be obtained by integrating equations (25) and (26).The parameters of the three-state feedback controller can be obtained by substituting the selected parameter values ofξc and ωc into the following equation:(27)kdp=1,kvp=2ξcωc,kap=1ωc2.


## 4. Experiment

When the construction vehicle drives on random roads, a test system collects the vibration information data of the vehicle and uses the attitude calculation algorithm in this study to obtain the vehicle vibration information. The experimental seat is installed on a 6-DOF vibration representation platform. Furthermore, the counterweight is fixed to the seat, and the vibration representation test bed is controlled by using real vehicle acquisition data, as depicted in Figure10. Table 3 summarises the main parameters of the electrohydraulic servo system for the vehicle vibration representation test bed. The recorded attitude information after completing the test is compared with the attitude information of the moving platform. Because the measured value is relatively close to the target attitude, the motion curve within a short period is compared when the vibration is relatively intense. The attitude following curve of the system is demonstrated in Figure 11.Figure 10
6-DOF vibration representation test bed of a vehicle.Table 3
Parameters of the electrohydraulic servo system.
Parameter name
Parameter symbol
Value
Unit
Inner diameter of the hydraulic cylinder
D
38
mm
Diameter of the piston rod
d
20
mm
Flow coefficient of the valve port
C
d
0.85
—
Area gradient of the valve port
w
34.6
mm
Density of the hydraulic oil
ρ
855
kg
/
m
3
Supply pressure of the system
P
s
10
MPa
Bulk modulus hydraulic oil
E
y
7
×
10
8
Pa
Equivalent volume of the hydraulic cylinder
V
e
1.317
×
10
−
3
m
3
Total mass converted to the piston
m
t
300
kg
Pressure-flow coefficient
k
ce
3
×
10
−
11
—
Gain of the servo amplifier
k
α
50
mA/VFigure 11
Attitude following curve of the vibration representation platform: (a)X displacement; (b) Y displacement; (c) Z displacement; (d) roll angle; (e) pitch angle; (f) yaw angle.
(a)
(b)
(c)
(d)
(e)
(f)The attitude following error of the vibration representation system is calculated, and the following error of the 6-DOF test platform is analysed. The attitude following error curve of the vibration representation system is exhibited in Figure12.Figure 12
Error curve of the vibration representation system: (a)X error; (b) Y error; (c) Z error; (d) error of rolling angle; (e) error of pitch angle; (f) error of yaw angle.
(a)
(b)
(c)
(d)
(e)
(f)The position and attitude following curve and its error curve indicate the response lag of the vibration representation system. This phenomenon is mainly due to the inertia of the mechanical and hydraulic systems; such inertia causes a certain response delay to the input signal of the test platform. The response lag of the system is less than 10 ms, which satisfies the design requirements. A certain error exists between the position, attitude of the vibration platform, and the target signal. The attitude angle error is less than 0.5°, and the platform translation error is less than 2 mm. The errors are caused by two main reasons as follows: (1) further optimisation of control parameters and (2) assembly gap and error of each branched joint of the parallel platform. However, the errors of experimental results satisfy the design requirements, and it verifies the effectiveness of vibration information acquisition and representation control strategy.


## 5. Conclusions

(1)
To increase accuracy of vehicle attitude measurement, an improved adaptive algorithm of multisensor attitude fusion is proposed. The algorithm uses weighted least squares method and complementary filtering fusion algorithm to fuse the information of accelerometer, magnetometer, and gyroscope and calculate the vehicle attitude. Experiments show that the algorithm can effectively fuse multisensor information and accurately obtain vehicle attitude.
(2)
In order to obtain the vibration displacement of vehicle body, a displacement reconstruction algorithm based on acceleration measurement is proposed. The algorithm converts the acceleration measurement signal to the geodetic coordinate system according to vehicle attitude and then reconstructs the vibration displacement information from the acceleration signal by using the low-frequency attenuation integral method. The algorithm can effectively remove the trend term in the integration process, and the integration results are more accurate.
(3)
A three-state controller is designed for the six-degree-of-freedom vibration test bed. The damping ratio of system is improved by three-state feedback, and the dynamic characteristics of system are improved. Then, the dominant poles of the closed-loop system are cancelled by three-state feedforward, which further expands the frequency range of the vibration test bed to reproduce road information.
(4)
The collected vibration signal of the test carrier is used as the excitation signal to test the vibration reproduction system. The experimental results show that the platform attitude angle error is less than 0.5 degrees and the displacement error is less than 2 mm. The system can meet the design requirements.



---

*Source: 9808604-2019-03-24.xml*