CN106885566A

CN106885566A - A kind of method of wearable motion sensor and its anti-magnetic interference

Info

Publication number: CN106885566A
Application number: CN201710001913.9A
Authority: CN
Inventors: 刘涛; 范冰飞; 李庆国
Original assignee: Zhejiang University ZJU
Current assignee: Nanjing Zheli Intelligent Manufacturing Research Institute Co ltd
Priority date: 2017-01-03
Filing date: 2017-01-03
Publication date: 2017-06-23
Anticipated expiration: 2037-01-03
Also published as: CN106885566B

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 present invention is easy to use, is not limited by the site, has low cost, can measure the attitude 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

Wearable motion sensor and magnetic field interference resisting method thereof

Technical Field

The invention belongs to the field of wearable sensors, and particularly relates to a wearable motion sensor and a method for resisting magnetic field interference.

Background

With the development of micro-electro-mechanical systems (MEMS) technology, MEMS-based inertial sensors and magnetometers are widely applied in the field of human motion analysis, such as gesture recognition, joint kinematics analysis, and daily activity monitoring, with the advantages of low cost, small size, and light weight. In general, a typical motion sensor includes a three-axis accelerometer, a three-axis gyroscope, and a three-axis magnetometer, and it is very critical for human motion analysis to accurately measure the posture of a body part. In order to accurately acquire the posture of the body part, the multi-sensor information fusion through a fusion algorithm is a common method, and the more common fusion algorithm includes an extended kalman filter method, a gradient descent method, a complementary filter method and the like. By these algorithms, the accuracy of estimating the pose can be improved. However, the accuracy of the estimation is still susceptible to disturbances from the external environment, in particular magnetic field disturbances in the environment. Because the earth magnetic field is very weak, buildings, electromagnetic equipment, computers, mobile phones and the like in daily environments can generate large magnetic field interference. The root mean square error of the yaw angle caused by magnetic field disturbances may reach 15.4 ° (Yadav et al. attitude Sensors 142014 are accurately estimated by AHRS under magnetic field disturbance conditions). In order to solve the influence of magnetic field interference on the estimation accuracy, some methods set a magnetic field strength threshold, and a magnetic field exceeding the threshold is considered as a null magnetic field, but the adjustment of the threshold is a very tedious process and it is difficult to find a very suitable threshold. Also disclosed in patent application No. CN201310431846.6 is a motion inertial tracking system that avoids the problem of magnetic field interference affecting accuracy by removing magnetometer modules. However, after reducing the magnetometer modules, the absolute yaw angle cannot be obtained, and the yaw angle has a drift error. The invention patent with the application number of CN201510666248.6 discloses an indoor positioning method based on micro inertial sensors, which has a static and dynamic measurement data identification module, and can adopt different methods to estimate attitude angles for static and dynamic states, but the method is not particularly optimized for magnetic field interference, and when there is magnetic field interference in the environment, a large estimation error is still introduced. The accurate estimation of the attitude angle under the condition of magnetic field interference has important significance for human motion analysis, and a method for carrying out special optimization aiming at external magnetic interference is necessary to be provided, so that the attitude estimation of the motion sensor has stronger magnetic field interference resistance.

Disclosure of Invention

The invention aims to solve the problem of weak anti-magnetic field interference capability in the prior art, provides a wearable motion sensor and a method for resisting magnetic field interference thereof, and solves the technical problem of reduced precision of the motion sensor when magnetic field interference exists outside the motion sensor. In order to solve the technical problem, the invention adopts the following specific technical scheme: the utility model provides a wearable motion sensor, includes lithium cell, power management module, MCU module, sensor module and status indication module, the lithium cell links to each other with power management module, sensor module and status indication module all link to each other with the MCU module.

Furthermore, WIFI is integrated on the MCU module.

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 resisting magnetic field interference using a wearable motion sensor, comprising the steps of: measuring acceleration, angular velocity and magnetic field information of the sensor module in real time, performing static judgment according to the acceleration and angular velocity information, if the acceleration and angular velocity information is static, keeping the current attitude unchanged, if the acceleration and angular velocity information is non-static, performing external magnetic field interference degree calculation, calculating weights of a 6-axis algorithm fusing the acceleration and the angular velocity and a 9-axis algorithm fusing the acceleration, the angular velocity and the magnetic field according to the magnetic field interference degree, weighting to obtain the current attitude of the sensor,

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

Further, the static state determination specifically comprises the following steps: when static detection is carried out, an acceleration static judgment condition and an angular velocity judgment condition are set, and the current sensor is judged to be in a static state only when the two conditions are met simultaneously.

The static judgment condition of the acceleration can be described as that the acceleration change amplitude in the 3-axis direction is smaller than a set threshold value in a certain time period, and can be represented as:

in the formula:indicating the acceleration of the sensor in the X-axis direction at time t, t₀An adjustable time interval is indicated which is,is t-t₀Acceleration in the direction of the X-axis at time th_aThe threshold value is set for static determination, the static determination conditions for acceleration in the Y-axis and Z-axis directions are the same as those for the X-axis, and the static determination conditions for acceleration in the X, Y, Z-axis are in an and relationship.

The condition for judging the angular velocity data can be described as that the angular velocities of the 3 axes must be respectively smaller than a set threshold value, which can be expressed as:

in the formula: omega_xω_yω_zAngular velocities, th, of 3 axes, respectively_gyroIs a set angular velocity stationary determination threshold value, and the angular velocity stationary determination condition of the X, Y, Z axes is an and relationship.

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

the measured magnetic field strength and inclination are compared with the earth's magnetic field to determine the degree of disturbance, which can be expressed as:

λ₁＝|||mag||-m₀|/m₀ifλ₁＞1，λ₁＝1

λ₂＝|θ_dip-θ₀|/th_dipifλ₂＞1，λ₂＝1

λ＝(λ₁+λ₂)/2

in the formula: | mag | is the currently measured magnetic field magnitude, θ_dipFor the currently measured declination angle, m₀And theta₀Respectively the magnitude of the earth magnetic field and the angle of inclination, th_dipFor a set maximum declination error, λ₁Is the degree of magnetic field interference, λ, calculated from the magnitude of the magnetic field₂Is calculated according to the magnetic inclination angle, and the final weight lambda is lambda₁、λ₂Average value of (a).

Compared with the prior art, the invention has the beneficial effects that:

1. the attitude angle is calculated by using the method, so that the angle estimation precision of the sensor can realize the interference immunity of the magnetic field with any intensity and duration under the static condition.

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

3. The method is independent of a specific fusion algorithm, has good universality, and can be added to a common attitude estimation algorithm to help the attitude estimation algorithm to enhance the magnetic field interference resistance.

4. The invention has simple hardware structure and few used components, and can obviously reduce the volume of the circuit board and reduce the cost.

Drawings

FIG. 1 is a schematic diagram of a motion sensor according to the present invention;

FIG. 2 is a block diagram of the method of the present invention for resisting magnetic field interference;

FIG. 3 is a schematic diagram illustrating determination of static acceleration determination parameters according to the present invention;

FIG. 4 is a schematic diagram of a static magnetic field disturbance rejection verification method according to the present invention;

FIG. 5 is a diagram illustrating the result of verifying the static state of the present invention against magnetic field interference;

FIG. 6 is a schematic diagram of a verification method for dynamic anti-magnetic field interference according to the present invention;

FIG. 7 is a graph of external magnetic field strength during dynamic anti-magnetic field disturbance verification in accordance with the present invention;

FIG. 8 is a graph of relative Euler angle error for dynamic anti-magnetic field disturbance validation in accordance with the present invention;

FIG. 9 is a diagram showing a relative Euler angle root mean square error statistics of the dynamic anti-magnetic field interference verification of the present invention.

Detailed Description

The invention will be further described below with reference to the accompanying drawings for better understanding. The technical features of the present invention can be combined with each other without conflicting with each other, and are not limited.

Some of the nouns referred to in the present invention have the following meanings:

the Euler angle is a Euler angle in a ZYX rotation sequence, wherein the Euler angle is a yaw angle by rotating around a Z axis, a pitch angle by rotating around a Y axis and a roll angle by rotating around the X axis.

Quaternions are another method for representing attitude, and can be understood as rotating an angle around a unit vector, the quaternion representation can avoid the singularity problem of the euler angle representation, and one quaternion can be represented as:

wherein,e＝[e_xe_ye_z]representing the axis of rotation and theta representing the angle of rotation of the vector about the axis of rotation.

The 6-axis algorithm is a multi-sensor information fusion algorithm for estimating attitude angles only by fusing triaxial acceleration and triaxial angular velocity information.

The 9-axis algorithm is a multi-sensor information fusion algorithm for estimating an attitude angle by fusing triaxial acceleration, triaxial angular velocity and triaxial magnetic field information.

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

1) preparation work:

fig. 1 is a block diagram of a motion sensor system according to the present invention, which includes a lithium battery, a power management module, an MCU module, a sensor module, and a status indication module, wherein the lithium battery is connected to the power management module, and the power management module, the sensor module, and the status indication module are all connected to the MCU module. The method for resisting magnetic field interference of the invention realizes real-time estimation of attitude angle in a motion sensor by a programming mode, a WiFi module is integrated on the MCU module, and the MCU module is selected as a CC3200 chip of TI company, but is not limited to the CC3200 chip; the sensor module comprises an accelerometer, a gyroscope and a magnetometer, and in the embodiment, the 9-axis sensor integrated sensor MPU9250 manufactured by invansense company is selected, but not limited to this. Before the motion sensor is used, magnetometer calibration is needed to remove fixed magnetic field interference. The sampling frequency of the motion sensor is 200Hz during use.

2) Static judgment and magnetic field interference degree calculation

Fig. 2 is a structural block diagram of the method for resisting magnetic field interference according to the present invention, in which a motion sensor collects current acceleration, angular velocity and magnetic field information in real time, and first performs static state detection according to the acceleration and the angular velocity, if the current attitude is determined to be static, the current attitude is kept unchanged, and if the current attitude is not static, the degree of external magnetic field interference is calculated. When static detection is carried out, an acceleration static judgment condition and an angular velocity judgment condition are set, and the current sensor is judged to be in a static state only when the two conditions are met simultaneously.

in the formula:indicating the acceleration of the sensor in the X-axis direction at time t, t₀An adjustable time interval is indicated which is,is t-t₀Acceleration in the direction of the X-axis at time th_aThe threshold value is set for static determination, the static determination conditions for acceleration in the Y-axis and Z-axis directions are the same as those for the X-axis, and the static determination conditions for acceleration in the X, Y, Z-axis are in an and relationship. FIG. 3 is a schematic diagram showing the selection of the threshold for static determination of acceleration, th_aShould be slightly larger than the peak-to-peak value of the accelerometer at rest, t₀The actual static to decision static delay is determined, and in this embodiment, t₀Is selected to be 0.5s, th_a0.04g was selected.

Likewise, th of setting_gyroShould be slightly larger than the peak-to-peak value of the gyroscope at rest, in this embodiment th_gyroThe selection was 3 °/s.

When the magnetic field interference degree is calculated, the measured magnetic field intensity and the magnetic inclination angle are compared with the geomagnetic field, so as to determine the interference degree, and the calculation formula can be expressed as:

λ₁＝|||mag||-m₀|/m₀ifλ₁＞1，λ₁＝1

λ₂＝|θ_dip-θ₀|/th_dipifλ₂＞1，λ₂＝1

λ＝(λ₁+λ₂)/2

in the formula: | mag | is the currently measured magnetic field magnitude, θ_dipFor the currently measured declination angle, m₀And theta₀The magnitude of the earth magnetic field and the magnetic tilt, respectively, whose values are determined during the calibration of the magnetometer. th (h)_dipFor a set maximum declination error, λ₁The magnetic field interference degree, lambda, is calculated according to the magnitude of the magnetic field₂Is calculated according to the magnetic inclination angle, and the final weight lambda is lambda₁、λ₂Average value of (a).

In the present embodiment, the parameters for the calculation of the degree of magnetic field interference are determined as follows: m is₀＝0.46Gs，θ₀＝40.6°，th_dipThe real-time magnetic tilt angle is represented by the formula theta at 20 DEG_dipCalculation is given as arccos (a (q) g · h/| | h | |), where a (q) is the rotation matrix form of the current pose, g is the gravitational acceleration, and h is the measured magnetic field.

3) Calculation of sensor attitude angle

As shown in fig. 2, in the method of the present invention, a 6-axis algorithm for fusing acceleration and angular velocity information and a 9-axis algorithm for fusing acceleration and angular velocity magnetic field information are performed simultaneously, and both the 6-axis algorithm and the 9-axis algorithm of the present embodiment are based on a gradient descent method. In the sensor attitude solving process, when the sensor is in a static state, the attitude estimated in the previous time is directly used as the attitude of the time. When the sensor is in a dynamic state, applying the interference degree weights lambda and 1-lambda obtained by calculation to a 6-axis algorithm and a 9-axis algorithm respectively to obtain the attitude of the current time, which can be expressed as:

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

4) Static anti-magnetic field interference verification

Fig. 4 is a schematic diagram of a static anti-magnetic field interference verification method in the present invention, in this experiment, a sensor is placed on a flat plate without magnetic field interference, a circular permanent magnet is moved back and forth to approach a motion sensor, in this way, external magnetic field interference is simulated, acceleration, angular velocity and magnetometer information at this time are collected, meanwhile, the current attitude angle of the sensor is estimated by using the method of the present invention and an original 9-axis algorithm, and the static anti-magnetic field interference effect of the present invention is determined by comparing the attitude angles obtained by the two methods.

5) Dynamic anti-magnetic field interference verification

The dynamic magnetic field interference resistance verification experiment is carried out on a 3-axis instrument rotary table, the 3-axis rotary table has three XYZ rotational degrees of freedom and corresponds to XYZ axes of an Euler angle attitude representation method one by one, each rotating shaft of the rotary table is provided with a motor and an encoder, the motor is used for providing rotating power, the encoder is used for measuring the rotating angle of each axis, and due to the fact that the accuracy of the encoder is high, the measured 3-axis angle is used as a standard value to be compared with the estimated attitude angle, and therefore the accuracy of the estimated attitude angle can be obtained.

Fig. 6 is a schematic diagram of the dynamic anti-magnetic field interference verification method according to this embodiment, where the lower left corner is a coordinate system of the instrument turntable, and the motion sensor is fixed on the X-axis frame. The X-axis frame is provided with a magnetic field interference simulation device which comprises a square plastic pipe with two closed ends and a round permanent magnet. When the experiment was carried out, XYZ axle rotates simultaneously, and circular permanent magnet can get down to slide back under the action of gravity, and the distance of permanent magnet and motion sensor also changes thereupon to produce the magnetic field interference of change, simulate external magnetic field interference with this mode. Under the condition, the attitude angle of the sensor is respectively estimated by using the method, the original 9-axis algorithm and the original 6-axis algorithm, the attitude angle is compared with the standard attitude angle provided by the rotary table to obtain the relative Euler angle error, and the dynamic anti-magnetic field interference effect of the invention is determined by comparing the estimation errors of the 3 algorithms.

6) Effect against magnetic field interference

In the embodiment, a static anti-magnetic field interference experiment and a dynamic anti-interference experiment are performed, and fig. 5 shows a static anti-interference experiment result, so that it can be seen that the attitude angle estimated by the method is completely not influenced by the external magnetic field interference, and still remains unchanged under the strong magnetic field interference, which is in line with the actual situation, and the deviation of the yaw angle of the original 9-axis algorithm reaches 50 °. In this embodiment, the dynamic anti-interference experiment is performed 10 times, and fig. 7 is an external magnetic field intensity diagram in one of the dynamic anti-magnetic field interference experiments, and it can be seen from the diagram that magnetic field interference occurs periodically. Fig. 8 is a relative euler angle error graph estimated by three comparison methods in the experiment, fig. 9 is a relative euler angle root mean square error graph obtained in all 10 experiments, and as can be seen from fig. 8 and 9, the relative euler angle error obtained by the method of the present invention is obviously smaller than the original 6-axis algorithm and 9-axis algorithm.

Therefore, the method improves the accuracy of estimating the attitude angle under static or dynamic conditions.

The above description is only for the preferred embodiment of the present invention, and is not intended to limit the present invention, and those skilled in the art can make various changes and modifications without departing from the spirit and scope of the present invention. As the selected 6-axis algorithm and 9-axis algorithm, a fusion algorithm based on extended kalman filtering or complementary filtering may also be used. It is therefore contemplated that the present invention cover any and all modifications, equivalents, and improvements that fall within the spirit and scope of the present invention.

Claims

1. A wearable motion sensor, is characterized in that, comprises lithium battery, power management module, MCU module, sensor module and state indicator module etc., described lithium battery is connected with power management module, and described power management module, sensor The module and the status indication module are all connected with the MCU module.

2. The wearable motion sensor according to claim 1, wherein a WIFI module is integrated on the MCU module.

3. The wearable motion sensor according to claim 1, wherein the sensor module is composed of an accelerometer, a gyroscope and a magnetometer.

4. A method utilizing the anti-magnetic field interference of the wearable motion sensor according to claim 1, characterized in that, the method is specifically as follows: real-time measurement of the acceleration, angular velocity and magnetic field information of the sensor module, according to the acceleration and angular velocity information Perform static judgment, 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 according to the degree of magnetic field interference and the fusion of acceleration, angular velocity and magnetic field The weight of the 9-axis algorithm, after weighting, the current attitude of the sensor is obtained,

\begin{matrix} {q q}_{E E.}^{S S}_{t t} = = λ λ {q q}_{E E.}^{S S}_{I I M m U u,, t t} + + ((11 - - λ λ)) {q q}_{E E.}^{S S}_{M m I I M m U u,, t t} & 00 \leq \leq λ λ \leq \leq 11 \end{matrix}

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.

5. The anti-magnetic field interference method of the wearable motion sensor according to claim 4, 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:

\{\begin{matrix} | | {a a}_{{x x}_{t t}} - - {a a}_{{x x}_{t t - - {t t}_{00}}} | | < < t t {h h}_{a a} \\ | | {a a}_{{y the y}_{t t}} - - {a a}_{{y the y}_{t t - - {t t}_{00}}} | | < < {th the th}_{a a} \\ | | {a a}_{{z z}_{t t}} - - {a a}_{{z z}_{t t - - {t t}_{00}}} | | < < {th the th}_{a a} \end{matrix}

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:

\{\begin{matrix} | | {ω ω}_{x x} | | < < t t {h h}_{g g y the y r r o o} \\ | | {ω ω}_{y the y} | | < < {th the th}_{g g y the y r r o o} \\ | | {ω ω}_{z z} | | < < {th the th}_{g g y the y r r o o} \end{matrix}

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".

6. the method for the anti-magnetic field interference of wearable motion sensor according to claim 4, 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:

\begin{matrix} {λ λ}_{11} = = \frac{| | | | | | m m a a g g | | | | - - {m m}_{00} | |}{{m m}_{00}} & i i f f & {λ λ}_{11} > > 11,, {λ λ}_{11} = = 11 \end{matrix}

\begin{matrix} {λ λ}_{22} = = \frac{| | {θ θ}_{d d i i p p} - - {θ θ}_{00} | |}{{th the th}_{d d i i p p}} & i i f f & {λ λ}_{22} > > 11,, {λ λ}_{22} = = 11 \end{matrix}

λ=(λ ₁ +λ ₂ )/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 λ ₂ .