CN106989773B - A kind of attitude transducer and posture renewal method - Google Patents
A kind of attitude transducer and posture renewal method Download PDFInfo
- Publication number
- CN106989773B CN106989773B CN201710223535.9A CN201710223535A CN106989773B CN 106989773 B CN106989773 B CN 106989773B CN 201710223535 A CN201710223535 A CN 201710223535A CN 106989773 B CN106989773 B CN 106989773B
- Authority
- CN
- China
- Prior art keywords
- distance measuring
- sensor
- measuring sensor
- attitude
- distance
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 20
- 230000001133 acceleration Effects 0.000 claims abstract description 35
- 238000005259 measurement Methods 0.000 claims abstract description 26
- 210000003141 lower extremity Anatomy 0.000 claims abstract description 5
- 239000013598 vector Substances 0.000 claims description 42
- 239000011159 matrix material Substances 0.000 claims description 40
- 238000001914 filtration Methods 0.000 claims description 13
- 230000005484 gravity Effects 0.000 claims description 6
- 239000000523 sample Substances 0.000 claims description 6
- 238000006073 displacement reaction Methods 0.000 claims description 4
- 238000009434 installation Methods 0.000 claims description 4
- 238000005070 sampling Methods 0.000 claims description 3
- 230000004927 fusion Effects 0.000 abstract 1
- 230000036544 posture Effects 0.000 description 9
- 210000003414 extremity Anatomy 0.000 description 4
- 238000004364 calculation method Methods 0.000 description 2
- 230000000295 complement effect Effects 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000005021 gait Effects 0.000 description 2
- 238000012856 packing Methods 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 210000001699 lower leg Anatomy 0.000 description 1
- 239000000463 material Substances 0.000 description 1
- 230000002093 peripheral effect Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01D—MEASURING NOT SPECIALLY ADAPTED FOR A SPECIFIC VARIABLE; ARRANGEMENTS FOR MEASURING TWO OR MORE VARIABLES NOT COVERED IN A SINGLE OTHER SUBCLASS; TARIFF METERING APPARATUS; MEASURING OR TESTING NOT OTHERWISE PROVIDED FOR
- G01D21/00—Measuring or testing not otherwise provided for
- G01D21/02—Measuring two or more variables by means not covered by a single other subclass
Landscapes
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Gyroscopes (AREA)
- Measurement Of The Respiration, Hearing Ability, Form, And Blood Characteristics Of Living Organisms (AREA)
- Navigation (AREA)
Abstract
The invention discloses a kind of attitude transducer and posture renewal methods, it includes shell, 3 distance measuring sensors, gyroscope, accelerometer, magnetometer, WIFI module, CPU, power modules.Gyroscope and accelerometer are also known as inertial sensor, merge with magnetometer, are usually used in the posture renewal of navigation system.CPU is powered by power module, and the angular speed of sensor measurement, acceleration, magnetic field strength, range information are sent to computer by WIFI module by CPU connection WIFI module, inertial sensor, magnetometer and 3 distance measuring sensors.Posture is updated by posture renewal method fusion angle speed, acceleration, magnetic field strength and range data.The configuration of the present invention is simple, it is low in cost, it is suitble to measure the long-time of human body lower limbs posture, posture renewal method is easy to extend, and efficiently solves the drifting problem of gyroscope and accelerometer.
Description
Technical Field
The invention relates to a posture updating method integrating a ranging sensor array with a gyroscope, an accelerometer and a magnetometer, in particular to measurement of lower limb postures in gait analysis, such as the postures of near-ground limbs such as feet and crus.
Background
The attitude sensors based on the MEMS process, which are widely used at present, are mainly inertial sensors (IMU: gyroscope, accelerometer) and magnetometers, and the most commonly used attitude sensors are of models such as MPU6050, MPU6500, MPU9150 and MPU9250, which are manufactured by invansense corporation, the first two types only include the gyroscope and the accelerometer, and the last two types include the gyroscope, the accelerometer and the magnetometer. For the sensors, the commonly used attitude acquisition algorithm is algorithms such as complementary Kalman filtering and extended Kalman filtering, and most state variables are selected from Euler angles and four elements. The attitude can not be determined only by using the IMU to update the attitude, the complementary filtering algorithm is required to fuse the data of the magnetometer to correct the yaw angle, or the extended Kalman filtering algorithm is used to fuse the data of the magnetometer to the Euler angle or the quad-element, but when the magnetometer is interfered by a peripheral strong magnetic field, a new error is introduced by the method.
Under some experimental and working condition environments, other sensors and measuring methods can be used for acquiring information such as postures, displacements and the like instead of the posture sensor according to the needs of specific situations. In human gait measurement, a more accurate method is to use an infrared high-speed camera to capture the posture of a human limb. The mark points made of infrared reflecting materials are attached to all physiological positions of the limbs of the human body, the high-speed camera captures the spatial positions of all the physiological positions of the human body, and the postures of all the limbs of the human body are restored according to the human body model. However, this system is expensive, has high requirements on the light conditions of the environment, can only be used indoors, and has a limited range of space.
Disclosure of Invention
The invention aims to overcome the defects of the prior art and provides an attitude sensor and an attitude updating method.
The purpose of the invention is realized by the following technical scheme: an attitude sensor comprises a shell, a first distance measuring sensor, an inertial sensor, a magnetometer, a second distance measuring sensor, a power supply module, a CPU, a WIFI module and a third distance measuring sensor, wherein the inertial sensor consists of a gyroscope and an accelerometer; the first distance measuring sensor, the second distance measuring sensor and the third distance measuring sensor are horizontally arranged outside the shell, and the installation positions of the first distance measuring sensor, the second distance measuring sensor and the third distance measuring sensor are positioned in the same horizontal plane; the inertial sensor, the magnetometer, the power supply module, the CPU and the WIFI module are all arranged in the shell; CPU is supplied power by power module, CPU passes through the bus and connects first range finding sensor respectively, inertial sensor, the magnetometer, the second range finding sensor, WIFI module and third range finding sensor, first range finding sensor, the distance information of self distance ground is measured to second range finding sensor and third range finding sensor, the angular velocity of attitude sensor is measured to the gyroscope, the acceleration of attitude sensor is measured to the accelerometer, the magnetometer is measured the magnetic field intensity of attitude sensor's all ring edge borders, CPU conveys the external computer through the WIFI module after with the distance information of gathering, the angular velocity, the acceleration, the packing of magnetic field intensity.
The attitude updating method of the attitude sensor comprises the following steps:
(1) binding the attitude sensor on the lower limb to be measured, enabling the measuring probes of the 3 distance measuring sensors to face the ground, measuring the distance information between the measuring probes and the ground, and starting to measure by the attitude sensor to obtain the angular velocity, the acceleration, the magnetic field intensity and the distance information of the 3 distance measuring sensors;
(2) deducing and establishing a Kalman filtering algorithm fusing angular speed, acceleration, magnetic field intensity and distance information; the method comprises the following two substeps;
(2.1) predictive update of the state vector X; determining a state vector X ═ qp v]TWherein q is four elements, p is the displacement of the attitude sensor, v is the velocity of the attitude sensor, and the state equation is used for the prediction update of the state vector X:
Xk/k-1＝F·Xk-1+uk-1
wherein, Xk/k-1Predicting the updated state variable, X, for time kk-1Is the state variable at the time k-1, u is the input quantity, uk-1I represents an identity matrix and F is a state equation coefficient matrix, wherein I is the input quantity at the moment of k-1:
wherein, yA＝[yA,x yA,y yA,z]TFor the acceleration information measured by the accelerometer, the lower right small labels x, y and z respectively represent the components of the vector on the x axis, the y axis and the z axis, the lower right small label A represents the accelerometer, and y represents the accelerometerA,k-1For the acceleration measured at time k-1,is the value of the acceleration of gravity,an attitude matrix of the attitude sensor, T is sampling time, and phi is a coefficient matrix of a four-element q:
the three-dimensional angular velocity is measured by a gyroscope;
(2.2) updating the measurement of the state vector X; using the measured variable Z to predict the updated state variable Xk/k-1Performing measurement update, namely:
Xk＝Xk/k-1+Kk·(Z-H·Xk/k-1)
wherein, XkUpdated state variable for measurement at time K, KkFor filter gain, vector is measured
Z＝[yA yM d1 d2 d3 d21 d32 d13]TAmount ofThe equation coefficient matrix H is:
wherein, yA＝[yA,x yA,y yA,z]TAcceleration information obtained for accelerometer measurements, yM＝[yM,x yM,yyM,z]TInformation on the strength of the magnetic field measured for the magnetometer, d1、d2、d3Respectively measuring the distance information obtained by the first distance measuring sensor, the second distance measuring sensor and the third distance measuring sensor, and recording the measurement difference value of the distance measuring sensors as dji＝dj-di(i,j＝1,2,3)，d21、d32、d13Respectively obtaining the distance difference information of a second distance measuring sensor and a first distance measuring sensor, a third distance measuring sensor and a second distance measuring sensor, and a first distance measuring sensor and a third distance measuring sensor;
HA(k) is an acceleration yACoefficient matrix of (2):
wherein,is the three-dimensional vector value of the gravity acceleration, q ═ q0 q1 q2 q3]TIs four elements;
HM(k) is the magnetic field strength yMCoefficient matrix of (2):
wherein,is a three-dimensional vector value of the earth magnetic field strength;
note the bookRespectively recording space distance vectors from the first distance measuring sensor, the second distance measuring sensor and the third distance measuring sensor to the inertial sensorFor the uniform expression form of the space distance vector, the distance information measured by the distance measuring sensor is uniformly expressed as di(i＝1,2,3)，Are respectively d1、d2、d3Coefficient matrix of (2), distance information diCoefficient matrix ofThe unified expression form is:
note P1、P2、P3The spatial positions of the first distance measuring sensor, the second distance measuring sensor and the third distance measuring sensor respectively so as toRepresenting a space vector from a first ranging sensor to a second ranging sensor toRepresenting a space vector from the second ranging sensor to the third ranging sensor toRepresenting the space vector from the third ranging sensor to the first ranging sensor, and recording the measurement difference information of the ranging sensors as dji＝dj-di(i, j ═ 1,2,3), and the space vector of the mutual positions of the ranging sensors isMeasuring difference information d for distance measuring sensors21、d32、d13Coefficient matrix of (2), their unified expressionComprises the following steps:
(3) calculating according to a Kalman filtering algorithm to obtain four elements; the step comprises the following substeps:
(3.1) predicting and updating the state variable X by using three-dimensional angular velocity information measured by a gyroscope and acceleration information measured by an accelerometer;
(3.2) updating the state variable X measurement by using acceleration information measured by an accelerometer, magnetic field intensity information measured by a magnetometer, distance information measured by a first distance measuring sensor, a second distance measuring sensor and a third distance measuring sensor, and distance difference information between the second distance measuring sensor and the first distance measuring sensor, between the third distance measuring sensor and the second distance measuring sensor, and between the first distance measuring sensor and the third distance measuring sensor;
the state variable X comprises four elements q, so that the values of the four elements are known after Kalman filtering;
(4) calculating by using four elements to obtain an attitude matrix and an attitude angle of the attitude sensor;
according to the four elements q ═ q0 q1 q2 q3]TComputing attitude matrix for attitude sensorComprises the following steps:
then the attitude angle of the attitude sensor can be calculated from the attitude matrix, and the pitch angle θ, roll angle γ, and azimuth angle Ψ of the attitude sensor are determined as follows:
θ＝arcsinT32
the invention has the advantages that the traditional attitude sensor integrating the IMU (gyroscope and accelerometer) and the magnetometer is improved, the original structural part is fully reserved, and the hardware is easy to expand. And present range finding sensor based on MEMS technology such as laser range finding sensor VL53L0X low price, the chip is small, and convenient installation is around original attitude sensor shell, can not mutually interfere in the short distance to use I2C communication, can carry a plurality ofly simultaneously on same CPU circuit. The attitude updating algorithm of the novel attitude sensor is very similar to the commonly used extended Kalman updating algorithm, only the state variable and the measurement variable are supplemented, the calculation part of the original algorithm is still not influenced, and the rows of the measurement variable and a measurement equation coefficient matrix can be simply deleted to realize the conversion of the algorithm. Because the novel attitude sensor fuses 3 distance measuring sensors, and the spatial attitude of the plane can be accurately known by knowing the length from the perpendicular line of 3 points of the plane to the ground, the data of the 3 distance measuring sensors is theoretically very accurate to the correction of the attitude and can completely eliminate the offset error of the IMU, the realization of the algorithm is not complex, and the method has great advantages compared with the traditional method.
Drawings
FIG. 1 is a schematic structural diagram of an attitude sensor according to the present invention;
FIG. 2 is a schematic diagram of the spatial location of various sensors according to the present invention;
FIG. 3 is a simplified flow chart of a method for updating gestures according to the present invention;
in the figure, the device comprises a shell 1, a first ranging sensor 2, an inertial sensor 3, a gyroscope 4, an accelerometer 5, a magnetometer 6, a second ranging sensor 7, a power module 8, a CPU9, a WIFI module 10 and a third ranging sensor 11.
Detailed Description
As shown in fig. 1, the attitude sensor includes a housing 1, a first distance measuring sensor 2, an inertial sensor 3, a magnetometer 6, a second distance measuring sensor 7, a power module 8, a CPU9, a WIFI module 10, and a third distance measuring sensor 11, where the inertial sensor 3 is composed of a gyroscope 4 and an accelerometer 5; the first distance measuring sensor 2, the second distance measuring sensor 7 and the third distance measuring sensor 11 are horizontally arranged outside the shell 1, and the installation positions of the first distance measuring sensor 2, the second distance measuring sensor 7 and the third distance measuring sensor 11 are positioned in the same horizontal plane; the inertial sensor 3, the magnetometer 6, the power supply module 8, the CPU9 and the WIFI module 10 are all arranged in the shell 1; CPU9 is supplied power by power module 8, CPU9 connects first range finding sensor 2 respectively through the bus, inertial sensor 3, magnetometer 6, second range finding sensor 7, WIFI module 10 and third range finding sensor 11, first range finding sensor 2, second range finding sensor 7 and third range finding sensor 11 measure the distance information of self apart from the ground, gyroscope 4 measures attitude sensor's angular velocity, accelerometer 5 measures attitude sensor's acceleration, magnetometer 6 measures attitude sensor's the magnetic field intensity of the surrounding environment, CPU9 is with the distance information who gathers, angular velocity, acceleration, convey to external computer through WIFI module 10 after the magnetic field intensity packing.
As shown in fig. 2 and 3, the method for updating the posture of the posture sensor according to the present invention includes the following steps:
1. the attitude sensor is bound on the lower limb to be measured, so that the measuring probes of the 3 distance measuring sensors face the ground and can measure the distance information between the measuring probes and the ground, and the attitude sensor starts to measure to obtain the angular velocity, the acceleration, the magnetic field intensity and the distance information of the 3 distance measuring sensors.
2. And deducing and establishing a Kalman filtering algorithm fusing angular velocity, acceleration, magnetic field intensity and distance information, wherein the Kalman filtering algorithm comprises two steps of prediction updating and measurement updating.
First, the prediction of state vector X is updated. Determining a state vector X ═ qp v]TWherein q is four elements, p is the displacement of the attitude sensor, v is the velocity of the attitude sensor, and the state equation is used for the prediction update of the state vector X:
Xk/k-1＝F·Xk-1+uk-1
wherein, Xk/k-1Predicting the updated state variable, X, for time kk-1Is the state variable at the time k-1, u is the input quantity, uk-1I represents an identity matrix and F is a state equation coefficient matrix, wherein I is the input quantity at the moment of k-1:
wherein, yA＝[yA,x yA,y yA,z]TFor the acceleration information measured by the accelerometer 5, the lower right corner small labels x, y and z respectively represent the components of the vector on the x axis, the y axis and the z axis, the lower right corner small label A represents the accelerometer, and y represents the accelerationA,k-1For the acceleration measured at time k-1,is the value of the acceleration of gravity,an attitude matrix of the attitude sensor, T is sampling time, and phi is a coefficient matrix of a four-element q:
is the three-dimensional angular velocity measured by the gyroscope 4.
Second, the measurement of the state vector X is updated. Using the measured variable Z to predict the updated state variable Xk/k-1Perform measurement update, i.e.
Xk＝Xk/k-1+Kk·(Z-H·Xk/k-1)
Wherein, XkUpdated state variable for measurement at time K, KkFor filter gain, the measurement vector Z ═ yA yM d1d2 d3 d21 d32 d13]TThe measurement equation coefficient matrix H is:
wherein, yA＝[yA,x yA,y yA,z]TAcceleration information measured for the accelerometer 5, yM＝[yM,x yM,yyM,z]TInformation on the strength of the magnetic field measured for the magnetometer 6, d1、d2、d3Respectively the distance information measured by the first distance measuring sensor 2, the second distance measuring sensor 7 and the third distance measuring sensor 11, and the measuring difference value of the distance measuring sensors is recorded as dji＝dj-di(i,j＝1,2,3)，d21、d32、d13The distance difference information of the second distance measuring sensor 7 and the first distance measuring sensor 2, the third distance measuring sensor 11 and the second distance measuring sensor 7, and the first distance measuring sensor 2 and the third distance measuring sensor 11 are respectively.
HA(k) Is an acceleration yACoefficient matrix of (2):
wherein,is the three-dimensional vector value of the gravity acceleration, q ═ q0 q1 q2 q3]TIs four elements.
HM(k) Is the magnetic field strength yMCoefficient matrix of (2):
wherein,is the earth magnetismThree-dimensional vector values of field strength.
Note the bookThe space distance vectors from the first distance measuring sensor 2, the second distance measuring sensor 7 and the third distance measuring sensor 11 to the inertial sensor 3 are recorded respectivelyFor the uniform expression form of the space distance vector, the distance information measured by the distance measuring sensor is uniformly expressed as di(i＝1,2,3)， Are respectively d1、d2、d3Coefficient matrix of (2), distance information diCoefficient matrix ofThe unified expression form is:
note P1、P2、P3The spatial positions of the first distance measuring sensor 2, the second distance measuring sensor 7 and the third distance measuring sensor 11 respectively, so as toRepresenting the space vector from the first ranging sensor 2 to the second ranging sensor 7 toRepresenting the space vector of the second 7 to third 11 ranging sensors toRepresenting the space vector from the third distance measuring sensor 11 to the first distance measuring sensor 2, and recording the information of the difference value measured by the distance measuring sensors as dji＝dj-di(i, j ═ 1,2,3), and the space vector of the mutual positions of the ranging sensors isMeasuring difference information d for distance measuring sensors21、d32、d13Coefficient matrix of (2), their unified expressionComprises the following steps:
3. and calculating according to a Kalman filtering algorithm to obtain four elements. Namely:
in the first step, the state variable X is updated by prediction using three-dimensional angular velocity information measured by the gyroscope 4 and acceleration information measured by the accelerometer 5.
And secondly, measuring and updating the state variable X by using acceleration information measured by the accelerometer 5, magnetic field intensity information measured by the magnetometer 6, distance information measured by the first distance measuring sensor 2, the second distance measuring sensor 7 and the third distance measuring sensor 11, and distance difference information between the second distance measuring sensor 7 and the first distance measuring sensor 2, between the third distance measuring sensor 11 and the second distance measuring sensor 7, and between the first distance measuring sensor 2 and the third distance measuring sensor 11.
The state variable X contains four elements q, so the values of the four elements after kalman filtering are known.
4. And obtaining an attitude matrix and an attitude angle of the attitude sensor by using four-element calculation.
According to the four elements q ═ q0 q1 q2 q3]TComputing attitude matrix for attitude sensorComprises the following steps:
then the attitude angle of the attitude sensor can be calculated from the attitude matrix, and the pitch angle θ, roll angle γ, and azimuth angle Ψ of the attitude sensor are determined as follows:
θ＝arcsinT32
Claims (1)
1. A posture updating method of a posture sensor comprises a shell (1), a first distance measuring sensor (2), an inertial sensor (3), a magnetometer (6), a second distance measuring sensor (7), a power supply module (8), a CPU (9), a WIFI module (10) and a third distance measuring sensor (11), wherein the inertial sensor (3) consists of a gyroscope (4) and an accelerometer (5); the first distance measuring sensor (2), the second distance measuring sensor (7) and the third distance measuring sensor (11) are horizontally arranged outside the shell (1), and the installation positions of the first distance measuring sensor (2), the second distance measuring sensor (7) and the third distance measuring sensor (11) are positioned in the same horizontal plane; the inertial sensor (3), the magnetometer (6), the power module (8), the CPU (9) and the WIFI module (10) are all arranged in the shell (1); the system comprises a CPU (9), a power module (8) and a power supply module (10), wherein the CPU (9) is respectively connected with a first distance measuring sensor (2), an inertial sensor (3), a magnetometer (6), a second distance measuring sensor (7), a WIFI module (10) and a third distance measuring sensor (11) through buses, the first distance measuring sensor (2), the second distance measuring sensor (7) and the third distance measuring sensor (11) measure distance information from the first distance measuring sensor to the ground, a gyroscope (4) measures the angular velocity of an attitude sensor, an accelerometer (5) measures the acceleration of the attitude sensor, the magnetometer (6) measures the magnetic field strength of the surrounding environment of the attitude sensor, and the CPU (9) packages the collected distance information, the angular velocity, the acceleration and the magnetic field strength information and then transmits the packed information to an external computer through the WIFI module (10); the method is characterized by comprising the following steps:
(1) binding the attitude sensor on the lower limb to be measured, enabling the measuring probes of the 3 distance measuring sensors to face the ground, measuring the distance information between the measuring probes and the ground, and starting to measure by the attitude sensor to obtain the angular velocity, the acceleration, the magnetic field intensity and the distance information of the 3 distance measuring sensors;
(2) deducing and establishing a Kalman filtering algorithm fusing angular speed, acceleration, magnetic field intensity and distance information; the method comprises the following two substeps;
(2.1) predictive update of the state vector X; determining a state vector X ═ qp v]TWherein q is four elements, p is the displacement of the attitude sensor, v is the velocity of the attitude sensor, and the state equation is used for the prediction update of the state vector X:
Xk/k-1＝F·Xk-1+uk-1
wherein, Xk/k-1Predicting the updated state variable, X, for time kk-1Is the state variable at the time k-1, u is the input quantity, uk-1I represents an identity matrix and F is a state equation coefficient matrix, wherein I is the input quantity at the moment of k-1:
wherein, yA＝[yA,x yA,y yA,z]TFor the acceleration information measured by the accelerometer (5), the components of the vector on the x axis, the y axis and the z axis are respectively represented by small marks x, y and z at the lower right corner, the accelerometer is represented by a small mark A at the lower right corner, and y is represented by a small mark A at the lower right cornerA,k-1For the acceleration measured at time k-1,is the value of the acceleration of gravity,an attitude matrix of the attitude sensor, T is sampling time, and phi is a coefficient matrix of a four-element q:
the three-dimensional angular velocity is measured by the gyroscope (4);
(2.2) updating the measurement of the state vector X; using the measured variable Z to predict the updated state variable Xk/k-1Performing measurement update, namely:
Xk＝Xk/k-1+Kk·(Z-H·Xk/k-1)
wherein, XkUpdated state variable for measurement at time K, KkFor filter gain, the measurement vector Z ═ yA yM d1 d2 d3d21 d32 d13]TThe measurement equation coefficient matrix H is:
wherein, yA＝[yA,x yA,y yA,z]TAcceleration information measured for an accelerometer (5), yM＝[yM,x yM,y yM,z]TInformation on the strength of the magnetic field measured for the magnetometer (6), d1、d2、d3Respectively obtaining distance information measured by a first distance measuring sensor (2), a second distance measuring sensor (7) and a third distance measuring sensor (11), and recording the measurement difference value of the distance measuring sensors as dji＝dj-di(i,j＝1,2,3)，d21、d32、d13Respectively are the distance difference information of the second distance measuring sensor (7) and the first distance measuring sensor (2), the third distance measuring sensor (11) and the second distance measuring sensor (7), and the first distance measuring sensor (2) and the third distance measuring sensor (11);
HA(k) is an acceleration yACoefficient matrix of (2):
wherein,is the three-dimensional vector value of the gravity acceleration, q ═ q0 q1 q2 q3]TIs four elements;
HM(k) is the magnetic field strength yMCoefficient matrix of (2):
wherein,is a three-dimensional vector value of the earth magnetic field strength;
note the bookThe space distance vectors from the first distance measuring sensor (2), the second distance measuring sensor (7) and the third distance measuring sensor (11) to the inertial sensor (3) are recorded respectivelyFor the uniform expression form of the space distance vector, the distance information measured by the distance measuring sensor is uniformly expressed as di(i＝1,2,3)， Are respectively d1、d2、d3Coefficient matrix of (2), distance information diCoefficient matrix ofThe unified expression form is:
note P1、P2、P3The spatial positions of the first distance measuring sensor (2), the second distance measuring sensor (7) and the third distance measuring sensor (11) respectively so as toRepresenting a space vector from the first (2) to the second (7) ranging sensorRepresenting the space vector of the second (7) to third (11) ranging sensorsRepresenting the space vector from the third distance measuring sensor (11) to the first distance measuring sensor (2), and recording the information of the difference value measured by the distance measuring sensors as dji＝dj-di(i, j ═ 1,2,3), and the space vector of the mutual positions of the ranging sensors isMeasuring difference information d for distance measuring sensors21、d32、d13Coefficient matrix of (2), their unified expressionComprises the following steps:
(3) calculating according to a Kalman filtering algorithm to obtain four elements; the step comprises the following substeps:
(3.1) using three-dimensional angular velocity information measured by the gyroscope (4) and acceleration information measured by the accelerometer (5) to update the state variable X prediction;
(3.2) updating the measurement of the state variable X by using acceleration information measured by an accelerometer (5), magnetic field intensity information measured by a magnetometer (6), distance information measured by a first distance measuring sensor (2), a second distance measuring sensor (7) and a third distance measuring sensor (11), and distance difference information between the second distance measuring sensor (7) and the first distance measuring sensor (2), between the third distance measuring sensor (11) and the second distance measuring sensor (7) and between the first distance measuring sensor (2) and the third distance measuring sensor (11); the state variable X contains four elements q;
(4) calculating by using four elements to obtain an attitude matrix and an attitude angle of the attitude sensor;
according to the four elements q ═ q0 q1 q2 q3]TComputing attitude matrix for attitude sensorComprises the following steps:
then the attitude angle of the attitude sensor can be calculated from the attitude matrix, and the pitch angle θ, roll angle γ, and azimuth angle Ψ of the attitude sensor are determined as follows:
θ＝arcsinT32
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710223535.9A CN106989773B (en) | 2017-04-07 | 2017-04-07 | A kind of attitude transducer and posture renewal method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710223535.9A CN106989773B (en) | 2017-04-07 | 2017-04-07 | A kind of attitude transducer and posture renewal method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN106989773A CN106989773A (en) | 2017-07-28 |
| CN106989773B true CN106989773B (en) | 2019-07-16 |
Family
ID=59414824
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201710223535.9A Active CN106989773B (en) | 2017-04-07 | 2017-04-07 | A kind of attitude transducer and posture renewal method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN106989773B (en) |
Families Citing this family (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20220178692A1 (en) * | 2017-12-21 | 2022-06-09 | Mindmaze Holding Sa | System, method and apparatus of a motion sensing stack with a camera system |
| CN108196701B (en) * | 2018-01-03 | 2020-06-02 | 海信视像科技股份有限公司 | Method and device for determining posture and VR equipment |
| CN108594798B (en) * | 2018-01-09 | 2021-04-16 | 南京理工大学 | Robot trolley system capable of realizing bee-hive control and control method thereof |
| CN109141414A (en) * | 2018-09-05 | 2019-01-04 | 华南农业大学 | A kind of identification of farm work vehicle external acceleration and posture synchronous estimation method |
| CN109673529A (en) * | 2018-12-24 | 2019-04-26 | 公安部南京警犬研究所 | Police dog gesture recognition data vest and gesture recognition method based on multisensor |
| CN114090140B (en) * | 2020-08-05 | 2024-10-11 | 华为技术有限公司 | Interaction method between devices based on pointing operation and electronic device |
| CN113229804A (en) * | 2021-05-07 | 2021-08-10 | 陕西福音假肢有限责任公司 | Magnetic field data fusion circuit and method for joint mobility |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101726295A (en) * | 2008-10-24 | 2010-06-09 | 中国科学院自动化研究所 | Unscented Kalman filter-based method for tracking inertial pose according to acceleration compensation |
| CN102323854A (en) * | 2011-03-11 | 2012-01-18 | 中国科学院研究生院 | Human motion capture device |
| CN103808316A (en) * | 2012-11-12 | 2014-05-21 | 哈尔滨恒誉名翔科技有限公司 | Indoor-flying intelligent body inertial system and laser range finder combination type navigation improving method |
| CN104501814A (en) * | 2014-12-12 | 2015-04-08 | 浙江大学 | Attitude and position estimation method based on vision and inertia information |
| CN105651283A (en) * | 2016-03-14 | 2016-06-08 | 安徽斯玛特物联网科技有限公司 | Precision indoor positioning method based on acceleration, magnetometer, gyroscope and pressure sensor |
| CN205486161U (en) * | 2016-01-08 | 2016-08-17 | 北京永利范思科技有限公司 | Head motion trapping apparatus of virtual reality helmet |
| CN106197428A (en) * | 2016-07-10 | 2016-12-07 | 北京工业大学 | A kind of SLAM method utilizing metrical information Optimum distribution formula EKF estimation procedure |
| CN205899390U (en) * | 2016-08-09 | 2017-01-18 | 青海北斗开阳航空科技有限公司 | Agriculture and forestry plant protection unmanned aerial vehicle automatic flight control system |
-
2017
- 2017-04-07 CN CN201710223535.9A patent/CN106989773B/en active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101726295A (en) * | 2008-10-24 | 2010-06-09 | 中国科学院自动化研究所 | Unscented Kalman filter-based method for tracking inertial pose according to acceleration compensation |
| CN102323854A (en) * | 2011-03-11 | 2012-01-18 | 中国科学院研究生院 | Human motion capture device |
| CN103808316A (en) * | 2012-11-12 | 2014-05-21 | 哈尔滨恒誉名翔科技有限公司 | Indoor-flying intelligent body inertial system and laser range finder combination type navigation improving method |
| CN104501814A (en) * | 2014-12-12 | 2015-04-08 | 浙江大学 | Attitude and position estimation method based on vision and inertia information |
| CN205486161U (en) * | 2016-01-08 | 2016-08-17 | 北京永利范思科技有限公司 | Head motion trapping apparatus of virtual reality helmet |
| CN105651283A (en) * | 2016-03-14 | 2016-06-08 | 安徽斯玛特物联网科技有限公司 | Precision indoor positioning method based on acceleration, magnetometer, gyroscope and pressure sensor |
| CN106197428A (en) * | 2016-07-10 | 2016-12-07 | 北京工业大学 | A kind of SLAM method utilizing metrical information Optimum distribution formula EKF estimation procedure |
| CN205899390U (en) * | 2016-08-09 | 2017-01-18 | 青海北斗开阳航空科技有限公司 | Agriculture and forestry plant protection unmanned aerial vehicle automatic flight control system |
Also Published As
| Publication number | Publication date |
|---|---|
| CN106989773A (en) | 2017-07-28 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN106989773B (en) | A kind of attitude transducer and posture renewal method | |
| CN103776451B (en) | A kind of high-precision three-dimensional attitude inertial measurement system based on MEMS and measuring method | |
| CN111462231B (en) | Positioning method based on RGBD sensor and IMU sensor | |
| CN108731672B (en) | Coal mining machine attitude detection system and method based on binocular vision and inertial navigation | |
| CN101405570B (en) | Motion capture device and associated method | |
| CN110327048B (en) | Human upper limb posture reconstruction system based on wearable inertial sensor | |
| CN108036785A (en) | A kind of aircraft position and orientation estimation method based on direct method and inertial navigation fusion | |
| CN106767804A (en) | The multidimensional data measurement apparatus and method of a kind of moving object | |
| CN109540135B (en) | Method and device for detecting pose and extracting yaw angle of paddy field tractor | |
| CN109724602A (en) | A kind of attitude algorithm system and its calculation method based on hardware FPU | |
| CN109798891A (en) | Inertial Measurement Unit calibration system based on high-precision motion capture system | |
| CN108225370A (en) | A kind of data fusion and calculation method of athletic posture sensor | |
| CN108534744A (en) | A kind of attitude angle acquisition methods, device and handle | |
| Gong et al. | Robust inertial motion tracking through deep sensor fusion across smart earbuds and smartphone | |
| CN108444468B (en) | Directional compass integrating downward vision and inertial navigation information | |
| CN104181573A (en) | Beidou inertial navigation deep integration navigation microsystem | |
| CN110728716B (en) | Calibration method and device and aircraft | |
| CN109685852B (en) | Calibration method, system, equipment and storage medium for camera and inertial sensor | |
| CN107478222A (en) | A kind of wireless wearable human attitude monitoring system based on MEMS technology | |
| CN106227368A (en) | A kind of human synovial angle calculation method and device | |
| CN113229806A (en) | Wearable human body gait detection and navigation system and operation method thereof | |
| CN111811421B (en) | High-speed real-time deformation monitoring method and system | |
| CN111197974B (en) | Barometer height measuring and calculating method based on Android inertial platform | |
| CN112729297A (en) | Miniature navigation attitude positioning device based on multiple MEMS sensors | |
| CN109343713B (en) | Human body action mapping method based on inertial measurement unit |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |