CN114932909A - Slope estimation method for realizing acceleration correction based on complementary filtering - Google Patents

Abstract

The invention provides a slope estimation method for realizing acceleration correction based on complementary filtering, which comprises the steps of signal acquisition and preprocessing, initial slope angle calculation, quaternion introduction, quaternion initial value updating, acceleration normalization, complementary filtering acceleration correction and the like, and finally the slope of automobile running is obtained through calculation; the method is suitable for estimating the gradient on a straight road in the straight driving process.

Description

Slope estimation method for realizing acceleration correction based on complementary filtering

Technical Field

The invention relates to a slope estimation method, in particular to a slope estimation method for realizing acceleration correction based on complementary filtering.

Background

The environmental perception technology is one of four core technologies for intelligent driving, and the development of modern automobiles is greatly promoted. Under the road conditions that the automobile is difficult to drive, the information of the road surface gradient is quickly and accurately estimated, and great help is provided in the aspects of the stability, the safety, the controllability and the like of the automobile. For example, when the heavy transport vehicle runs on a mountain road, the road surface gradient change is accurately obtained, the vehicle is guided to automatically shift, the safety can be guaranteed, the fuel is saved, and the driving experience of a driver can be improved. How to quickly and accurately estimate the slope of the automobile is always a difficult problem in the field of intelligent driving of the automobile.

At present, the common slope analysis can be mainly divided into two categories, namely constructing a state observer based on an automobile longitudinal dynamic model and analyzing time-frequency signals based on sensor signals. The method based on the automobile longitudinal dynamics model is characterized in that various current semaphores are obtained through an automobile CAN bus, and the longitudinal gradient of a road is estimated by adopting an automobile longitudinal dynamics balance equation, a model parameter identification method, a recursive least square method, a Kalman filtering method and the like. However, many signals need to be acquired for realizing the method, key parameters need to be accurately given, the acquired vehicle speed and the vehicle longitudinal acceleration are often superimposed with road information and interference noise, and the parameters such as the vehicle speed and the like are difficult to estimate for a newly-emerging four-wheel-drive electric vehicle due to the fact that the rotating speed of a driven wheel is not used as a reference.

Based on a sensor signal processing method, a learner provides a laser radar method for sensing the gradient of a vehicle as early as 2009, and through the improvement of the learner for several years, the scheme still has the defects of poor road boundary model, excessive sensors, signal redundancy and the like. At present, most scholars select electronic sensors such as an acceleration sensor, a gyroscope, a GPS (global positioning system) and a gas pressure sensor to acquire information outside an automobile, and estimate the road gradient by a multi-information fusion method. On one hand, the change of the acceleration serving as a signal for gradient analysis can reflect the real motion state of the automobile by collecting the acceleration in real time, and make the adjustment most suitable for the acceleration change, on the other hand, the method does not relate to an automobile dynamic model, reduces the number of signals, and the precision of an estimation result completely depends on the filtering and fusion processing mode of the signals, so that the algorithm is simpler, more sensitive and more accurate. The learner also establishes a comprehensive automobile dynamic model with better adaptability, joint estimation is carried out by adopting a forgetting least square method and an extended Kalman filtering method, but different dynamic coefficients are required to be substituted for calculation under relatively complicated and different working conditions, only the approximate range of Kalman gain can be obtained, and certain errors still exist.

Disclosure of Invention

In order to solve the technical problem, the invention provides a slope estimation method for realizing acceleration correction based on complementary filtering, wherein the error of a gyroscope is compensated by long-term integral of an accelerometer, the driving attitude of an automobile is described by a quaternion, a quaternion rotation transformation matrix is constructed, the error between an automobile body coordinate system and an acceleration coordinate system is calculated by a complementary filtering method, the original gyroscope value is compensated by a PID (proportion integration differentiation) controller, and finally the slope of the automobile is accurately estimated in real time. The method specifically comprises the following steps:

signal acquisition and preprocessing:

collecting signals of a gyroscope and a three-axis accelerometer;

preprocessing data values of a gyroscope and a triaxial accelerometer:

(1) eliminating errors existing in the collected data equipment:

in order to reduce errors, the angular acceleration measured by the gyroscope immediately after power-on needs to be measured for multiple times, and the average value is taken as the estimated zero drift value omega ₀ ：

(2) Eliminating noise errors present when collecting data:

eliminating interference of high frequency noise by using first order low pass filter a _k For the filtered acceleration signal k times, a _k-1 For the last filtered acceleration signal, a _raw For the originally acquired acceleration signal, α is the filter coefficient:

a _k ＝αa _raw +(1-a _k-1 α) (2)

respectively acquiring the initial acceleration a of the automobile in the longitudinal direction, the transverse direction and the vertical direction _raw Filtering to obtain initial triaxial acceleration of the accelerometer after filteringRespectively using a _x 、a _y 、a _z And (3) representing longitudinal, transverse and vertical acceleration signal values obtained by the accelerometer, and calculating the local gravity acceleration g through multiple measurements:

after first-order low-pass filtering, the components of the local gravity acceleration g in the longitudinal direction, the transverse direction and the vertical direction are respectively recorded as g _x ，g _y ，g _z ；

(II) calculating an initial slope angle:

in order to obtain the initial slope angle between the inclined plane of the vehicle and the horizontal plane, the slope of the vehicle is measured by two methods: on one hand, the angular acceleration collected by the gyroscope subtracts the null shift, and then the short-time integration is carried out to obtain the initial gradient theta of the automobile:

on the other hand, the longitudinal acceleration of the whole vehicle is measured firstly, only the component of the gravity acceleration in the longitudinal direction is left after data processing, the gravity component is divided by the gravity constant, and the gradient of the vehicle can be obtained by solving the inverse trigonometric function, and the specific flow is as follows:

the longitudinal acceleration a of the vehicle can be known by the accelerometer _x And θ represents the gradient at which the vehicle is in the initial state:

from the formula (5), the longitudinal acceleration of the automobile is removed as far as possible, and the processed value is close to the true gradient value as far as possible; in order to reduce larger errors, the slope of the automobile can be more accurately estimated by performing long-term integration on the acceleration signal acquired by the accelerometer;

the method is obtained by taking the first differential of the longitudinal component of the speed of the whole automobile running along the slope;

will be provided with

As much as possible from g _x G 'obtained after removal of' _x The value is basically the real value of g sin theta, the processed value is divided by the local gravity acceleration, and then the arcsine function value is taken, and the obtained value is the slope angle theta of the road surface and the horizontal plane measured by the accelerometer:

since the gyroscope and the accelerometer have obvious errors in different intervals, a complementary filtering method is used for compensating theta.

(III) introducing quaternion:

converting the initial angle collected by the short-time integral of the gyroscope into a quaternion, wherein the quaternion consists of a rotating shaft and an angle rotating around the rotating shaft, and alpha represents the angle rotating around the rotating shaft, and cos (beta) _x )、cos(β _y )、cos(β _z ) Representing the components of the axis of rotation in the x, y, z directions by quaternions q ₀ 、q ₁ 、q ₂ 、q ₃ Describing the car attitude, the following concepts are introduced:

q ₀ ＝cosα/2

q ₁ ＝sin(α/2)cos(β _x )

q ₂ ＝sin(α/2)cos(β _y )

q ₃ ＝sin(α/2)cos(β _z ) (6)

quaternions are introduced as parameters for describing the vehicle body attitude, and the Euler angle is converted into a quaternion formula as follows:

q＝[q ₀ q ₁ q ₂ q ₃ ] ^T

|q| ² ＝q ₀ ² +q ₁ ² +q ₂ ² +q ₃ ² ＝1

wherein the content of the first and second substances,

representing roll angle, theta represents pitch angle (i.e., measured slope angle), psi represents heading angle, and quaternion is converted to euler angle as follows:

the variation relationship between quaternion and euler angle is:

θ＝arcsin(2(q ₀ q ₂ -q ₁ q ₃ ))

C _R for the rotation matrix, the formula for transforming the quaternion and the rotation matrix is as follows:

and (IV) updating the initial value of the quaternion:

let initial quaternion q ₀ ＝1，q ₁ ＝0，q ₂ ＝0，q ₃ ＝0，W _x 、W _y 、W _z Respectively recording the three-axis angular rate acquired by the gyroscope and the angular rate acquired by the gyroscope as gx, gy and gz;

(V) acceleration normalization:

the filtered accelerometer values need to be normalized due to changes in mode length, as measured values of the gravity vector in the body coordinate system, a' _x ,a′ _y ,a′ _z And norm is the length-sum reciprocal of the acceleration mode:

a′ _x ＝a _x *norm

a′ _y ＝a _y *norm

a′ _z ＝a _z *norm (11)

(VI) complementary filtering acceleration correction:

the accelerometer is used for compensating the gyroscope, so that errors can be eliminated to the greatest extent during long-time integration, and the real attitude of the automobile can be acquired as far as possible;

updating the quaternion value q ₀ q ₁ q ₂ q ₃ Substituting into formula (10)In the method, a rotation matrix C under a machine body coordinate system is obtained _R The neutral vector of the geographic coordinate system is taken as [0,0,1 ]] ^T Converting the coordinate system into a coordinate system of the body taking the automobile as the center by using [ vx, vy, vz] ^T Representing the acceleration matrix after coordinate system rotation transformation:

error vector between two coordinate systems

Is prepared from' _x Ay ', az' forming a gravity vector

Vector outer product measured with the body coordinate system:

eliminating errors of the two coordinate systems by using a PID controller, setting Kp, Ki and Kd numerical values, realizing the compensation of the errors of the gyroscope and the accelerometer, and aiming at [ gx, gy and gz] ^T Updating the value of (1):

performing a first-order Rungekutta algorithm of a differential equation on the updated value of the quaternion, and updating the numerical value of the quaternion:

the quaternion value is updated, the modulus of the quaternion is changed, and when the Euler angle is calculated by the quaternion, the quaternion is normalized and q ″, is used ₀ ，q″ ₁ ，q″ ₂ ，q″ ₃ Represent normalized quaternion values:

q″ ₀ ＝q′ ₀ *norm

q″ ₁ ＝q′ ₁ *norm

q″ ₂ ＝q′ ₁ *norm

q″ ₃ ＝q′ ₁ *norm (16)

finally, the gradient of the running of the automobile, namely the pitch angle pitch of the automobile is arcsin (2(q ″) ₀ q″ ₂ -q″ ₁ q″ ₃ ))。

The invention has the beneficial effects that:

the invention provides a method capable of realizing automatic gradient adjustment with less influence of environmental factors, which is characterized in that according to signals collected by a gyroscope and an accelerometer, initial null shift of the gyroscope is eliminated, initial angles collected by short-time integration of the gyroscope are converted into quaternions, and a body coordinate system is established; the acceleration signal acquired by the accelerometer obtains three-axis acceleration through first-order low-pass filtering, high-frequency noise such as noise of an automobile is eliminated, an acceleration vector is obtained after the speed value is normalized, the vector outer product of the acceleration vector and a coordinate system of a machine body is a system error, the error is compensated through a PI calculation value, gyroscope data are updated through a first-order Runge Kutta algorithm, the error generated by long-time integration of the gyroscope is compensated through the accelerometer, and finally a gradient estimation value is calculated. According to the invention, by means of the change of the longitudinal acceleration, the error is reduced and the slope value is estimated through a series of algorithms, and the calculation of an empirical value is not required, so that the accuracy is improved, and the result of the slope estimation result is more reliable than that of the traditional slope meter; in the process, an automobile dynamics model is not involved, so that the precision of an estimation result completely depends on the filtering and fusion processing mode of signals, and meanwhile, the number of the acquired signals is reduced, and the calculation is faster. The method uses a complementary filtering method to replace a Kalman filtering method, and solves the difficulty that the error statistical characteristic quantity is difficult to give when the Kalman filtering algorithm is directly implemented. The method is suitable for estimating the gradient on a straight road in the straight driving process.

Drawings

FIG. 1 is a schematic overall flow diagram of the present invention;

FIG. 2 is a schematic view of an approximate driving condition of the vehicle of the present invention;

FIG. 3 is a schematic diagram of the roll angle, pitch angle, and cruise angle of an automobile;

Detailed Description

The invention provides a slope estimation method for realizing acceleration correction based on complementary filtering, which comprises the following steps of:

signal acquisition and preprocessing:

collecting signals of a gyroscope and a three-axis accelerometer;

preprocessing data:

(1) eliminating errors existing in the collected data equipment:

because the Q points of each stage of the direct coupling amplifying type circuit are mutually related, correlated and mutually influenced, even a small change of the first stage can generate a large error of the output stage due to the amplifying action of each stage in operation; when the input is short-circuited, the output can slowly change along with time, and the phenomenon generated in this way is called zero drift;in order to reduce errors, the angular acceleration measured by the gyroscope immediately after power-on needs to be measured for multiple times, and the average value is taken as the estimated zero drift value omega ₀ ：

(2) Eliminating noise errors present when collecting data:

because the component of the gravity acceleration on the ramp and the acceleration of the automobile are low-frequency signals, the noise acceleration generated by the vibration of the automobile in the driving process belongs to high-frequency signals, and the interference of high-frequency noise is eliminated by using a first-order low-pass filter, a _k For the filtered acceleration signal k times, a _k-1 For the last filtered acceleration signal, a _raw For the originally acquired acceleration signal, α is the filter coefficient:

a _k ＝αa _raw +(1-α)a _k-1 (2)

respectively acquiring the initial acquisition acceleration a in the longitudinal direction, the transverse direction and the vertical direction of the automobile _raw Filtering to obtain initial triaxial acceleration of the accelerometer, using a _x 、a _y 、a _z And (3) representing longitudinal, transverse and vertical acceleration signal values obtained by the accelerometer, and calculating the local gravity acceleration g through multiple measurements:

after first-order low-pass filtering, the components of the local gravity acceleration g in the longitudinal direction, the transverse direction and the vertical direction are respectively recorded as g _x ，g _y ，g _z ；

After the two types of errors are eliminated, acceleration correction is carried out through the idea of compensating the gyroscope through long-term integration by means of Kalman filtering, and because Kalman weighting factors are difficult to give, the method for eliminating noise interference by means of complementary filtering simultaneously achieves mutual compensation between short-term integration and long-term integration.

(II) calculating an initial slope angle:

the complex real driving situation is abstracted as shown in the following figure 2, in order to obtain the initial slope angle between the inclined plane where the vehicle is located and the horizontal plane, the slope where the vehicle is located can be measured by two methods, one is that the angular acceleration collected by a gyroscope subtracts zero drift, and then the initial slope theta where the vehicle is located is obtained by short-time integration:

the other method is that the longitudinal acceleration of the whole vehicle is measured, only the component of the gravity acceleration in the longitudinal direction is left after data processing, the gravity component is divided by the gravity constant, and the gradient of the vehicle can be obtained by solving the inverse trigonometric function, and the specific flow is as follows:

the longitudinal acceleration a of the vehicle can be known by the accelerometer _x And θ represents the gradient at which the vehicle is in the initial state:

from the formula (5), the longitudinal acceleration of the automobile is removed as much as possible, the processed value is close to the true gradient value as much as possible, the currently generally adopted method is to receive the automobile speed fed back by the automobile CAN bus to perform differential differentiation, but the method has larger errors when the automobile is started, suddenly changes speed and makes a sharp turn, and is generally suitable for straight roads; in order to reduce larger errors under the above conditions, the slope of the automobile can be more accurately estimated by performing long-term integration on the acceleration signal acquired by the accelerometer;

the speed differential method is obtained by taking the first differential of the longitudinal component of the speed of the whole automobile running along the slope;

will be provided with

As much as possible from g _x G 'obtained after removal of' _x The value is basically the real value of g sin theta, the value obtained after processing is divided by the local gravity acceleration, then the arcsine function value is taken, and the obtained value is the slope angle theta of the road surface and the horizontal plane measured by the accelerometer:

since the gyroscope and the accelerometer have obvious errors in different intervals, a complementary filtering method is used for compensating theta.

(III) introducing quaternion:

converting the initial angle collected by the short-time integral of the gyroscope into a quaternion, wherein the quaternion consists of a rotating shaft and an angle rotating around the rotating shaft, and alpha represents the angle rotating around the rotating shaft, and cos (beta) _x )、cos(β _y )、cos(β _z ) Representing the components of the axis of rotation in the x, y, z directions by quaternions q ₀ 、q ₁ 、q ₂ 、q ₃ Describing the car attitude, the following concepts are introduced:

q ₀ ＝cosα/2

q ₁ ＝sin(α/2)cos(β _x )

q ₂ ＝sin(α/2)cos(β _y )

q ₃ ＝sin(α/2)cos(β _z ) (6)

quaternions are introduced as parameters for describing the vehicle body attitude, and the Euler angle is converted into a quaternion formula as follows:

q＝[q ₀ q ₁ q ₂ q ₃ ] ^T

|q| ² ＝q ₀ ² +q ₁ ² +q ₂ ² +q ₃ ² ＝1

wherein, the first and the second end of the pipe are connected with each other,

represents the roll angle, theta represents the pitch angle (i.e., the measured grade angle), and psi represents the heading angle, as shown in fig. 3;

the quaternion is converted to the euler angle by the equation:

the variation relationship between quaternion and euler angle is:

θ＝arcsin(2(q ₀ q ₂ -q ₁ q ₃ ))

for the automobiles which often run on cities and good roads, the maximum climbing gradient is about 10 degrees; for a truck, the truck sometimes needs to run on a bad road, the maximum climbing gradient should be about 30 percent, namely 16.5 degrees, and the arctan value and the arcsin value of theta output belong to

Can represent all the common angles of the automobile;

C _R for the rotation matrix, the formula of quaternion and rotation matrix transformation:

and (IV) updating the initial value of the quaternion:

let initial quaternion q ₀ ＝1，q ₁ ＝0，q ₂ ＝0，q ₃ ＝0，W _x 、W _y 、W _z Respectively recording the three-axis angular rate acquired by the gyroscope and the angular rate acquired by the gyroscope as gx, gy and gz;

(V) acceleration normalization:

the filtered accelerometer values need to be normalized due to changes in mode length, as measured values of the gravity vector in the body coordinate system, a' _x ,a′ _y ,a′ _z The normalized longitudinal, transverse and vertical accelerations are shown, norm is the sum reciprocal of the acceleration modulus length:

a′ _x ＝a _x *norm

a′ _y ＝a _y *norm

a′ _z ＝a _z *norm (11)

(VI) complementary filtering acceleration correction:

writing an MATLAB program to realize a complementary filtering method, wherein the core of the complementary filtering method is that an accelerometer is used for compensating a gyroscope, so that errors can be eliminated to the greatest extent during long-time integration, and the real attitude of the automobile can be obtained as far as possible.

Updating the quaternion value q ₀ q ₁ q ₂ q ₃ Substituting into formula (10) to obtain rotation matrix in the coordinate system of the body, and taking the neutral vector of the geographic coordinate system as [0,0, 1%] ^T Converting the coordinate system into a body coordinate system with the automobile as the center by using [ vx, vy, vz] ^T Representing the acceleration matrix after coordinate system rotation transformation:

error vector between two coordinate systems

Is prepared from' _x Ay ', az' of the gravity vector

Vector outer product measured with the body coordinate system:

the updating of the quaternion is data acquired by the gyroscope, the correction of errors of a long-time integral area of the gyroscope by the accelerometer is realized through a formula, errors of two coordinate systems are eliminated by a PID (proportion integration differentiation) controller, and given Kp, Ki and Kd numerical values, the PID controller not only has the proportional action, but also has the residual error elimination capability of the integral action and also has the advance control function of the differential action; realize the compensation of the errors of the gyroscope and the accelerometer, and pair [ gx, gy, gz] ^T Updating the value of (c):

and performing a first-order Runge Kutta algorithm of a differential equation on the updated value of the quaternion, and updating the numerical value of the quaternion:

the quaternion value is updated, the modulus of the quaternion is changed, and when the Euler angle is calculated by the quaternion, the quaternion is normalized and q ″, is used ₀ ，q″ ₁ ，q″ ₂ ，q″ ₃ Represent normalized quaternion values:

q″ ₀ ＝q′ ₀ *norm

q″ ₁ ＝q′ ₁ *norm

q″ ₂ ＝q′ ₁ *norm

q″ ₃ ＝q′ ₁ *norm (16)

finally, the gradient of the running of the automobile, namely the pitch angle pitch of the automobile is arcsin (2(q ″) ₀ q″ ₂ -q″ ₁ q″ ₃ ))。

Test and predicted results

In order to reduce the cost, improve the success rate and ensure the accuracy of a slope estimation result, a mathematical model is built to test the slope estimation method, a Simulink tool in MATLAB is used for modeling a dynamic system of vehicle climbing, and after a plurality of tests, if the slope estimation is less than 1%, a slope meter CAN be connected with a CAN bus to carry out the next real vehicle test.

In a simulation experiment in a Simulink module of Matlab, a step signal is input to simulate the change of the gradient, and because noise exists in the actual running working condition of an automobile, signals acquired under the condition that the noise is added to simulate the actual working condition need to be input in an input value at the same time.

Claims (1)

1. A slope estimation method for realizing acceleration correction based on complementary filtering is characterized in that: the method comprises the following steps:

signal acquisition and preprocessing:

collecting signals of a gyroscope and a three-axis accelerometer;

preprocessing data values of a gyroscope and a triaxial accelerometer:

(1) eliminating errors in the collected data equipment:

measuring the angular acceleration measured by the gyroscope immediately after power-on for multiple times, and taking the average value as the estimated value omega of zero drift ₀ ：

(2) Eliminating noise errors present when collecting data:

eliminating interference of high frequency noise by using first order low pass filter _k For the filtered acceleration signal k times, a _k-1 For the last filtered acceleration signal, a _raw For the originally acquired acceleration signal, α is the filter coefficient:

a _k ＝αa _raw +(1-a _k-1 α) (2)

respectively acquiring the initial acceleration a of the automobile in the longitudinal direction, the transverse direction and the vertical direction _raw Filtering to obtain initial triaxial acceleration of the accelerometer, using a _x 、a _y 、a _z Representing longitudinal, transverse and vertical acceleration signal values obtained by the accelerometer, and calculating the local gravity acceleration g through multiple measurements:

after first-order low-pass filtering, the components of the local gravity acceleration g in the longitudinal direction, the transverse direction and the vertical direction are respectively recorded as g _x ，g _y ，g _z ；

(II) calculating an initial slope angle:

the initial slope angle of the automobile is measured by two methods:

on one hand, the angular acceleration collected by the gyroscope subtracts the zero drift, and then the short-time integration is carried out to obtain the initial gradient theta of the automobile:

on the other hand, the longitudinal acceleration of the whole vehicle is firstly measured, only the component of the gravity acceleration in the longitudinal direction is left, and the longitudinal acceleration a of the vehicle can be known by the accelerometer _x And θ represents the gradient at which the vehicle is in the initial state:

the method is obtained by taking the first differential of the longitudinal component of the speed of the whole automobile running along the slope;

will be provided with

From g _x Is removed to obtain the true value g 'of g sin theta' _x Dividing the processed numerical value by the local gravity acceleration, and then taking the arcsine function value of the processed numerical value, wherein the obtained numerical value is the slope angle theta of the road surface and the horizontal plane measured by the accelerometer:

(III) introducing quaternion:

converting the initial angle collected by the short-time integral of the gyroscope into a quaternion, wherein the quaternion consists of a rotating shaft and an angle rotating around the rotating shaft, and alpha represents the angle rotating around the rotating shaft, and cos (beta) _x )、cos(β _y )、cos(β _z ) Representing the components of the axis of rotation in the x, y, z directions by quaternions q ₀ 、q ₁ 、q ₂ 、q ₃ Describing the car attitude, the following concepts are introduced:

q ₀ ＝cosα/2

q ₁ ＝sin(α/2)cos(β _x )

q ₂ ＝sin(α/2)cos(β _y )

q ₃ ＝sin(α/2)cos(β _z ) (6)

quaternions are introduced as parameters for describing the vehicle body attitude, and the Euler angle is converted into a quaternion formula as follows:

q＝[q ₀ q ₁ q ₂ q ₃ ] ^T

|q| ² ＝q ₀ ² +q ₁ ² +q ₂ ² +q ₃ ² ＝1

wherein the content of the first and second substances,

representing roll angle, theta represents pitch angle, i.e. measured pitch angle, psi represents heading angle, and quaternion is converted to euler angle as follows:

the variation relationship between quaternion and euler angle is:

θ＝arcsin(2(q ₀ q ₂ -q ₁ q ₃ ))

C _R for the rotation matrix, the quaternion and the rotation matrix are expressed by the following formula:

and (IV) updating initial values of quaternions:

let initial quaternion q ₀ ＝1，q ₁ ＝0，q ₂ ＝0，q ₃ ＝0，W _x 、W _y 、W _z Respectively recording the three-axis angular rate acquired by the gyroscope and the angular rate acquired by the gyroscope as gx, gy and gz;

(V) acceleration normalization:

the filtered accelerometer value is normalized and is used as the measured value a 'of the gravity vector under the body coordinate system' _x ,a′ _y ,a′ _z The normalized longitudinal, transverse and vertical accelerations are shown, norm is the sum reciprocal of the acceleration modulus length:

a′ _x ＝a _x *norm

a′ _y ＝a _y *norm

a′ _z ＝a _z *norm (11)

(VI) complementary filtering acceleration correction:

updating the quaternion value q ₀ q ₁ q ₂ q ₃ Substituting the obtained value into the formula (10) to obtain a rotation matrix C under the coordinate system of the body _R The neutral vector of the geographic coordinate system is taken as [0,0,1 ]] ^T Converting the coordinate system into a coordinate system of the body taking the automobile as the center by using [ vx, vy, vz] ^T Representing the acceleration matrix after coordinate system rotation transformation:

error vector between two coordinate systems

Is prepared from' _x Ay ', az' forming a gravity vector

Vector outer product measured with the body coordinate system:

eliminating errors of the two coordinate systems by using a PID controller, setting Kp, Ki and Kd numerical values, realizing the compensation of the errors of the gyroscope and the accelerometer, and aiming at [ gx, gy and gz] ^T Updating the value of (1):

and performing a first-order Runge Kutta algorithm of a differential equation on the updated value of the quaternion, and updating the numerical value of the quaternion:

when calculating Euler angle from quaternion, the updated quaternion is normalized and used as q ″) ₀ ，q″ ₁ ，q″ ₂ ，q″ ₃ Represent normalized quaternion values:

q″ ₀ ＝q′ ₀ *norm

q″ ₁ ＝q′ ₁ *norm

q″ ₂ ＝q′ ₁ *norm

q″ ₃ ＝q′ ₁ *norm (16)

finally, the gradient of the running of the automobile, namely the pitch angle pitch of the automobile is arcsin (2(q ″) ₀ q″ ₂ -q″ ₁ q″ ₃ ))。

Images