CN109883451A - An adaptive gain complementary filtering method and system for pedestrian orientation estimation - Google Patents

Abstract

The present disclosure provides an adaptive gain complementary filtering method and system for pedestrian orientation estimation, including: data acquisition; gyroscope-based orientation estimation; using Gauss-Newton optimization method to calculate the orientation of accelerometer and magnetometer measurement values; The time-varying parameters of the adaptive filter are set as weights in each direction, and the gain is adaptively adjusted according to the convergence rate of the low-pass frequency based on the Gauss-Newton optimization algorithm and the divergence rate of the high-pass frequency based on the gyroscope. The final orientation estimate of the gyroscope. The convergence rate from the Gauss-Newton optimization algorithm and the divergence rate from the gyroscope are effectively complemented. It not only has the advantage of one iteration calculation, but also has a small calculation load, so that the precise direction of the gyroscope measurement error can be obtained in real time. .

Description

An adaptive gain complementary filtering method and system for pedestrian orientation estimation

technical field

The present disclosure relates to the technical field of filtering data processing, and in particular, to an adaptive gain complementary filtering method and system for pedestrian orientation estimation.

Background technique

Recently, the development of human motion capture systems (motion tracking) has been highly stimulated by clinical medicine, where gait analysis is the main application of motion capture. The high-resolution, quantitative data obtained through these systems can be used to better understand the causes of a variety of diseases, leading to effective treatments, such as the rehabilitation of outpatients who have lost motor function or stroke patients. Continuous monitoring of a patient's body movements in a free-living environment can provide more valuable feedback to guide clinical treatment. However, going beyond laboratory settings and obtaining accurate measurements of human activity, especially in free-living settings, is extremely challenging. Currently, commercial human motion capture systems have excellent capture and reconstruction systems, but require external equipment (such as cameras) and cannot provide users with important information in their everyday environment.

Generally, the human body can be modeled as a multi-segment linkage system connected by joints, which is called a kinematic chain. If the orientation relative to each segment can be determined, the overall motion of the human body based on that kinematic chain can be calculated. Therefore, accurate estimation of joint angles plays an important role in human motion analysis. The orientation of each segment can be measured by inertial sensor modules attached to the human body. This sensor module usually consists of a three-axis accelerometer, a three-axis gyroscope, and a three-axis magnetometer, and is compact in size and can be called a wearable inertial sensor.

However, there are two main challenges in estimating orientation using inertial sensors. First, it is a challenge to directly integrate the angular velocity of the three-axis gyroscope; because the inherent physical limitations of inertial sensors can negatively affect the accuracy of orientation estimation, inertial sensors can generate noise while outputting data, and in long-term tracking Quickly cause drift errors to accumulate. The drift error accumulated during long-term tracking hinders its effective application in human motion capture evaluation. Second, as auxiliary sensors for mitigating directional drift: a three-axis accelerometer and a magnetometer, which are used as vertical (Earth gravity) and horizontal (Earth magnetic field) references, respectively. However, accelerometers are sensitive to excess acceleration of the body (in addition to gravity) under dynamic motion conditions, while magnetometer measurements are susceptible to changes in local Earth's magnetic field, such as those caused by electrical appliances or objects made of ferrous materials Changes in the orientation of the Earth's magnetic field, thereby affecting the desired orientation estimate. In addition, the outputs of the accelerometers and magnetometers also contain measurement errors.

SUMMARY OF THE INVENTION

In order to solve the deficiencies of the prior art, the embodiments of the present disclosure provide an adaptive gain complementary filtering method for pedestrian orientation estimation, which effectively combines the convergence rate from the Gauss-Newton optimization algorithm and the divergence rate from the gyroscope. Complementary, it not only has the advantage of one iterative calculation to reduce the computational load, but also can obtain the precise direction of the gyroscope measurement error.

The embodiment of this specification discloses an adaptive gain complementary filtering method for pedestrian orientation estimation, which is realized by the following technical solutions:

include:

Data acquisition: use the inertial sensor integrated with a three-axis accelerometer, a three-axis gyroscope, and a three-axis magnetometer to obtain the three-axis acceleration, angular velocity and magnetic field strength of the pedestrian carrier;

Direction estimation based on gyroscope: The data measured by the three-axis gyroscope is represented by a quaternion, and the derivative of the quaternion is used to describe the direction change rate of the sensor coordinate system relative to the earth coordinate system. Under known initial conditions, The orientation of the sensor coordinate system relative to the earth coordinate system is obtained by integrating the derivative of the quaternion;

Calculate the orientation of the accelerometer and magnetometer measurements using the Gauss-Newton optimization method;

The time-varying parameters of the adaptive filter are set as the weights in each direction, and the gain is adaptively adjusted according to the convergence rate of the low-pass frequency based on the Gauss-Newton optimization algorithm and the divergence rate of the high-pass frequency based on the gyroscope. The final orientation estimate of the gyroscope.

The embodiment of this specification discloses an adaptive gain complementary filtering system for pedestrian orientation estimation, which is realized by the following technical solutions:

include:

The data acquisition unit is configured to: obtain the three-axis acceleration, angular velocity and magnetic field strength of the pedestrian carrier by using an inertial sensor integrating a three-axis accelerometer, a three-axis gyroscope, and a three-axis magnetometer;

The gyroscope-based direction estimation unit is configured to: represent the data measured by the three-axis gyroscope with a quaternion, and use the derivative of the quaternion to describe the direction change rate of the sensor coordinate system relative to the earth coordinate system. Under the known initial conditions, the direction of the sensor coordinate system relative to the earth coordinate system is obtained by integrating the derivative of the quaternion;

an orientation calculation unit for the measured values of the accelerometer and the magnetometer, configured to: use a Gauss-Newton optimization method to calculate the orientation of the measured values of the accelerometer and the magnetometer;

The final direction estimation unit of the gyroscope is configured as: the time-varying parameters of the adaptive filter are set as the weights of each direction, and the gain is based on the convergence rate of the low-pass frequency based on the Gauss-Newton optimization algorithm and the gyroscope-based The divergence rate of the high-pass frequency is adaptively adjusted to obtain the final orientation estimate of the gyroscope.

Embodiments of this specification disclose an adaptive gain complementary filter for pedestrian orientation estimation, the filter being configured to perform the following:

Direction estimation based on gyroscope: The data measured by the three-axis gyroscope is represented by a quaternion, and the derivative of the quaternion is used to describe the direction change rate of the sensor coordinate system relative to the earth coordinate system. Under known initial conditions, Obtain the orientation of the sensor coordinate system relative to the earth coordinate system by integrating the derivative of the quaternion;

Calculate the orientation of the accelerometer and magnetometer measurements using the Gauss-Newton optimization method;

The time-varying parameters of the adaptive filter are set as the weights in each direction, and the gain is adaptively adjusted according to the convergence rate of the low-pass frequency based on the Gauss-Newton optimization algorithm and the divergence rate of the high-pass frequency based on the gyroscope. The final orientation estimate of the gyroscope.

Compared with the prior art, the beneficial effects of the present disclosure are:

The technical solution of the present disclosure effectively complements the convergence rate from the Gauss-Newton optimization algorithm and the divergence rate from the gyroscope. It not only has the advantage of one-time iterative calculation, but also has a small calculation load, so that the gyroscope can be obtained in real time. Precise direction of measurement error.

Description of drawings

The accompanying drawings that constitute a part of the present disclosure are used to provide further understanding of the present disclosure, and the exemplary embodiments of the present disclosure and their descriptions are used to explain the present disclosure and do not constitute an improper limitation of the present disclosure.

FIG. 1 is a main architectural diagram of an adaptive gain complementary filter algorithm of a good example of the present disclosure;

FIG. 2 is a conversion relationship between the sensor coordinate system and the earth coordinate system of a good example of the present disclosure. In this patent, the sensor coordinate system is recorded as s, and the earth coordinate system is recorded as e;

FIG. 3 is a specific formula block diagram of the adaptive gain complementary filter algorithm of a good example of the present disclosure, which is the specific formula form of FIG. 1;

FIG. 4 shows the magnetic interference of three axes when the magnetic interference is included in a good example of the present disclosure. At the moment of swinging the magnet, the magnetic interference of the three axes is obvious;

5 is a comparison of the direction estimation of the adaptive gain complementary filter algorithm proposed by the present disclosure and the Medgwick method in the case of magnetic interference;

FIG. 6 is a change value of acceleration data under a sudden and rapid movement of a good example of the present disclosure;

FIG. 7 is the direction estimation under sudden and fast motion of a good example of the present disclosure;

FIG. 8 is an estimation result of gyroscope x, y and z axis deviations according to a good example of the present disclosure.

Detailed ways

It should be noted that the following detailed description is exemplary and intended to provide further explanation of the present disclosure. Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this disclosure belongs.

It should be noted that the terminology used herein is for the purpose of describing specific embodiments only, and is not intended to limit the exemplary embodiments according to the present disclosure. As used herein, unless the context clearly dictates otherwise, the singular is intended to include the plural as well, furthermore, it is to be understood that when the terms "comprising" and/or "including" are used in this specification, it indicates that There are features, steps, operations, devices, components and/or combinations thereof.

Terminology explanation part: Gauss-Newton optimization algorithm (Gauss-Newton optimization algorithm, GNA).

Example 1

This embodiment discloses an adaptive gain complementary filtering method for pedestrian orientation estimation, which can 1) accurately estimate the measurement error of the gyroscope to correct the instantaneous measurement value of the gyroscope; 2) deal with the magnetic field distortion; 3) reduce the calculation load and reduce sensor error accumulation.

In this example, the adaptive gain complementary filter algorithm is based on the convergence rate and gyroscope divergence rate of the Gauss-Newton optimization algorithm (GNA), and the key technologies mainly include: (1) data acquisition; (2) Quaternion representation; (3) Gyro integration; (4) Vector observation; (5) Complementary filter; (6) Compensation scheme; (7) Filter gain.

In Figure 1, the adaptive gain complementary filtering method is mainly composed of four parts: 1) Gyro integration 2) Vector observation 3) Complementary filter 4) Compensation scheme; to correct the sensor, because the sensor integrates a three-axis accelerometer , the three-axis magnetometer and the three-axis gyroscope have been calibrated when the sensor leaves the factory, so there is no need to recalibrate. The first is gyro integration, the angular velocity output from the gyroscope is integrated once, and the output is used as the input of 3) the complementary filter; the second is vector observation, the acceleration output from the accelerometer and the magnetic force output from the magnetometer are first 4) The intensity is used as the input of the compensation scheme, and then the measurement vector is selected, and then the output result is used as the input of the Gauss-Newton method, and the output of the Gauss-Newton method is used as another input of 3) the complementary filter, and finally from the complementary filter The optimal orientation estimate at the current moment is output in the device.

In this example, regarding the data acquisition part, the LPMS-B2 inertial sensor is used for data acquisition, which integrates a three-axis accelerometer, a three-axis gyroscope, and a three-axis magnetometer. The value range of the Euler angle: roll angle: ( ±180°), pitch angle: (±90°), yaw angle (±180°), connected with the host computer software LpmsContro through Bluetooth, the acceleration, angular velocity and magnetic field strength of the three axes can be obtained in real time, and the carrier can be calculated in real time The attitude direction, linear acceleration and other information. The ultimate purpose of the measured data is to analyze and process the data to estimate the orientation of pedestrians. The data collection frequency of this experiment is 50Hz. Real-time acceleration, angular velocity and magnetic force data are obtained through LpmsContro software.

In one embodiment, inertial sensors can be placed on a person's arm as desired.

In this embodiment, regarding the quaternion representation, two coordinate systems are used in this embodiment: the carrier coordinate system (denoted as s) and the earth coordinate system (denoted as e). Convert the coordinates of the carrier coordinate system to the earth coordinate system, so you need to know the conversion relationship between the two coordinate systems. When the direction of the coordinate system of the carrier is specified relative to the absolute coordinate system, the direction of the carrier in space can be determined. The absolute coordinate system is usually called the earth coordinate system e (the x _e axis points to the local magnetic north and the z _e axis points to the opposite direction of gravity) ,as shown in picture 2. For the convenience of the following description, the names of each variable are listed here:

s: the lower sensor coordinate system fixed to the sensor;

e: Earth coordinate system;

g: gravity vector;

^s a: the measured value of the acceleration in the sensor coordinate system;

^s ω: the measured value of the angular velocity in the sensor coordinate system;

^s m: the measured value of the magnetic force in the sensor coordinate system;

At time t, the direction from the sensor coordinate system s to the earth coordinate system e based on the quaternion;

At time t, the orientation estimate derived from the gyroscope data;

orientation estimates from the accelerometer and magnetometer at time t;

The direction estimate obtained after complementary filtering and fusion at time t.

In order to conveniently represent the rotation between these two coordinate systems, three commonly used rotation representations are: Euler angle, rotation matrix and quaternion. Although the Euler angle method has fewer parameters, it has poles, while the rotation matrix method has more parameters and is less computationally efficient. Therefore, the quaternion method is used in this embodiment to represent the rotation between the two coordinate systems.

Specifically, a quaternion can be regarded as a vector with four components, which can be composed of a scalar s and an ordinary vector u, and can be calculated by the quaternion about the unit vector v=[v _x , v _y , v _z ] Rotation relative to angle θ. If the unit vector v is defined in the s coordinate system, the corresponding position in the e coordinate system can be obtained by rotating θ around the unit vector v in the s coordinate system, so the relative relationship between the s coordinate system and the e coordinate system Orientation can use unit quaternions express.

As mentioned above, the coordinate system e can be obtained by rotating θ around the unit vector ^s v under the sensor coordinate system s (the superscript s indicates that this vector is defined in the coordinate system s), and the direction of the coordinate system s relative to the coordinate system e can be Use unit quaternions express. So this unit vector is defined in the e coordinate system, and the corresponding relationship is as formula (3)

Yes the conjugate of , which is actually equal to therefore,

In this embodiment, the direction estimation based on the gyroscope, here the estimation based on the direction of the gyroscope is obtained according to formulas (5), (6), (7). In the initial system, we default to time t=0, the earth coordinate system and the sensor coordinate system are coincident, the earth coordinate system is fixed, and the sensor coordinate system changes, and in the actual calculation, the sensor coordinate system needs to be converted. Go to the earth coordinate system and perform the solution.

In the sensor coordinate system ^s , the data measured by the three-axis gyroscope is represented by ^s ω, which can be redefined as a row vector with 4 elements: ^s ω = [0 ω _x ω _y ω _z ], using Quaternion representation. Derivatives of Quaternions It is used to describe the direction change rate of the sensor coordinate system relative to the earth coordinate system, which can be calculated using formula (5):

where Ω( ^s ω) is the obliquely symmetric matrix of the quaternion ^s ω=[0 ω _x ω _y ω _z ],

Therefore, under known initial conditions Under the premise that the direction of the sensor coordinate system relative to the earth coordinate system at time t+Δt can be Points are obtained as listed in Equation (7).

where Δt is the sampling time, The orientation estimate derived from the gyroscope data for time t. Here, the data measured by the three-axis gyroscope is represented by a quaternion, and the derivative of the quaternion is used to describe the direction change rate of the sensor coordinate system relative to the earth coordinate system. The integration of the derivative of , obtains the direction of the sensor coordinate system relative to the earth coordinate system, and the specific solution is as formula (7).

In this example, the Gauss-Newton method is applied to vector observations (three-axis acceleration data and magnetic force data measured by accelerometers and magnetometers), since accelerometers and magnetometers can measure absolute directions relative to the earth, it is possible to The direction of its measurements is calculated using a method called fast convergence, which is known as the Gauss-Newton optimization method. their normalized reference orientations in Earth coordinates (accelerometer ^ez _a = [0 0 0 1], magnetometer And the initial moment ^s m _t=0 /|| ^s m _t=0 || can be adaptively changed according to the current measurement value of the magnetometer and the optimal value at the previous moment. Group 1.2 in Figure 3, when magnetic interference occurs, its normalized measurement can be derived from (8):

The Gauss-Newton method is modeled by the following steps to calculate the unit quaternion

Look for:

minimize:

in:

To meet the conditions:

where f is the value function, which is defined as the square of the error function ε = [ε _a ε _m ] ^T , and ε consists of the acceleration error ε _a and the magnetometer error ε _m . By multiplying ^ez _a , ^ez _m,t , ^s A _t , ^s M _t and quaternion as previously defined:

Taking into account the naturally existing dynamic motion and magnetic field interference phenomena, as well as the inherent error of the sensor, the error factors of accelerometers and gyroscopes can be expressed in terms of ρ _a and ρ _m . The objective function of error can be rewritten as

2) Fast Gauss-Newton method

This patent proposes a new fast Gauss-Newton method, which requires only one iterative operation. According to the above formulation problem, the traditional Gauss-Newton method includes the following optimization steps:

where k is the number of iterations and J(k) is the Jacobian determinant of _∈ , which can be calculated by formula (10). A 6×4 constant matrix represented by K, ρ _a consists of the first three rows of the matrix, and ρ _m consists of the remaining rows. The operator .* represents the dot product of a matrix. Equation (9) describes the known initial value Using the Gauss-Newton method for multiple iterations under the premise of A direction estimation result of . Equation (9) can be further optimized as Equation (11).

In fact, it is the last optimization direction at time t+Δt, which is expressed as (The subscript a represents the direction estimated by the accelerometer and magnetometer), refers to the direction estimated at the previous time t, and is denoted as (The subscript f denotes the orientation estimate after complementary filter fusion), _λk will be changed to the optimal value at each iteration. The rate of convergence controlled by λt _+Δt shown in (12) is equal to or greater than the rate of change of direction, so it is acceptable to compute one iteration per time period. The optimal value of λ _t+Δt guarantees that the rate of convergence, which is subject to the rate of change of orientation calculated from the gyroscope measurement data , thus avoiding over-regulation due to unnecessarily large step sizes. Therefore, formula (11) can be simply written as formula (12) with only one iteration calculation, and the optimal value of λ _t+Δt can be calculated by formula (13).

where α is the increase in λ considering the presence of noise in the accelerometer and magnetometer.

In this embodiment, regarding the adaptive gain complementary filter, the complementary filter is mainly designed for two noise sources with complementary spectral characteristics. In this embodiment, the acceleration and magnetic force signals processed by the low-pass filter and the high-pass filter are processed The gyroscope signals are combined to obtain the final optimal rate. Usually, the gain of the traditional complementary filter used to measure noise is a constant, the adaptive filter proposed in this example combines the advantages of complementary filter and Kalman filter, time-varying parameters and 1-k _t are set as weights in each direction, and robust results can be obtained. Optimizing the time-varying parameters and gain can improve the convergence speed and stability of the adaptive filter when estimating the direction, which is represented by Equation (14), the gain k _t is based on the low-pass frequency based on the Gauss-Newton optimization algorithm The convergence rate of and the divergence rate β of the high-pass frequency based on the gyroscope is adaptively adjusted. As shown in group 3 in Figure 3.

Therefore, the final orientation estimate can be obtained:

in, For the estimated rate of orientation, the rate of orientation change from the gyroscope can be used Magnitude of gyroscope measurement error (usually zero mean error) β and rate of change of orientation error from accelerometer and magnetometer express.

In one example, with respect to the compensation scheme:

1) Zero offset drift compensation of gyroscope: due to long-term tracking, the inertial sensor will generate accumulated drift error. Indicates the zero offset drift compensation of the gyroscope. Angular rate error ^s ω _e , angular rate deviation ^s ω _b , angular velocity ^s ω _c After each gyroscope compensation, the normalized direction is obtained As shown in Figure 3 Group 2.

Among them, ζ represents the convergence rate of the gyroscope measurement error after eliminating the non-zero mean value, and it can be concluded that it is approximate to β through several experiments.

2) Accelerometer and magnetometer compensation: In order to select the most reliable vector in the sensor measurement value as the input of the filter, the measurement value of the accelerometer and magnetometer needs to be compensated. For example, neither the ^s A _t measurement in the case of fast motion nor the ^s M _t measurement in the case of temporary magnetic disturbances can be used as reliable data information. Therefore, in these cases, ^sω is the more reliable measurement information.

Through these concepts above, the input vectors ^s A (gravity related) and ^s M (magnetic related) can be compensated in the Gauss-Newton optimization algorithm by the following steps (see Figure 3 Group 1.1): (a) Reference vector ^e z _m At time t, it can adaptively change in the earth's magnetic field. (b) The reference vector is selected as ^ez _a or ^ez _a,t ; (c) The gain of the filter is adaptively adjusted, which can not only compensate for the situation of fast motion and magnetic interference, but also can compensate for the accumulated direction caused by the integration of the gyroscope. error is compensated.

In an embodiment, with respect to filter gain:

The filter gain β accounts for all zero-mean gyroscope measurement errors, the angular rate error ^s ω _e , and the angular rate deviation ^s ω _b mentioned in the zero-offset drift compensation of the gyroscope in the previous step. These errors typically come from sensor noise, signal aliasing, quantization errors, calibration errors, sensor misalignment, and frequency response characteristics. The filter gain ζ represents the rate of convergence, especially the removal of non-zero mean gyroscope measurement errors, and is also represented as the magnitude of the quaternion derivative. Both of these errors represent the bias of the gyroscope. This example uses and to define β and ζ, is an estimate of the mean error of gyroscope measurements expressed as three axes (x, y, z), is the estimated rate of bias drift for a three-axis gyroscope. and The value of can be determined by several experiments. According to the relationship described by Equation (5), β and ζ can be determined by Equation (18) and Equation (19), respectively, where is an arbitrary unit quaternion.

Figure 4 shows the magnetic interference of three axes with magnetic interference. At the moment of swinging the magnet, the magnetic interference of the three axes is obvious. 5 is a comparison of the direction estimation of the adaptive gain complementary filter algorithm proposed by the technical solution of the present disclosure and the Medgwick method in the case of magnetic interference. The Madgwick method is a relatively mature nine-axis fusion algorithm, and the technical solution of the present disclosure is By comparison with this method. Mainly analyze and compare the angles of yaw angle, pitch angle and roll angle. It can be seen from the figure that the swing of the three angles obtained by our proposed method is almost close to 0 without the magnetic interference of motion. , while the Medgwick method has a more obvious change in angle. Figure 6 shows the change in acceleration data under sudden and rapid motion. Figure 7 shows the direction estimation under sudden and fast motion. Sudden and rapid motion in a magnetically homogeneous environment, compared to the measurement results of the Vicon system (Vicon is a motion capture system with good accuracy and high precision), even in this case, our proposed algorithm provides A fairly accurate estimate of performance. However, with Madgwick's method, the corresponding estimated angles are far from the actual values and drift significantly after returning to the initial state. Figure 8 shows the estimation results of the x, y and z axis deviation of the gyroscope, which is the compensation for the zero offset drift with magnetic interference. It can be seen from these results that even in the magnetic interference, the adaptive gain proposed in this patent The complementary filtering method can also successfully estimate the bias of the gyroscope, while Madgwick's method yields an erroneous estimate.

The present disclosure proposes an adaptive gain complementary filtering method and system for pedestrian azimuth estimation, which solves the problem of drift error accumulation caused by relying on inertial navigation in the long-term tracking process, as well as errors and magnetic field distortion caused by the influence of surrounding magnetic fields question. It not only has the advantage of one-time iterative calculation, reduces the computational load, accurately estimates the gyro measurement error to correct the instantaneous measurement value of the gyro, but also obtains the precise direction of the gyro measurement error. The adaptive gain complementary filtering method is based on the convergence rate of the Gauss-Newton optimization algorithm (GNA) and the divergence rate of the gyroscope, and effectively complements the convergence rate and the divergence rate from the gyroscope. The whole process consists of: gyroscope Integral to estimate the direction; the Gauss-Newton method is applied to the vector observation; the adaptive gain complementary filter; the compensation scheme is composed of four parts. and robust estimation under magnetic distortion conditions. The experimental results verify the effectiveness of the method and show that the method has better localization accuracy than several commonly used localization estimation methods.

Example 2

This embodiment discloses an adaptive gain complementary filtering system for pedestrian orientation estimation, including:

The data acquisition unit is configured to: obtain the three-axis acceleration, angular velocity and magnetic field strength of the pedestrian carrier by using an inertial sensor integrating a three-axis accelerometer, a three-axis gyroscope, and a three-axis magnetometer;

The gyroscope-based direction estimation unit is configured to: represent the data measured by the three-axis gyroscope with a quaternion, and use the derivative of the quaternion to describe the direction change rate of the sensor coordinate system relative to the earth coordinate system. Under the known initial conditions, the direction of the sensor coordinate system relative to the earth coordinate system is obtained by integrating the derivative of the quaternion;

an orientation calculation unit for the measured values of the accelerometer and the magnetometer, configured to: use a Gauss-Newton optimization method to calculate the orientation of the measured values of the accelerometer and the magnetometer;

The final direction estimation unit of the gyroscope is configured as: the time-varying parameters of the adaptive filter are set as the weights of each direction, and the gain is based on the convergence rate of the low-pass frequency based on the Gauss-Newton optimization algorithm and the gyroscope-based The divergence rate of the high-pass frequency is adaptively adjusted to obtain the final orientation estimate of the gyroscope.

This embodiment further includes: a zero-offset drift compensation unit of the gyroscope, because the inertial sensor generates drift error accumulation due to long-term tracking, using Indicates the zero offset drift compensation of the gyroscope.

Accelerometer and Magnetometer Compensation Unit: In order to select the most reliable vector among the sensor measurements as the input to the filter, the accelerometer and magnetometer measurements need to be compensated.

Example three

This embodiment discloses an adaptive gain complementary filter for pedestrian orientation estimation, the filter being configured to perform the following:

Direction estimation based on gyroscope: The data measured by the three-axis gyroscope is represented by a quaternion, and the derivative of the quaternion is used to describe the direction change rate of the sensor coordinate system relative to the earth coordinate system. Under known initial conditions, Obtain the orientation of the sensor coordinate system relative to the earth coordinate system by integrating the derivative of the quaternion;

Calculate the orientation of the accelerometer and magnetometer measurements using the Gauss-Newton optimization method;

The time-varying parameters of the adaptive filter are set as the weights in each direction, and the gain is adaptively adjusted according to the convergence rate of the low-pass frequency based on the Gauss-Newton optimization algorithm and the divergence rate of the high-pass frequency based on the gyroscope. The final orientation estimate of the gyroscope.

Example 4

This embodiment discloses a computer device, including a memory, a processor, and a computer program stored in the memory and running on the processor, wherein the processor implements the following steps when executing the program:

Data acquisition: use the inertial sensor integrated with a three-axis accelerometer, a three-axis gyroscope, and a three-axis magnetometer to obtain the three-axis acceleration, angular velocity and magnetic field strength of the pedestrian carrier;

Direction estimation based on gyroscope: The data measured by the three-axis gyroscope is represented by a quaternion, and the derivative of the quaternion is used to describe the direction change rate of the sensor coordinate system relative to the earth coordinate system. Under known initial conditions, Obtain the orientation of the sensor coordinate system relative to the earth coordinate system by integrating the derivative of the quaternion;

Calculate the orientation of the accelerometer and magnetometer measurements using the Gauss-Newton optimization method;

The time-varying parameters of the adaptive filter are set as the weights in each direction, and the gain is adaptively adjusted according to the convergence rate of the low-pass frequency based on the Gauss-Newton optimization algorithm and the divergence rate of the high-pass frequency based on the gyroscope. The final orientation estimate of the gyroscope.

Also includes:

The zero offset drift compensation of the gyroscope, due to the long-term tracking, the inertial sensor will produce accumulated drift error, this example uses Indicates the zero offset drift compensation of the gyroscope.

Accelerometer and Magnetometer Compensation: In order to select the most reliable vector among the sensor measurements as the input to the filter, the accelerometer and magnetometer measurements need to be compensated.

For the technical content of the relevant steps in this embodiment example, reference may be made to the specific technical content in the first embodiment, which will not be repeated here.

Example 5

This embodiment discloses a computer-readable storage medium on which a computer program is stored, and is characterized in that, when the program is executed by a processor, the following steps are implemented:

Data acquisition: use the inertial sensor integrated with a three-axis accelerometer, a three-axis gyroscope, and a three-axis magnetometer to obtain the three-axis acceleration, angular velocity and magnetic field strength of the pedestrian carrier;

Direction estimation based on gyroscope: The data measured by the three-axis gyroscope is represented by a quaternion, and the derivative of the quaternion is used to describe the direction change rate of the sensor coordinate system relative to the earth coordinate system. Under known initial conditions, Obtain the orientation of the sensor coordinate system relative to the earth coordinate system by integrating the derivative of the quaternion;

Calculate the orientation of the accelerometer and magnetometer measurements using the Gauss-Newton optimization method;

The time-varying parameters of the adaptive filter are set as the weights in each direction, and the gain is adaptively adjusted according to the convergence rate of the low-pass frequency based on the Gauss-Newton optimization algorithm and the divergence rate of the high-pass frequency based on the gyroscope. The final orientation estimate of the gyroscope.

Also includes:

The zero offset drift compensation of the gyroscope, due to the long-term tracking, the inertial sensor will produce accumulated drift error, this example uses Indicates the zero offset drift compensation of the gyroscope.

Accelerometer and Magnetometer Compensation: In order to select the most reliable vector among the sensor measurements as the input to the filter, the accelerometer and magnetometer measurements need to be compensated.

For the technical content of the relevant steps in this embodiment example, reference may be made to the specific technical content in the first embodiment, which will not be repeated here.

The above descriptions are only preferred embodiments of the present disclosure, and are not intended to limit the present disclosure. For those skilled in the art, the present disclosure may have various modifications and changes. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present disclosure shall be included within the protection scope of the present disclosure.

Claims (10)

1. An adaptive gain complementary filtering method for pedestrian orientation estimation is characterized by comprising the following steps:

data acquisition: acquiring the three-axis acceleration, the angular velocity and the magnetic field intensity of the pedestrian carrier by using inertial sensors integrating a three-axis accelerometer, a three-axis gyroscope and a three-axis magnetometer;

gyroscope-based direction estimation: expressing data measured by a three-axis gyroscope by using a quaternion, describing a direction change rate of a sensor coordinate system relative to a terrestrial coordinate system by using a derivative of the quaternion, and acquiring the direction of the sensor coordinate system relative to the terrestrial coordinate system by integrating the derivative of the quaternion under a known initial condition;

calculating the directions of the accelerometer and magnetometer measurements by using a Gauss-Newton optimization method;

the time-varying parameters of the adaptive filter are set as the weight of each direction, and the gain is adaptively adjusted according to the convergence rate of the low-pass frequency based on the Gauss-Newton optimization algorithm and the divergence rate of the high-pass frequency based on the gyroscope, so as to obtain the final direction estimation of the gyroscope.

2. The adaptive gain compensation filtering method for pedestrian orientation estimation according to claim 1, wherein angular velocity output from a gyroscope of the inertial sensor is integrated once, and the result is output as an input to the adaptive filter.

3. The adaptive gain complementary filtering method for pedestrian orientation estimation according to claim 1, wherein the absolute orientation of the accelerometer and magnetometer relative to the earth is calculated using a Gaussian-Newton optimization method,

wherein,in fact, the last optimization direction at time t + Δ t, expressed asThe subscript a indicates the direction estimated by means of the accelerometer and magnetometer,refers to the direction estimated at the previous time t, and is noted asThe subscript f denotes the direction estimate, λ, after the complementary filter fusion_kWill be changed to the optimum value, λ, in each iteration_t+ΔtThe rate of convergence of the control is equal to or greater than the rate of change of direction, so that one iteration per time period calculation is acceptable, α being an increased value of lambda in view of the presence of noise in the accelerometer and magnetometer.

4. The method of claim 1, wherein the acceleration and magnetomechanical signals processed by the low pass filter are combined with the gyroscope signals processed by the high pass filter to obtain a final optimal velocity.

5. The method of claim 1, wherein the compensation scheme includes a zero offset drift compensation of a gyroscope and accelerometer and magnetometer compensation.

6. An adaptive gain complementary filtering system for pedestrian orientation estimation, comprising:

a data acquisition unit configured to: acquiring the three-axis acceleration, the angular velocity and the magnetic field intensity of the pedestrian carrier by using inertial sensors integrating a three-axis accelerometer, a three-axis gyroscope and a three-axis magnetometer;

a gyroscope-based direction estimation unit configured to: expressing data measured by a three-axis gyroscope by using a quaternion, describing a direction change rate of a sensor coordinate system relative to a terrestrial coordinate system by using a derivative of the quaternion, and acquiring the direction of the sensor coordinate system relative to the terrestrial coordinate system by integrating the derivative of the quaternion under a known initial condition;

an accelerometer and magnetometer measurements direction calculation unit configured to: calculating the directions of the accelerometer and magnetometer measurements by using a Gauss-Newton optimization method;

a gyroscope last direction estimation unit configured to: the time-varying parameters of the adaptive filter are set as the weight of each direction, and the gain is adaptively adjusted according to the convergence rate of the low-pass frequency based on the Gauss-Newton optimization algorithm and the divergence rate of the high-pass frequency based on the gyroscope, so as to obtain the final direction estimation of the gyroscope.

7. The adaptive gain complementary filtering system for pedestrian orientation estimation of claim 6, further comprising: the zero offset drift compensation unit of gyroscope is used by accumulating drift errors generated by long-time tracking of inertial sensorRepresenting gyroscope zero offset drift compensation;

accelerometer and magnetometer compensation unit: in order to select the most reliable vector among the sensor measurements as the input to the filter, the accelerometer and magnetometer measurements need to be compensated.

8. An adaptive gain complimentary filter for pedestrian orientation estimation, the filter configured to perform the steps of:

gyroscope-based direction estimation: expressing data measured by a three-axis gyroscope by using a quaternion, describing a direction change rate of a sensor coordinate system relative to a terrestrial coordinate system by using a derivative of the quaternion, and acquiring the direction of the sensor coordinate system relative to the terrestrial coordinate system by integrating the derivative of the quaternion under a known initial condition;

calculating the directions of the accelerometer and magnetometer measurements by using a Gauss-Newton optimization method;

the time-varying parameters of the adaptive filter are set as the weight of each direction, and the gain is adaptively adjusted according to the convergence rate of the low-pass frequency based on the Gauss-Newton optimization algorithm and the divergence rate of the high-pass frequency based on the gyroscope, so as to obtain the final direction estimation of the gyroscope.

9. A computer device comprising a memory, a processor and a computer program stored on the memory and executable on the processor, wherein the processor implements the steps of a method of adaptive gain complementary filtering for pedestrian orientation estimation as claimed in any one of claims 1 to 5 when executing said program.

10. A computer-readable storage medium, on which a computer program is stored which, when being executed by a processor, carries out the steps of a method for adaptive gain complementary filtering for pedestrian orientation estimation according to any one of claims 1 to 5.