CN106885566B - A wearable motion sensor and its anti-magnetic field interference method - Google Patents

Abstract

The invention discloses a wearable motion sensor and a method for resisting magnetic field interference thereof, belonging to the research field of wearable sensors, and can accurately estimate the current attitude angle of the motion sensor in real time under the condition of magnetic field interference. The present invention uses the real-time data of acceleration, angular velocity and magnetic field collected by the sensor module, judges the current motion state of the sensor and the interference of the external magnetic field according to these information, and then adopts an adaptive strategy to perform multi-sensor information fusion, calculates and outputs the attitude of the motion sensor horn. The invention is easy to use, is not limited by the site, has low cost, can measure the posture angle of human body parts in real time and with high precision under the condition of magnetic field interference, and has high reliability and good promotion prospect.

Description

A wearable motion sensor and its anti-magnetic field interference method

technical field

The invention belongs to the field of wearable sensors, in particular to a wearable motion sensor and its anti-magnetic field interference method.

Background technique

With the development of micro-electromechanical systems (MEMS) technology, MEMS-based inertial sensors and magnetometers are widely used in the field of human motion analysis, such as gesture recognition, joint kinematics analysis, Daily activity monitoring. Generally, a typical motion sensor includes a three-axis accelerometer, a three-axis gyroscope, and a three-axis magnetometer. For human motion analysis, it is critical to accurately measure the posture of body parts. In order to accurately obtain the posture of body parts, multi-sensor information fusion through fusion algorithms is a common method. The more commonly used fusion algorithms include extended Kalman filter method, gradient descent method, complementary filter method, etc. Through these algorithms, the accuracy of the estimated pose can be improved. However, the estimation accuracy is still vulnerable to the interference of the external environment, especially the magnetic field interference in the environment. Because the earth's magnetic field is very weak, buildings, electromagnetic equipment, computers, mobile phones, etc. in the daily environment will generate large magnetic field interference. The root mean square error of the yaw angle caused by magnetic field interference will reach 15.4° (Yadav et al. Precise estimation of attitude by AHRS under the condition of magnetic field interference Sensors 14 2014). In order to solve the influence of magnetic field interference on the estimation accuracy, some methods set a threshold of magnetic field strength, and the magnetic field exceeding the threshold is considered as an invalid magnetic field, but the adjustment of the threshold is a very cumbersome process, and it is difficult to find a very suitable one. threshold. Another example is a patent for invention with application number CN201310431846.6, which discloses a motion inertial tracking system, which avoids the problem of the influence of magnetic field interference on accuracy by removing the magnetometer module. However, after reducing the magnetometer module, the absolute yaw angle cannot be obtained, and there will be drift errors in the yaw angle. The invention patent with the application number CN201510666248.6 discloses an indoor positioning method based on micro-inertial sensors. This method has a static and dynamic measurement data identification module, and can adopt different methods for static and dynamic estimation of attitude angles. However, this method The method is not specially optimized for magnetic field interference, and large estimation errors will still be introduced when there is magnetic field interference in the environment. Accurately estimating the attitude angle under the condition of magnetic field interference is of great significance to human motion analysis. It is necessary to propose a method specially optimized for external magnetic interference, so that the attitude estimation of the motion sensor has a strong ability to resist magnetic field interference.

Contents of the invention

The purpose of the present invention is to solve the problem of weak anti-magnetic interference ability in the prior art, and provide a wearable motion sensor and its anti-magnetic field interference method, which solves the technical problem that the accuracy of the motion sensor decreases when there is external magnetic field interference . In order to solve the technical problem, the present invention adopts the following specific technical scheme: a wearable motion sensor, including a lithium battery, a power management module, an MCU module, a sensor module and a status indication module, and the lithium battery is connected to the power management module , the power management module, the sensor module and the status indication module are all connected to the MCU module.

Further, the MCU module is integrated with WIFI.

Further, the sensor module is composed of an accelerometer, a gyroscope and a magnetometer.

Another object of the present invention is to provide a method for using a wearable motion sensor to resist magnetic field interference, including the following steps: measure the acceleration, angular velocity and magnetic field information of the sensor module in real time, and perform static judgment according to the acceleration and angular velocity information. If it is static, keep the current attitude unchanged. If it is non-static, calculate the degree of external magnetic field interference, and calculate the weight of the 6-axis algorithm that fuses acceleration and angular velocity and the 9-axis algorithm that fuses acceleration, angular velocity, and magnetic field according to the degree of magnetic field interference. After weighting, the current attitude of the sensor is obtained,

In the formula: is the estimated attitude of the sensor at time t (quaternion form), is the attitude obtained by the 6-axis algorithm, is the attitude obtained by the 9-axis algorithm, and λ and 1-λ are the weights of the 6-axis algorithm and the 9-axis algorithm, respectively.

Further, the specific steps of the static judgment are as follows: when performing static detection, the acceleration static judgment condition and the angular velocity judgment condition are set, and only when these two conditions are satisfied at the same time, it is judged that the current sensor is in a static state.

The static judgment condition of acceleration can be described as that within a certain period of time, the acceleration variation in the 3-axis direction is less than the set threshold, which can be expressed as:

In the formula: Indicates the acceleration of the sensor in the X-axis direction at time t, t ₀ represents an adjustable time interval, is the acceleration in the X-axis direction at time tt ₀ , th _a is the threshold value of the static judgment set, the static judgment conditions for the acceleration in the Y-axis and Z-axis directions are the same as the conditions for the X-axis, and the static judgment conditions for the acceleration in the X, Y, and Z axes It is an "and" relationship.

The condition for judging angular velocity data can be described as that the angular velocity of the three axes must be less than a set threshold, which can be expressed as:

In the formula: ω _x ω _y ω _z are the angular velocities of the three axes, th _gyro is the set angular velocity static judgment threshold, and the angular velocity static judgment conditions of the X, Y, and Z axes are in the relationship of "and".

Further, the solution process of the weight λ of the 6-axis algorithm is as follows:

Compare the measured magnetic field strength and magnetic inclination with the geomagnetic field to determine the degree of interference. The calculation formula can be expressed as:

λ ₁ =|||mag||-m ₀ |/m ₀ if λ ₁ >1, λ ₁ =1

λ ₂ =|θ _dip -θ ₀ |/th _dip if λ ₂ >1, λ ₂ =1

λ=(λ ₁ +λ ₂ )/2

In the formula: ||mag|| is the current measured magnetic field size, θ _dip is the current measured magnetic dip angle, m ₀ and θ ₀ are the geomagnetic field size and magnetic dip angle respectively, and th _dip is the set maximum magnetic dip angle error , λ ₁ is the magnetic field interference degree calculated by the magnetic field size, λ ₂ is the interference degree calculated by the magnetic inclination angle, and the final weight λ is the average value of λ ₁ and λ ₂ .

Compared with the prior art, the present invention has the beneficial effects of:

1. Using the present invention to calculate the attitude angle, under static conditions, the angle estimation accuracy of the sensor can be immune to magnetic field interference of arbitrary strength and duration.

2. Under dynamic conditions, the influence of external magnetic field interference on the yaw angle estimation accuracy can be significantly reduced.

3. The present invention does not depend on a specific fusion algorithm, has good versatility, and can be added to a commonly used attitude estimation algorithm to help it enhance its ability to resist magnetic field interference.

4. The hardware structure of the present invention is simple, uses few components and parts, and can obviously reduce the volume of the circuit board and reduce the cost.

Description of drawings

Fig. 1 is a schematic structural diagram of a motion sensor in the present invention;

Fig. 2 is the structural diagram of anti-magnetic field interference method among the present invention;

Fig. 3 is a schematic diagram of determination of acceleration static judgment parameters in the present invention;

Fig. 4 is a schematic diagram of a static anti-magnetic field interference verification method in the present invention;

Fig. 5 is the verification result figure of static anti-magnetic field interference among the present invention;

6 is a schematic diagram of a verification method for dynamic anti-magnetic field interference in the present invention;

Fig. 7 is the external magnetic field strength diagram during dynamic anti-magnetic field interference verification in the present invention;

Fig. 8 is the relative Euler angle error diagram of dynamic anti-magnetic field interference verification in the present invention;

Fig. 9 is a statistical diagram of root mean square error relative to Euler angles in dynamic anti-magnetic field interference verification in the present invention.

Detailed ways

The present invention will be further described below in conjunction with accompanying drawings, because it is convenient to understand better. The technical features in the present invention can be combined with each other under the premise of not conflicting with each other, which does not constitute a limitation.

Part noun meanings involved in the present invention are as follows:

Attitude angle is also called Euler angle, which is an intuitive attitude representation method. The Euler angle involved in the present invention is the Euler angle of the ZYX rotation sequence, wherein the rotation around the Z axis is the yaw angle, and the rotation around the Y axis is the pitch Angle, the rotation around the X axis is the roll angle.

Quaternion is another method of attitude representation. It can be understood as rotating an angle around a unit vector. Quaternion representation can avoid the singularity problem of Euler angle representation. A quaternion can be expressed as:

in, e=[e _x e _y e _z ] represents the axis of rotation, and θ represents the angle at which the vector rotates around the axis of rotation.

The 6-axis algorithm refers to a multi-sensor information fusion algorithm that estimates the attitude angle only by fusing three-axis acceleration and three-axis angular velocity information.

The 9-axis algorithm refers to a multi-sensor information fusion algorithm that estimates the attitude angle by fusing three-axis acceleration, three-axis angular velocity, and three-axis magnetic field information.

The present invention uses a motion sensor with an accelerometer, a gyroscope and a magnetometer and an anti-magnetic field interference method for the device to estimate the current attitude of the sensor in real time. Concrete implementation process of the present invention is as follows:

1) Preparation:

Fig. 1 is a block diagram of a motion sensor system module in the present invention, including a lithium battery, a power management module, an MCU module, a sensor module and a status indication module, the lithium battery is connected to the power management module, the power management module, the sensor module and The status indication modules are all connected with the MCU module. The anti-magnetic field interference method of the present invention is implemented in the motion sensor by programming, and the real-time estimation of the attitude angle is carried out. The WiFi module is integrated on the MCU module. The CC3200 chip of TI Company is selected in this embodiment, but it is not limited to Here; the sensor module is composed of an accelerometer, a gyroscope, and a magnetometer. In this embodiment, the 9-axis sensor integrated sensor MPU9250 from InvenSense Company is selected, but it is not limited thereto. Before the motion sensor is used, a magnetometer calibration is required to remove fixed magnetic field interference. When the motion sensor is in use, the sampling frequency is 200Hz.

2) Static judgment and calculation of magnetic field interference degree

Fig. 2 shows the structural block diagram of anti-magnetic field interference method in the present invention, and motion sensor collects current acceleration in real time, angular velocity and magnetic field information, first carries out static state detection according to acceleration and angular velocity, if judged to be static, then keep current posture unchanged , if it is non-static, calculate the degree of external magnetic field interference. When performing static detection, the acceleration static judgment condition and the angular velocity judgment condition are set, and only when these two conditions are met at the same time can it be judged that the current sensor is in a static state.

The static judgment condition of acceleration can be described as that within a certain period of time, the acceleration variation in the 3-axis direction is less than the set threshold, which can be expressed as:

In the formula: Indicates the acceleration of the sensor in the X-axis direction at time t, t ₀ represents an adjustable time interval, is the acceleration in the X-axis direction at time tt ₀ , th _a is the threshold value of the static judgment set, the static judgment conditions for the acceleration in the Y-axis and Z-axis directions are the same as the conditions for the X-axis, and the static judgment conditions for the acceleration in the X, Y, and Z axes It is an "and" relationship. Fig. 3 shows the schematic diagram of acceleration static judgment threshold selection, the selected th _a should be slightly larger than the peak-to-peak value of the accelerometer when it is static, t ₀ determines the delay from the actual static state to the judgment static state, and in the present embodiment, t ₀ It is selected as 0.5s, and th _a is selected as 0.04g.

The condition for judging angular velocity data can be described as that the angular velocity of the three axes must be less than a set threshold, which can be expressed as:

In the formula: ω _x ω _y ω _z are the angular velocities of the three axes respectively, th _gyro is the set angular velocity static judgment threshold, and the angular velocity static judgment conditions of the X, Y, and Z axes are in the relationship of "and".

Likewise, the set th _gyro should be slightly larger than the peak-to-peak value of the gyroscope at static state, and in this embodiment, th _gyro is selected as 3°/s.

When calculating the degree of magnetic field interference, the measured magnetic field strength and magnetic inclination are compared with the geomagnetic field to determine the degree of interference. The calculation formula can be expressed as:

λ ₁ =|||mag||-m ₀ |/m ₀ if λ ₁ >1, λ ₁ =1

λ ₂ =|θ _dip -θ ₀ |/th _dip if λ ₂ >1, λ ₂ =1

λ=(λ ₁ +λ ₂ )/2

In the formula: ||mag|| is the current measured magnetic field size, θ _dip is the current measured magnetic inclination angle, m ₀ and θ ₀ are the geomagnetic field size and magnetic inclination angle respectively, and their values are in the process of magnetometer calibration Sure. th _dip is the set maximum magnetic inclination error, λ ₁ is the magnetic field interference degree calculated by the magnetic field size, λ ₂ is the interference degree calculated according to the magnetic inclination angle, and the final weight λ is the average value of λ ₁ and λ ₂ .

In this embodiment, the parameters for calculating the degree of magnetic field interference are determined as: m ₀ =0.46Gs, θ ₀ =40.6°, th _dip =20°, and the real-time magnetic dip angle is determined by the formula θ _dip =arccos(A(q)g· h/||h||), where A(q) is the rotation matrix form of the current attitude, g is the acceleration of gravity, and h is the measured magnetic field.

3) Calculation of sensor attitude angle

As shown in Figure 2, in the method of the present invention, the 6-axis algorithm of fusion acceleration and angular velocity information and the 9-axis algorithm of fusion acceleration and angular velocity magnetic field information are simultaneously performed. The 6-axis algorithm and the 9-axis algorithm of this embodiment are both Fusion algorithm based on gradient descent method. In the process of solving the sensor attitude, when the sensor is static, the attitude estimated last time is directly used as the current attitude. When the sensor is in dynamic state, according to the calculated interference degree weights λ and 1-λ, it is applied to the 6-axis algorithm and 9-axis algorithm respectively to obtain the attitude of this time, which can be expressed as:

In the formula: is the estimated attitude of the sensor at time t (quaternion form), is the attitude obtained by the 6-axis algorithm, The pose obtained by the 9-axis algorithm.

4) Static anti-magnetic field interference verification

Fig. 4 is a schematic diagram of the static anti-magnetic field interference verification method in the present invention. In this experiment, the sensor is placed statically on a flat plate without magnetic field interference, and a circular permanent magnet is brought back and forth close to the motion sensor to simulate the external environment. Magnetic field interference, gather the acceleration, angular velocity and magnetometer information of this period of time, estimate the current attitude angle of sensor with the method described in the present invention and original 9-axis algorithm at the same time, determine the present invention by comparing the attitude angle that two kinds of methods obtain Static anti-magnetic field interference effect.

5) Dynamic anti-magnetic field interference verification

The dynamic anti-magnetic field interference verification experiment is carried out on a 3-axis instrument turntable. The 3-axis turntable has three degrees of freedom of rotation, XYZ, corresponding to the XYZ axes of the Euler angle attitude representation. Each rotation axis of the turntable is equipped with a motor And the encoder, the motor is used to provide rotational power, and the encoder is used to measure the rotation angle of each axis. Because of the high accuracy of the encoder, the measured 3-axis angle is compared with the estimated attitude angle as a standard value, so that Get the accuracy of the estimated attitude angle.

FIG. 6 is a schematic diagram of the verification method for dynamic anti-magnetic field interference in this embodiment. The lower left corner is the coordinate system of the instrument turntable, and the motion sensor is fixed on the X-axis frame. A magnetic field interference simulation device is installed on the X-axis frame, including a square plastic tube closed at both ends and a circular permanent magnet. During the experiment, the XYZ axis rotates at the same time, the circular permanent magnet will slide back and forth under the action of gravity, and the distance between the permanent magnet and the motion sensor will also change accordingly, resulting in a changing magnetic field interference, in this way to simulate the external magnetic field interference. Under this condition, use the method of the present invention, the original 9-axis algorithm, and the original 6-axis algorithm to estimate the attitude angle of the sensor respectively, and compare it with the standard attitude angle provided by the turntable to obtain the relative Euler angle error. By comparing 3 The estimation error of the algorithm is used to determine the dynamic anti-magnetic field interference effect of the present invention.

6) The effect of anti-magnetic field interference

Carry out static anti-magnetic field interference experiment and dynamic anti-interference experiment in the present embodiment, Fig. 5 is static anti-interference experiment result, it can be seen that the attitude angle estimated by the present invention is not affected by external magnetic field interference at all, under strong magnetic field interference , remains unchanged, in line with the actual situation, while the yaw angle deviation of the original 9-axis algorithm has reached 50°. In this embodiment, a total of 10 dynamic anti-interference experiments were carried out, and FIG. 7 is a diagram of the external magnetic field strength during one of the dynamic anti-magnetic field interference experiments. It can be seen from the figure that magnetic field interference occurs periodically. Fig. 8 is the relative Euler angle error figure estimated by three kinds of comparison methods of this experiment, and Fig. 9 is the relative Euler angle root mean square error figure that all 10 experiments obtain, as seen from Fig. 8 and Fig. 9, the present invention The relative Euler angle error obtained by the above method is obviously smaller than the original 6-axis algorithm and 9-axis algorithm.

It can be seen that the present invention improves the accuracy of estimating the attitude angle no matter it is static or dynamic.

The above descriptions are only preferred embodiments of the present invention, and are not intended to limit the present invention. Those of ordinary skill in the relevant technical fields can also make various changes and changes without departing from the spirit and scope of the present invention. transform. For example, the selected 6-axis algorithm and 9-axis algorithm can also use fusion algorithms based on extended Kalman filtering or complementary filtering. It can be seen that any modification, equivalent replacement and improvement made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims (5)

1. A method for anti-magnetic field interference of wearable motion sensor, described wearable motion sensor comprises lithium battery, power management module, MCU module, sensor module and state indicating module, and described lithium battery is connected with power management module , the power management module, the sensor module and the state indication module are all connected to the MCU module, wherein the method is as follows: measure the acceleration, angular velocity and magnetic field information of the sensor module in real time, and perform static judgment according to the acceleration and angular velocity information, If it is static, keep the current attitude unchanged. If it is non-static, calculate the degree of external magnetic field interference, and calculate the 6-axis algorithm of fusion acceleration and angular velocity and the 9-axis algorithm of fusion acceleration, angular velocity and magnetic field according to the degree of magnetic field interference. Weight, after weighting, the current attitude of the sensor is obtained, In the formula: is the estimated attitude quaternion form of the sensor at time t, is the attitude obtained by the 6-axis algorithm, is the attitude obtained by the 9-axis algorithm, and λ and 1-λ are the weights of the 6-axis algorithm and the 9-axis algorithm, respectively. 2. The method for anti-magnetic field interference of the wearable motion sensor according to claim 1, wherein the specific steps of the static judgment are as follows: when performing static detection, the acceleration static judgment condition and the angular velocity judgment condition are set, and only When these two conditions are met at the same time, it is determined that the current sensor is static; The static judgment condition of acceleration can be described as that within a certain period of time, the acceleration variation in the 3-axis direction is less than the set threshold, which can be expressed as: In the formula: Indicates the acceleration of the sensor in the X-axis direction at time t, t ₀ represents an adjustable time interval, is the acceleration in the X-axis direction at time tt ₀ , th _a is the threshold value of the static judgment set, the static judgment conditions for the acceleration in the Y-axis and Z-axis directions are the same as the conditions for the X-axis, and the static judgment conditions for the acceleration in the X, Y, and Z axes It is the relationship of "and"; The condition for judging angular velocity data can be described as that the angular velocity of the three axes must be less than a set threshold, which can be expressed as: In the formula: ω _x ω _y ω _z are the angular velocities of the three axes respectively, th _gyro is the set angular velocity static judgment threshold, and the angular velocity static judgment conditions of the X, Y, and Z axes are in the relationship of "and". 3. the method for the anti-magnetic field interference of wearable motion sensor according to claim 1, is characterized in that, the solution process of the weight λ of described 6 axis algorithms is as follows: Compare the measured magnetic field strength and magnetic inclination with the geomagnetic field to determine the degree of interference. The calculation formula can be expressed as: λ=(λ ₁ +λ ₂ )/2 In the formula: ||mag|| is the current measured magnetic field size, θ _dip is the current measured magnetic dip angle, m ₀ and θ ₀ are the geomagnetic field size and magnetic dip angle respectively, and th _dip is the set maximum magnetic dip angle error , λ ₁ is the magnetic field interference degree calculated by the magnetic field size, λ ₂ is the interference degree calculated by the magnetic inclination angle, and the final weight λ is the average value of λ ₁ and λ ₂ . 4. The anti-magnetic field interference method of the wearable motion sensor according to claim 1, wherein a WIFI module is integrated on the MCU module. 5. The anti-magnetic field interference method of the wearable motion sensor according to claim 1, wherein the sensor module is composed of an accelerometer, a gyroscope and a magnetometer.