CN111854762A - Three-dimensional positioning method based on Kalman filtering algorithm and positioning system thereof - Google Patents

Three-dimensional positioning method based on Kalman filtering algorithm and positioning system thereof Download PDF

Info

Publication number
CN111854762A
CN111854762A CN202010780014.5A CN202010780014A CN111854762A CN 111854762 A CN111854762 A CN 111854762A CN 202010780014 A CN202010780014 A CN 202010780014A CN 111854762 A CN111854762 A CN 111854762A
Authority
CN
China
Prior art keywords
error
value
accelerometer
temperature
attitude
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.)
Withdrawn
Application number
CN202010780014.5A
Other languages
Chinese (zh)
Inventor
翟瑞永
王海生
于丽君
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Individual
Original Assignee
Individual
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Individual filed Critical Individual
Priority to CN202010780014.5A priority Critical patent/CN111854762A/en
Publication of CN111854762A publication Critical patent/CN111854762A/en
Withdrawn legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/04—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means
    • G01C21/08—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means involving use of the magnetic field of the earth
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation

Landscapes

  • Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Environmental & Geological Engineering (AREA)
  • General Life Sciences & Earth Sciences (AREA)
  • Geology (AREA)
  • Navigation (AREA)

Abstract

The invention relates to a three-dimensional positioning method based on a Kalman filtering algorithm and a positioning system thereof, belonging to the technical field of three-dimensional positioning methods and positioning systems thereof; the technical problem to be solved is as follows: the improvement of a three-dimensional positioning method based on a Kalman filtering algorithm is provided; the technical scheme for solving the technical problems is as follows: acquiring data information acquired by an MEMS sensor in a three-dimensional positioning system worn by a positioning target; performing respective error compensation on the acquired sensor data; correcting the processed sensor data error by adopting an extended Kalman filtering positioning algorithm of quaternion; error compensation is carried out on the sensor data by adopting a 15-element error state vector navigation algorithm; inputting the sensor data subjected to correction and error compensation in the third step and the fourth step into a Kalman filter for filtering, and outputting real-time position, attitude and speed information of a positioning target; the invention is applied to target positioning in the closed environment with weak GPS signals and weak network signals.

Description

Three-dimensional positioning method based on Kalman filtering algorithm and positioning system thereof
Technical Field
The invention discloses a three-dimensional positioning method based on a Kalman filtering algorithm and a positioning system thereof, belonging to the technical field of three-dimensional positioning methods and positioning systems thereof.
Background
At present, the phenomena of natural disasters such as fire, earthquake and the like happen frequently, the timeliness of rescue is the key of good work, when a series of problems occur, the key problem which needs to be solved urgently is the accurate position of a victim, and the current positioning technology mainly comprises satellite positioning navigation technology such as American GPS positioning technology, Russian GLONASS positioning technology, Chinese Beidou positioning technology and the like, ultrasonic positioning, optical positioning, infrared positioning, wireless positioning, inertial navigation technology INS, radio frequency signal-based RFID positioning technology and the like.
The GPS positioning technology has high performance and precision, but the signal of the GPS positioning technology is objectively existed in the defects that the signal is attenuated by barriers in the environments such as indoor environment, forest environment, underground environment and the like and can not penetrate through buildings for navigation positioning, and the working effect in a closed environment is not good; the positioning methods such as ultrasonic positioning, optical positioning, infrared positioning, wireless positioning and the like need to establish a wireless sensing network in advance, arrange positioning facilities, cause the signals to be easily interfered and the like, so that the method is expensive, convenient and poor in universality.
The prior art has certain limitation, is influenced by key factors such as networks, signals, errors and the like, and particularly has insufficient positioning accuracy in a closed environment and places with poor GPS signals such as indoor places, forests, underground places and the like, so that rescue delay is often caused, the situation is worsened, and irrecoverable tragedies occur, and therefore a method capable of accurately positioning in the closed environment, the environment with weak GPS signals and the environment with weak network signals is needed to be provided.
Disclosure of Invention
The invention overcomes the defects in the prior art, and solves the technical problems that: the improvement of the method for accurately positioning the three-dimensional position in the underground, indoor and closed space environment with poor GPS signals and weak network signals is provided.
In order to solve the technical problems, the invention adopts the technical scheme that: the method comprises the following steps:
the method comprises the following steps: acquiring data information acquired by an MEMS sensor in a three-dimensional positioning system worn by a positioning target by using a central controller, wherein the data information comprises data acquired by a magnetometer, a gyroscope, an accelerometer, a barometric altimeter, an airspeed meter and a GPS (global positioning system);
step two: the central controller carries out respective error compensation on the data acquired by the magnetometer, the gyroscope, the accelerometer and the barometric altimeter acquired in the step one to obtain an observed value;
step three: inputting observed values of a magnetometer, a gyroscope and an accelerometer into an attitude measurement module for data processing, wherein when the attitude measurement module processes data, an extended Kalman filtering algorithm is adopted to carry out error correction on a processed attitude angle error and an attitude angle obtained by a quaternion method to obtain real-time attitude information of a positioning target, and the real-time attitude information of the positioning target is input into a central controller through the attitude measurement module;
step four: inputting data collected by an airspeed meter and a GPS (global positioning system) into a navigation measurement and control module for data processing, resolving an observed value of an air pressure altimeter through air pressure temperature and obtaining a height value of a positioning target after error compensation, entering an extended Kalman filter for resolving the observed value of a magnetometer, a gyroscope and an accelerometer after attitude resolution, static accurate measurement, zero-position angular velocity updating and zero-position velocity updating compensation, and performing error compensation on sensor data by adopting a 15-element error state vector navigation algorithm when the central processing unit processes the data, wherein the error compensation comprises an attitude error, an angular velocity error, a position error, a velocity error and an acceleration error to obtain three-dimensional position, attitude and velocity real-time information of the positioning target, and the information of the positioning target is input into a central controller;
step five: the central controller outputs real-time position information, attitude information and speed information of the positioning target and transmits the information to the remote control center through the wireless communication module.
The method for performing error compensation on the data acquired by the magnetometer in the second step comprises hard magnetic interference error compensation and soft magnetic interference error compensation, wherein the soft magnetic interference error compensation is calculated according to the following formula:
Figure BDA0002619874050000021
in the above formula, (m)x,my) Is magnetometer data not subjected to magnetic field interference, (m'x,m′y) For magnetometer data after error compensation, alpha is an included angle between a long axis of an ellipse changed by soft magnetic interference and a horizontal straight line, and h is a proportional coefficient corresponding to mx and my when the graph is a standard circle.
The method for performing error compensation on the data acquired by the gyroscope in the second step is drift compensation on the gyroscope, and the specific method for the drift compensation is as follows:
step 3.1: the navigation measurement and control module is horizontally and statically placed in an interference-free environment, and the central controller acquires and stores sensor data in the navigation measurement and control module;
step 3.2: set at a temperature T1Then, the zero value of the gyroscope is obtained as S01At a temperature T2Then, the zero value of the gyroscope is obtained as S02Get it
Figure BDA0002619874050000022
The zero drift value of each gyroscope and the change value of the temperature have the following proportional relation:
ΔS＝ΔT·c+e，
in the above formula: e is the residual error of the equation, and c is the proportional coefficient of the equation;
the data of the two sets of gyroscopes with respect to zero drift and temperature variation are calculated according to the following formula:
Figure BDA0002619874050000031
finally, the least square method is adopted for calculation to obtain
c＝(TT·T)-1·TTS, in the above formula: delta S0(n) is the zero difference of the gyroscope at the time of sampling at the nth time, delta T0And (n) is the temperature zero difference value at the nth sampling.
The method for performing error compensation on the data acquired by the accelerometer in the second step is specifically realized by a method for calibrating the zero offset and the scale coefficient error of the accelerometer, and comprises the following calibration steps:
step 4.1: mounting the accelerometer on a rotating turntable, wherein a rotating shaft of the turntable is horizontal and vertical to a sensitive axis of the accelerometer;
step 4.2: controlling the rotating turntable to rotate at a constant speed at an angular rate omega, so that a sensitive shaft of the accelerometer 4 rotates in a plumb surface; step 4.3: the input of the components of the gravitational acceleration at different positions is represented as:
Figure BDA0002619874050000039
the output of the accelerometer is represented as:
Figure BDA0002619874050000032
general formula
Figure BDA0002619874050000038
Carry-in type
Figure BDA0002619874050000033
The following can be obtained:
Figure BDA0002619874050000034
in the above formula: g is gravity acceleration, theta is the output of a rotation angle sensor of the rotary turntable, Aout is the signal output of an accelerometer, k0Is the accelerometer zero offset, k1Is a scale factor, k2Is a quadratic non-linear error coefficient, k, of the scale factor3Is a cubic non-linear error coefficient of scale coefficients, wherein k is estimated using a recursive least squares method0、k1、k2、k3The value of (c).
The specific method for performing error compensation on the data acquired by the barometric altimeter in the step two is as follows:
the relationship between the atmospheric pressure P and the standard barometric altitude H at different altitudes, ranging from sea level H-0 m to H-11000 m, is defined as:
Figure BDA0002619874050000035
in the above formula P0Atmospheric pressure at sea level, T0Is the atmospheric temperature at sea level, beta1Is a temperature gradient, beta10.0065K/m, gn is gravity acceleration;
the compensated temperature value Temp is calculated according to the following formula,
Figure BDA0002619874050000036
in the above formula: d2Is a temperature value, C5As a reference temperature value, C6To measure the temperature coefficient;
and performing nonlinear compensation on the temperature value Temp after temperature compensation, specifically performing secondary correction on the temperature value and the pressure value through a second-order correction coefficient, wherein the calculation formula is as follows:
Figure BDA0002619874050000041
in the above formula: t is2As a temperature compensation constant, C1Is a pressure sensitivity value, C2Is a pressure offset value, C3Temperature coefficient of pressure sensitivity, C4Is a pressure-biased temperature coefficient, C5As a reference temperature value, C6For measuring temperature coefficient, D1Is a pressure value, D2As temperature value, TCS is pressure-sensitive temperature coefficient, SENS2For temperature compensation of sensitivity, OFF2The error is compensated for temperature.
In the third step, the error correction of the processed attitude angle error and the attitude angle obtained by the quaternion method by adopting an extended Kalman filtering algorithm comprises correcting an integral angle error of a gyroscope through data of an accelerometer and a magnetometer, and resetting and compensating static gait detection of a positioning target through logical calculation of the accelerometer and the gyroscope, wherein the specific steps of the static gait detection are as follows:
step 6.1: sending acceleration data collected by an accelerometer and angular velocity data collected by a gyroscope to an attitude measurement module as input signals for static gait detection;
step 6.2: the attitude measurement module sends a logic value obtained by taking the X-axis acceleration signal and the Y-axis angular velocity signal through a logical AND relationship to a central controller as a basis for judging whether a positioning target is static or dynamic, and the specific judgment steps of the logic value are as follows:
definition C1For the logic threshold of the X-axis acceleration signal, define C2Is a logic threshold of the Y-axis angular velocity signal, wherein
Figure BDA0002619874050000042
C＝C1&C2Wherein C is the static gait detection logic value.
The error state vector navigation algorithm of 15 elements in step four comprises the following steps:
step 7.1: let 15 element error state vector be:
Figure BDA0002619874050000043
in the above formula
Figure BDA0002619874050000044
Is the three-dimensional attitude error at time k,
Figure BDA0002619874050000045
is the angular rate error at time k,rkis the position error at the time k,vkis the speed error at the time k and,
Figure BDA0002619874050000046
the acceleration error at the moment k;
step 7.2: will be a formula
Figure BDA0002619874050000047
The state equation obtained after linearization is:
Xk/k-1＝ΦkXk-1/k-1+wk-1，
wherein, Xk/k-1Prediction error value, Xk-1/k-1Is an optimal estimated value, w, of k-1 time Kalman filteringk-1Is variance of Qk＝E(wkwk T) Process noise of phikIs a state transition matrix of 15 x 15,
Φkthe formula of (c) is shown as follows:
Figure BDA0002619874050000051
in the above formula
Figure BDA0002619874050000052
Figure BDA0002619874050000053
For the output of the accelerometer after error compensation after coordinate transformation,
Figure BDA0002619874050000054
is based on a diagonally symmetric matrix of accelerometer measurements,
Figure BDA0002619874050000055
the calculation formula of (a) is as follows:
Figure BDA0002619874050000056
wherein: zkIs an error measurement, H is a measurement matrix, Xk/kFor filtering error estimates at time k, nkIs a variance of Rk＝E(nknk T) The noise of the measurement of (2) is,
the calculation formula of the measurement matrix H is as follows:
Figure BDA0002619874050000057
the observation vector corresponding to the measurement matrix H is mk，mkThe calculation formula of (2) is as follows:
mk＝[ΔAk,Δωk,Δrzk,Δvk]，
wherein: delta AkIs a three-dimensional attitude angle ofkFor three-dimensional angular velocity values, Δ rzkAs height value,. DELTA.vkIs a three-dimensional velocity value; said Xk/kDerived from the previous moment by means of the Kalman filter state update equation, Xk/kThe calculation formula of (2) is as follows:
Xk/k＝Xk/k-1+Kk·[mk-HXk/k-1]，
wherein KkIs the Kalman filter gain, mkFor actual error measurements, Xk/k-1For one-step prediction of error state values, Kalman filter gain KkCalculated by the following formula:
Kk＝Pk/k-1HT(HPk/k-1HT+Rk)-1，
wherein: pk/k-1Estimating a covariance matrix for the error, calculated from the measurements at time k-1, said Pk/k-1The calculation formula of (2) is as follows:
Pk/k-1＝Φk-1Pk-1/k-1Φk-1 T+Qk-1；
step 7.4: obtaining a covariance matrix P according to the state equation obtained after linearization in the steps 7.1 and 7.2k/kThe calculation formula of (2) is as follows:
Pk/k＝(I15×15-KkH)Pk/k-1(I15×15-KkH)T+Rk，
wherein: i is15×1515 x 15 matrix.
Step 7.5: after determining the current speed, attitude and position information of the positioning target, the 15-element error state vector X is determinedk/kAnd setting 0, and continuously updating the state of the system by the observed quantity of the three-dimensional positioning system according to a Kalman filtering equation so as to obtain the real-time three-dimensional information of the positioning target.
The device comprises an annular shell worn on a positioning target, wherein a circuit control board, an attitude measurement module and a navigation measurement and control module are arranged in the shell, a central controller is integrated on the circuit control board, the attitude measurement module comprises a magnetometer, a gyroscope and an accelerometer, and the navigation measurement and control module comprises an air pressure altimeter, an airspeed meter and a GPS;
the central controller is respectively connected with the clock module, the wireless communication module, the AD conversion module, the data storage module and the power management module through wires;
the AD conversion module is respectively connected with the magnetometer, the gyroscope, the accelerometer, the barometric altimeter, the airspeed meter and the GPS through wires;
the casing outside is provided with SD card interface, USB interface, still be provided with the display screen on the casing, the display screen passes through the wire and is connected with central controller.
The wireless communication module specifically comprises a short-distance wireless module and a long-distance wireless module, wherein the model of the short-distance wireless module is nRF24L01, and the model of the long-distance wireless module is Xtend.
Compared with the prior art, the invention has the beneficial effects that: according to the invention, signals of an accelerometer, a gyroscope, a magnetometer and a barometric pressure gauge are collected, on the basis of error compensation, displacement is obtained by utilizing acceleration twice integration, the gyroscope, the accelerometer and the magnetometer are subjected to data fusion to solve attitude, static gait detection adopts logic AND judgment of the acceleration signal and an angular velocity signal, altitude information is obtained by filtering and fusing barometric pressure altitude information and acceleration altitude, the characteristic of high accuracy of the barometric pressure gauge in a short time is exerted, and under the support of a Kalman filtering algorithm, the positioning accuracy of a three-dimensional positioning target in the environments with poor GPS signals and weak network signals, such as underground, indoor and closed spaces, is greatly improved.
Drawings
The invention is further described below with reference to the accompanying drawings:
FIG. 1 is a schematic diagram of a fuzzy Kalman attitude measurement algorithm of the present invention;
FIG. 2 is an algorithmic flow chart of the three-dimensional positioning method of the present invention;
FIG. 3 is a schematic diagram of a three-dimensional positioning system according to the present invention;
FIG. 4 is a schematic circuit control diagram of the three-dimensional positioning system of the present invention;
FIG. 5 is a schematic circuit diagram of a three-dimensional positioning system according to the present invention;
fig. 6 is a diagram illustrating the results of a static gait detection experiment in an embodiment of the invention.
In the figure: the system comprises a central controller 1, a magnetometer 2, a gyroscope 3, an accelerometer 4, an air pressure altimeter 5, an airspeed meter 6, a GPS7, a clock module 8, a wireless communication module 9, an AD conversion module 10, a data storage module 11, a power management module 12 and a display screen 13.
Detailed Description
As shown in fig. 1 to 6, the three-dimensional positioning method based on the kalman filter algorithm of the present invention includes the following steps:
the method comprises the following steps: acquiring data information acquired by an MEMS sensor in a three-dimensional positioning system worn by a positioning target by using a central controller 1, wherein the data information comprises data acquired by a magnetometer 2, a gyroscope 3, an accelerometer 4, an air pressure altimeter 5, an airspeed meter 6 and a GPS 7;
step two: the central controller 1 performs respective error compensation on the data acquired by the magnetometer 2, the gyroscope 3, the accelerometer 4 and the barometric altimeter 5 acquired in the step one to obtain an observed value;
step three: inputting observed values of a magnetometer 2, a gyroscope 3 and an accelerometer 4 into an attitude measurement module for data processing, wherein when the attitude measurement module processes data, an extended Kalman filtering algorithm is adopted to carry out error correction on a processed attitude angle error and an attitude angle obtained by a quaternion method to obtain real-time attitude information of a positioning target, and the real-time attitude information of the positioning target is input into a central controller 1 through the attitude measurement module;
step four: inputting data collected by an airspeed meter 6 and a GPS7 into a navigation measurement and control module for data processing, resolving an observed value of an air pressure altimeter 5 through air pressure temperature and obtaining a height value of a positioning target after error compensation, entering an extended Kalman filter for resolving the observed values of a magnetometer 2, a gyroscope 3 and an accelerometer 4 after attitude resolution, static accurate measurement, zero angular velocity updating and zero velocity updating compensation, and obtaining three-dimensional position, attitude and velocity real-time information of the positioning target by adopting a 15-element error state vector navigation algorithm to carry out error compensation on sensor data when the central processor 1 processes the data, wherein the error compensation comprises an attitude error, an angular velocity error, a position error, a velocity error and an acceleration error, and the information of the positioning target is input into the central controller 1;
step five: the central controller 1 outputs real-time position information, attitude information and speed information of the positioning target and transmits to the remote control center through the wireless communication module 9.
The method for performing error compensation on the data acquired by the magnetometer 2 in the second step comprises hard magnetic interference error compensation and soft magnetic interference error compensation, wherein the soft magnetic interference error compensation is calculated according to the following formula:
Figure BDA0002619874050000071
in the above formula, (m)x,my) Is magnetometer data not subjected to magnetic field interference, (m'x,m′y) For magnetometer data after error compensation, alpha is an included angle between a long axis of an ellipse changed by soft magnetic interference and a horizontal straight line, and h is a proportional coefficient corresponding to mx and my when the graph is a standard circle.
The method for performing error compensation on the data acquired by the gyroscope 3 in the step two is drift compensation on the gyroscope, and the specific method for the drift compensation is as follows:
step 3.1: the navigation measurement and control module is horizontally and statically placed in an interference-free environment, and the central controller 1 acquires and stores sensor data in the navigation measurement and control module;
step 3.2: set at a temperature T1Then, the zero value of the gyroscope is obtained as S01At a temperature T2Then, the zero value of the gyroscope is obtained as S02Get it
Figure BDA0002619874050000081
The zero drift value of each gyroscope and the change value of the temperature have the following proportional relation:
ΔS＝ΔT·c+e，
in the above formula: e is the residual error of the equation, and c is the proportional coefficient of the equation;
the data of the two sets of gyroscopes with respect to zero drift and temperature variation are calculated according to the following formula:
Figure BDA0002619874050000082
finally, the least square method is adopted for calculation to obtain
c＝(TT·T)-1·TTS, in the above formula: delta S0(n) is the zero difference of the gyroscope at the time of sampling at the nth time, delta T0And (n) is the temperature zero difference value at the nth sampling.
The method for performing error compensation on the data acquired by the accelerometer 4 in the second step is specifically realized by a method for calibrating the zero offset and the scale coefficient error of the accelerometer 4, and comprises the following calibration steps:
step 4.1: mounting the accelerometer 4 on a rotating turret, wherein the axis of rotation of the turret is horizontal and perpendicular to the axis of sensitivity of the accelerometer;
step 4.2: controlling the rotating turntable to rotate at a constant speed at an angular rate omega, so that a sensitive shaft of the accelerometer 4 rotates in a plumb surface;
step 4.3: the input of the components of the gravitational acceleration at different positions is represented as:
Figure BDA0002619874050000088
the output of the accelerometer is represented as:
Figure BDA0002619874050000083
general formula
Figure BDA0002619874050000089
Carry-in type
Figure BDA0002619874050000084
The following can be obtained:
Figure BDA0002619874050000085
in the above formula: g is gravity acceleration, theta is the output of a rotation angle sensor of the rotary turntable, Aout is the signal output of an accelerometer, k0Is the accelerometer zero offset, k1Is a scale factor, k2Is a quadratic non-linear error coefficient, k, of the scale factor3Is a cubic non-linear error coefficient of scale coefficients, wherein k is estimated using a recursive least squares method0、k1、k2、k3The value of (c).
The specific method for performing error compensation on the data acquired by the barometric altimeter 5 in the step two is as follows:
the relationship between the atmospheric pressure P and the standard barometric altitude H at different altitudes, ranging from sea level H-0 m to H-11000 m, is defined as:
Figure BDA0002619874050000091
in the above formula P0Atmospheric pressure at sea level, T0Is the atmospheric temperature at sea level, beta1Is a temperature gradient, beta10.0065K/m, gn is gravity acceleration;
the compensated temperature value Temp is calculated according to the following formula,
Figure BDA0002619874050000092
in the above formula: d2Is a temperature value, C5Is prepared from radix GinsengExamination temperature value, C6To measure the temperature coefficient;
and performing nonlinear compensation on the temperature value Temp after temperature compensation, specifically performing secondary correction on the temperature value and the pressure value through a second-order correction coefficient, wherein the calculation formula is as follows:
Figure BDA0002619874050000093
in the above formula: t is2As a temperature compensation constant, C1Is a pressure sensitivity value, C2Is a pressure offset value, C3Temperature coefficient of pressure sensitivity, C4Is a pressure-biased temperature coefficient, C5As a reference temperature value, C6For measuring temperature coefficient, D1Is a pressure value, D2As temperature value, TCS is pressure-sensitive temperature coefficient, SENS2For temperature compensation of sensitivity, OFF2The error is compensated for temperature.
In the third step, the error correction of the processed attitude angle error and the attitude angle obtained by the quaternion method by adopting the extended Kalman filtering algorithm comprises the steps of correcting the integral angle error of a gyroscope 3 through data of an accelerometer 4 and a magnetometer 2, and resetting and compensating the static gait detection of the positioning target through logic calculation of the accelerometer 4 and the gyroscope 3, wherein the specific steps of the static gait detection are as follows:
step 6.1: sending acceleration data collected by the accelerometer 4 and angular velocity data collected by the gyroscope 3 to the attitude measurement module as input signals for static gait detection;
step 6.2: the attitude measurement module sends a logic value of the X-axis acceleration signal and the Y-axis angular velocity signal after passing through the logical AND relationship to the central controller 1 as a basis for judging whether the positioning target is in a static state or a dynamic state, and the specific judgment steps of the logic value are as follows:
definition C1For the logic threshold of the X-axis acceleration signal, define C2Is a logic threshold of the Y-axis angular velocity signal, wherein
Figure BDA0002619874050000094
C＝C1&C2Wherein C is the static gait detection logic value.
The error state vector navigation algorithm of 15 elements in step four comprises the following steps:
step 7.1: let 15 element error state vector be:
Figure BDA0002619874050000101
in the above formula
Figure BDA0002619874050000102
Is the three-dimensional attitude error at time k,
Figure BDA0002619874050000103
is the angular rate error at time k,rkis the position error at the time k,vkis the speed error at the time k and,
Figure BDA0002619874050000104
the acceleration error at the moment k;
step 7.2: will be a formula
Figure BDA0002619874050000105
The state equation obtained after linearization is:
Xk/k-1＝ΦkXk-1/k-1+wk-1，
wherein, Xk/k-1Is the prediction error value, Xk-1/k-1Is an optimal estimated value, w, of k-1 time Kalman filteringk-1Is variance of Qk＝E(wkwk T) Process noise of phikIs a state transition matrix of 15 x 15,
Φkthe formula of (c) is shown as follows:
Figure BDA0002619874050000106
in the above formula
Figure BDA0002619874050000107
Figure BDA0002619874050000108
For the output of the accelerometer after error compensation after coordinate transformation,
Figure BDA0002619874050000109
is based on a diagonally symmetric matrix of accelerometer measurements,
Figure BDA00026198740500001010
the calculation formula of (a) is as follows:
Figure BDA00026198740500001011
Figure BDA00026198740500001012
wherein: zkIs an error measurement, H is a measurement matrix, Xk/kFor filtering error estimates at time k, nkIs a variance of Rk＝E(nknk T) The noise of the measurement of (2) is,
the calculation formula of the measurement matrix H is as follows:
Figure BDA00026198740500001013
the observation vector corresponding to the measurement matrix H is mk，mkThe calculation formula of (2) is as follows:
mk＝[ΔAk,Δωk,Δrzk,Δvk]，
wherein: delta AkFor three-dimensional attitude angle values, Δ ωkFor three-dimensional angular velocity values, Δ rzkAs height value,. DELTA.vkIs a three-dimensional velocity value;
said Xk/kDerived from the previous moment by means of the Kalman filter state update equation, Xk/kMeter (2)The calculation formula is as follows:
Xk/k＝Xk/k-1+Kk·[mk-HXk/k-1]，
wherein KkIs the Kalman filter gain, mkFor actual error measurements, Xk/k-1For one-step prediction of error state values, Kalman filter gain KkCalculated by the following formula:
Kk＝Pk/k-1HT(HPk/k-1HT+Rk)-1，
wherein: pk/k-1Estimating a covariance matrix for the error, calculated from the measurements at time k-1, said Pk/k-1The calculation formula of (2) is as follows:
Pk/k-1＝Φk-1Pk-1/k-1Φk-1T+Qk-1；
step 7.4: obtaining a covariance matrix P according to the state equation obtained after linearization in the steps 7.1 and 7.2k/kThe calculation formula of (2) is as follows:
Pk/k＝(I15×15-KkH)Pk/k-1(I15×15-KkH)T+Rk，
wherein: i is15×1515 x 15 matrix.
Step 7.5: after determining the current speed, attitude and position information of the positioning target, the 15-element error state vector X is determinedk/kAnd setting 0, and continuously updating the state of the system by the observed quantity of the three-dimensional positioning system according to a Kalman filtering equation so as to obtain the real-time three-dimensional information of the positioning target.
The device comprises an annular shell worn on a positioning target, wherein a circuit control board, an attitude measurement module and a navigation measurement and control module are arranged in the shell, a central controller 1 is integrated on the circuit control board, the attitude measurement module comprises a magnetometer 2, a gyroscope 3 and an accelerometer 4, and the navigation measurement and control module comprises an air pressure altimeter 5, an airspeed meter 6 and a GPS 7;
the central controller 1 is respectively connected with the clock module 8, the wireless communication module 9, the AD conversion module 10, the data storage module 11 and the power management module 12 through leads;
the AD conversion module 10 is respectively connected with the magnetometer 2, the gyroscope 3, the accelerometer 4, the barometric altimeter 5, the airspeed meter 6 and the GPS7 through leads;
the casing outside is provided with SD card interface, USB interface, still be provided with display screen 13 on the casing, display screen 13 passes through the wire and is connected with central controller 1.
The wireless communication module 9 specifically includes a short-range wireless module and a long-range wireless module, the model of the short-range wireless module is nRF24L01, and the model of the long-range wireless module is Xtend.
The three-dimensional positioning method is suitable for being used in scenes with weak GPS signals such as indoor, forest, underground and the like, realizes the accurate positioning of the three-dimensional position of the positioning target by wearing the three-dimensional positioning system on the positioning target, and simultaneously overcomes the defects of the following technologies:
1) the MEMS sensor device error is the most main error source of an indoor positioning navigation system, the low-cost MEMS sensor has serious drift error, if the sensor error is not corrected, the position and the attitude after integration are seriously interfered and even unusable, and in order to ensure the output precision of the system, the error compensation needs to be carried out on the used inertial device before the calculation;
2) the reliability and the accuracy of the positioning system are guaranteed, the movement of a person in the walking process is periodic, the movement type of the person is basically determined, each step is a periodic speed change process in the plane direction, and the process of starting from rest, accelerating movement, decelerating movement and ending at rest; each step can be divided into two states: static state and dynamic state, in the static state, feet are all or partially on the ground, the static state is reliably and accurately detected, and the speed of each step in the static state can be reset, so that accumulated errors are reduced, the subsequent data processing is facilitated, and the positioning precision is improved;
3) the MEMS sensor is used for positioning and navigation, and the problems that attitude errors and positioning errors are increased along with time and distance need to be solved by selecting a proper algorithm, and the observation noise matrix of the algorithm is adjusted to be infinite, so that the influence of observation information on a measurement result is reduced.
The positioning system is provided with a magnetometer 2, a gyroscope 3, an accelerometer 4, an air pressure altimeter 5, an airspeed meter 6 and a GPS7, wherein the magnetometer 2 adopts a three-axis magnetometer, the gyroscope 3 adopts a three-axis gyroscope, and the accelerometer 4 adopts a three-axis accelerometer, the three-dimensional positioning method specifically comprises the following steps:
the method comprises the following steps: acquiring measurement values of a magnetometer 2, a gyroscope 3, an accelerometer 4 and an air pressure altimeter 5 for positioning a target as input quantities;
step two: before resolving, each input quantity is subjected to error compensation through different compensation methods, wherein data of the magnetometer 2 is subjected to error compensation through a magnetic error method, data of the gyroscope 3 is subjected to error compensation through a temperature drift method, data of the accelerometer 4 is subjected to error compensation through a zero offset and coefficient error calibration method, and data of the barometric altimeter 5 is subjected to error compensation through a calibration parameter temperature compensation method;
step three: resolving and fusing data after error compensation of the magnetometer 2, the gyroscope 3 and the accelerometer 4 to obtain real-time attitude information of a positioning target, resolving the data of the gyroscope after error compensation and the accelerometer after gravity acceleration removal, and obtaining the resolved attitude information, zero-position velocity update correction information and zero-position angular velocity update information through a static detection method;
step four: performing data fusion on the height value of the accelerometer and the height data of the barometric altimeter to obtain real-time height information of the positioning target;
step five: and (3) constructing a 15-element error state vector to improve the positioning precision of the navigation measurement and control module.
In the embodiment, the MEMS sensor is adopted to acquire the three-dimensional attitude, speed and position information of the positioning target, the precision of the MEMS sensor is low, the error is large, and the information processing capability of the micro processor is poor.
(1) And (3) accelerometer error compensation:
the accelerometer generally has larger zero offset and scale coefficient errors which directly cause the indoor navigation positioning resolving precision, and the invention provides a calibration method based on a small-sized rotating mechanism, which can accurately and quickly calibrate the zero offset and scale coefficient errors of an MEMS device; the calibration steps of the method are as follows:
the output of an inertial accelerometer can be expressed as:
Figure BDA0002619874050000131
in the above formula: aout is the signal output of the digital accelerometer, k0Is the accelerometer zero offset, k1Is a scale factor, k2,k3The second and third non-linear error coefficients of the scale coefficient are respectively.
In this embodiment, the accelerometer 4 is mounted on a rotating turntable, a rotating shaft of the turntable is horizontal and vertical to a sensitive axis of the accelerometer to be tested, the rotating mechanism is controlled by a control platform of the rotating mechanism to rotate at an angular rate at a uniform speed, the sensitive axis of the accelerometer rotates in a vertical plane, components of the gravity acceleration at different positions are tested and recorded, and the input of the components can represent the components
Figure BDA0002619874050000134
The formula (1-2) is brought into the formula (1-1) to obtain:
Figure BDA0002619874050000132
in the above formula: g is that the gravity acceleration is known, theta is the output of a rotation angle sensor of the rotation mechanism, rotation angle information theta i recorded in real time is used as input, an accelerometer sampling value Aout is used as output, and k can be estimated by using a recursive least square method0，k1，k2And k3The value of (c).
(2) And (3) gyroscope drift compensation:
the gyroscope is a passive device and is sensitive to external influence, and for a low-cost gyroscope, the gyroscope has the advantages of high short-time precision, large drift error if the gyroscope is not used, effective estimation and compensation are carried out on the drift error, the long-time precision and reliability of the gyroscope can be greatly improved, and the robustness and stability of a system are enhanced.
The zero point temperature drift refers to the zero point value S of the sensor0Changes with changes in temperature. At different temperatures, S0And the calculated actual value of the sensor has errors compared with the actual value, so that the final measurement precision is influenced. In the embodiment, the navigation measurement and control module is horizontally and statically placed in an interference-free environment, and data of about 10 minutes including various sensor data and temperature sensor data are recorded; ideally, the angular velocity data calculated from the output of the various sensors should be 0/s.
Set at a temperature T1Then, the zero value of the sensor is S01At a temperature T2Then, the zero value of the sensor is S02. Get
Figure BDA0002619874050000133
According to experimental data, the zero drift value and the change value of the temperature of each sensor can be approximately considered to have a proportional relationship, that is, the following formula exists:
ΔS0＝ΔT·c+e, (1-5)，
wherein: e is the residual of the equation and c is the scaling factor of the equation.
Data on temperature change and zero drift for two sets of sensors in the experiment:
Figure BDA0002619874050000141
resolving by using a least square method to obtain c ═ T ═T·T)-1·TT·S (1-7)。
(3) Magnetometer error:
the property that the earth's magnetic field always points to the magnetic north pole parallel to the ground plane allows the magnetic sensor to be used to resolve heading. When the magnetic sensor is used for resolving the magnetic heading angle, the proportional relation among the three-axis magnetic field data is mainly considered, and the data do not need to be converted into data with dimensions. However, in use, magnetic sensors are subject to interference from the surrounding magnetic environment, which is classified into hard magnetic interference and soft magnetic interference:
after the fixed magnetic field interference is superposed with the earth magnetic field, a constant bias is generated for the magnetic heading angle, and the magnetic heading angle obtained finally can be eliminated by direct correction. Hard iron interferes with the influence of the magnetic field and soft iron interferes with the influence of the magnetic field. Uniformly rotating the magnetic sensor for one circle in a horizontal plane when the magnetic sensor is not interfered by a magnetic field to obtain magnetic sensor data (m)x,my) The track is a circle with the center at (0, 0); when the magnetic field interference of hard iron is applied, the circle center of the circular track deviates (0,0), and the circular track changes into an ellipse when the magnetic field interference of soft iron is applied.
The calibration of the hard iron interference magnetic field only needs to obtain the deviated circle center, which can be regarded as a zero point value of the calibration magnetic sensor; the calibration of the soft iron interference magnetic field is complex, and an ellipse rotating shaft deflection angle is needed to be firstly changed into a circle, and finally the shaft deflection angle is reversely rotated. The difference with the accelerometer calibration is that when the index head is used, in order to avoid the interference of the steel index head on the magnetic field of the magnetometer, the carrier attitude measurement module is fixed on a pure aluminum heightening device.
To compensate for the soft iron interfering magnetic field, the ellipse is transformed into a circle. An included angle between the long axis of the ellipse changed by the soft magnetic interference and the horizontal straight line is set as alpha, and the following compensation formula is obtained through geometric transformation analysis:
Figure BDA0002619874050000142
in the above formula: (m)x,my) Is magnetometer data not subjected to magnetic field interference, (m'x,m′y) For error compensated magnetometer data, alpha is the major axis of the ellipse and the horizontal line that become subject to soft magnetic interferenceAnd the included angle h is a proportionality coefficient corresponding to mx and my when the graph is a standard circle.
(4) And (3) barometer error compensation:
the pressure sensor utilizes the measurement principle that atmospheric pressure changes along with altitude, and defines the concept of 'international standard atmosphere' by taking the average value of atmospheric data on latitude in northern hemisphere in summer as the standard internationally. The law of atmospheric pressure variation with altitude is the theoretical basis of a barometric altimeter. From theoretical derivation and meteorological data, we derive: in the range from sea level (H ═ 0m) to H ═ 11000m, the relationship between the atmospheric pressure P and the standard barometric altitude H there is as follows:
Figure BDA0002619874050000143
in the above formula, P0Atmospheric pressure at sea level, T0Is the atmospheric temperature (K), beta, of sea level1Is a temperature gradient, beta10.0065K/m, gn is the acceleration of gravity.
The 6 calibration parameters mainly comprise factory check data C1(pressure sensitivity SENST)1)、C2(pressure offset OFFT)1)、 C3(temperature coefficient of pressure sensitivity TCS), C4(pressure deflection temperature coefficient TC)O)、C5(reference temperatures TREF) and C6(measured temperature coefficient TEMPSENS), pressure data D1And temperature data D2. The compensated temperature value Temp is calculated as follows:
Figure BDA0002619874050000151
on the basis of the temperature compensation, in order to obtain the optimal precision in the full temperature range, the nonlinearity of the temperature is compensated, the temperature value and the pressure value are corrected again through a second-order correction coefficient, and different compensation modes are adopted in different temperature areas;
wherein the high temperature compensation coefficient is:
T2＝0
OFF2＝0
SENS2＝0，
the low-temperature compensation coefficient is as follows:
Figure BDA0002619874050000152
Figure BDA0002619874050000153
SENS2＝2(Temp-2000)2，
the temperature and pressure calculations after the final second-order temperature compensation are as follows:
Temp＝Temp-T2
Figure BDA0002619874050000154
in the above formula: t is2As a temperature compensation constant, C1Is a pressure sensitivity value, C2Is a pressure offset value, C3Temperature coefficient of pressure sensitivity, C4Is a pressure-biased temperature coefficient, C5As a reference temperature value, C6For measuring temperature coefficient, D1Is a pressure value, D2As temperature value, TCS is pressure-sensitive temperature coefficient, SENS2For temperature compensation of sensitivity, OFF2The error is compensated for temperature.
Aiming at the problems that the traditional MEMS sensor is used for positioning, and the error is increased along with the time and the distance, the invention provides a quaternion extended Kalman filtering positioning algorithm to realize self correction, corrects the integral angle error of a gyroscope by using data of an accelerometer and a magnetometer, realizes effective static gait detection by using logical calculation of the accelerometer and the gyroscope to reset and compensate, and improves the detection precision.
The sensor signal is periodic during human walking, static being the time period during which the signal waveform tends to be stationary, and dynamic being the time period during which the signal waveform is unstable. The periodicity of the acceleration and the angular velocity is obvious, and the acceleration and the angular velocity are selected as the signal input of the static detection in the three-dimensional positioning method. The acceleration of the X-axis is the most stable detection signal, and the angular velocity of the Y-axis is the most significant detection signal. When the three-dimensional positioning system is installed on a foot of a tester, absolute level cannot be guaranteed, and the horizontal output and the vertical output of the accelerometer are not absolute 0 and g, so that the accelerometer cannot be used as a gait detection signal alone; in the walking process of a tester, the Y-axis angular velocity signal has a shuttle phenomenon at about 0 value, and gait detection information obtained by independently using the Y-axis angular velocity as a gait detection signal is not credible. The invention provides that the X-axis acceleration signal and the Y-axis angular velocity signal are jointly used as gait detection signals through the logical AND relationship, and the gait detection signals are considered to be static state of a tester in walking only when two logical relations are established simultaneously, so that the gait detection effectiveness is greatly improved.
The specific static gait detection method comprises the following steps: defining two logic values C for detecting states1And C2As follows:
(1) the absolute value of the acceleration of the X axis must be lower than a threshold value, which can be obtained by experiments and trials, and the threshold value in this embodiment is set as follows:
Figure BDA0002619874050000161
(2) the absolute value of the Y-axis angular velocity must be below a threshold value to consider the tester as being static, and the threshold value in this embodiment is set as follows:
Figure BDA0002619874050000162
the final result is judged according to the results of the two detections, the static detection considers that the feet of the testee are on the ground only when the two detection states are simultaneously established, so as to obtain the exact static detection result, and the AND logic is adopted to achieve the final effect of effective detection, namely
C＝C1&C2, (1-13)。
As shown in fig. 6, L1 represents the static state after the final logic determination, L2 represents the static state detected according to the acceleration value, L3 represents the static state detected according to the angular velocity value, and after the and logic, the robustness of the static detection result is greatly improved, and at the same time, the actually detected static state time period is smaller than the duty ratio of the dynamic time period, and the experimental result is not very consistent with the actual result, so that the time for a person to land on the ground during walking is smaller than the walking mode of the foot in the air movement time, and the static time during walking is mistakenly considered to be smaller than the dynamic time, which is an effective static state detection, and the detection result rejects the inaccurate static detection result, thereby realizing effective zero setting of the sensor data in the extended kalman filter, finally realizing reliable feedback in the filter, and improving the precision of the position and the orientation.
In order to improve the reliability and accuracy guarantee of a three-dimensional positioning system, the invention provides an improved indoor three-dimensional positioning method, a Kalman filtering algorithm is used, the distance is obtained by twice integration of acceleration, an accelerometer, a gyroscope and a magnetometer output a fusion resolving attitude, the accuracy of gait detection is improved by introducing AND logic of acceleration and angular rate signals, the height of the accelerometer and the height of a barometer are subjected to data fusion resolving, the height is effectively resolved by effectively utilizing the characteristic that the barometric height is high in precision in a short time, and the reliability of accurate positioning is realized.
The navigation algorithm of the invention selects the error state vector of 15 elements to improve the precision of positioning and navigation
Figure BDA0002619874050000171
Including acceleration error at each sample point
Figure BDA0002619874050000172
And angular rate error
Figure BDA0002619874050000173
Three dimensional attitude errorAtSpeed errorVePosition error of the optical diskPo. The Update frequency of the Kalman filter is 100Hz as same as the output frequency of the data of the inertial unit, and zero Velocity Update correction is performed when a static detection algorithm detects detectionWhen the trial leg stays on the ground, the speed at the moment is set to be 0, the error of the speed integral is reduced, the ZARU (zero angular Rate updates) zero-position angular speed is updated, when the static detection algorithm detects that the tester stops, the angular speed is set to be 0, and two judgment AND logics further reduce the attitude error caused by the angular speed integral and improve the attitude accuracy on the premise of the compensation output by the accelerometer and the magnetometer.
Let 15-element error state vector be at time k
Figure BDA0002619874050000174
In the above formula
Figure BDA0002619874050000175
Is the three-dimensional attitude error at time k,
Figure BDA0002619874050000176
is the angular rate error at time k,rkis the position error at the time k,vkis the speed error at the time k and,
Figure BDA0002619874050000177
the acceleration error at the moment k;
the state equation thus linearized is
Xk/k-1＝ΦkXk-1/k-1+wk-1(1-16)，
Wherein, Xk/k-1Is the prediction error value, Xk-1/k-1Is an optimal estimated value, w, of k-1 time Kalman filteringk-1Is variance of Qk＝E(wkwk T) The process noise of (1). PhikIs a 15 x 15 state transition matrix.
Figure BDA0002619874050000178
In the formula
Figure BDA0002619874050000179
Indicating error-compensated accelerometer after coordinate conversionAnd outputting the signals to the computer for output,
Figure BDA00026198740500001710
the method is characterized in that an oblique symmetric matrix measured by an accelerometer is used as an inclinometer to obtain the inclination state of an inertial module;
Figure BDA00026198740500001711
Figure BDA00026198740500001712
the observation equation is as follows:
Zk＝HXk/k+nk(1-20)，
Zkis an error measurement value, H is a measurement matrix, nkIs a variance of Rk＝E(nknk T) Measurement noise of (1), filtering error estimate X at time kk/kThe Kalman filter state update equation is used to derive from the previous time instant:
Xk/k＝Xk/k-1+Kk·[mk-HXk/k-1](1-21)，
Kkis the Kalman filter gain, mkIs the actual error measurement, Xk/k-1Is a one-step prediction error state value. Kalman filter gain KkIs obtained from the formula
Kk＝Pk/k-1HT(HPk/k-1HT+Rk)-1(1-22)，
Wherein P isk/k-1Is an error estimation covariance matrix calculated from the measured value at the time of k-1
Pk/k-1＝Φk-1Pk-1/k-1Φk-1 T+Qk-1(1-23)，
Thereby obtaining a covariance matrix of
Pk/k＝(I15×15-KkH)Pk/k-1(I15×15-KkH)T+Rk(1-24)，
Filtered unbiased error state vector Xk/kAfter the current speed, attitude and position are determined, 0 should be set, and the time variation in the extended Kalman filtering should be the gyroscope error
Figure BDA0002619874050000181
And accelerometer error
Figure BDA0002619874050000182
The state of the system can be continuously updated from observations of the system according to the above kalman filter equations.
In the movement of going upstairs and downstairs, the three-dimensional scene is formed, and the displacement change in the horizontal plane direction and the height change exist. The displacement in the horizontal plane direction is obtained according to the acceleration integral in the horizontal direction, the accelerometer integral in the vertical direction outputs the acceleration height, the barometric altimeter obtains the barometric altitude according to a conversion formula after compensating temperature change, the barometric altitude is used as an observed value of the acceleration height, the precision of estimating the fusion height can be effectively improved, and the height positioning precision is 0.1 m. In this case, the extended kalman filter observation matrix is a matrix of 10 × 15, and includes velocity values, angular velocity values, height values, and attitude angle values.
Figure BDA0002619874050000183
Corresponding observation vector
mk＝[ΔAk,Δωk,Δrzk,Δvk](1-26)。
The three-dimensional positioning method of the invention relies on signals of an accelerometer, a gyroscope, a magnetometer and an altimeter, displacement is obtained by utilizing acceleration twice integration on the basis of error compensation, the gyroscope, the accelerometer and the magnetometer are used for data fusion and resolving attitude, gait detection adopts logic AND judgment of acceleration and angular velocity signals, air pressure height information and acceleration height are obtained by filtering and fusion of the height, the characteristic of high precision of the air pressure altimeter in a short time is exerted, and the three-dimensional positioning effect that the plane distance error in a short period is within 1% of the whole course error and the height error is 0.1 meter is realized under the support of Kalman filtering algorithm.
It should be noted that, regarding the specific structure of the present invention, the connection relationship between the modules adopted in the present invention is determined and can be realized, except for the specific description in the embodiment, the specific connection relationship can bring the corresponding technical effect, and the technical problem proposed by the present invention is solved on the premise of not depending on the execution of the corresponding software program.
Finally, it should be noted that: the above embodiments are only used to illustrate the technical solution of the present invention, and not to limit the same; while the invention has been described in detail and with reference to the foregoing embodiments, it will be understood by those skilled in the art that: the technical solutions described in the foregoing embodiments may still be modified, or some or all of the technical features may be equivalently replaced; and the modifications or the substitutions do not make the essence of the corresponding technical solutions depart from the scope of the technical solutions of the embodiments of the present invention.

Claims (9)

1. A three-dimensional positioning method based on Kalman filtering algorithm is characterized in that: the method comprises the following steps:
the method comprises the following steps: acquiring data information acquired by an MEMS sensor in a three-dimensional positioning system worn by a positioning target by using a central controller (1), wherein the data information comprises data acquired by a magnetometer (2), a gyroscope (3), an accelerometer (4), an air pressure altimeter (5), an airspeed meter (6) and a GPS (7);
step two: the central controller (1) performs respective error compensation on the data acquired by the magnetometer (2), the gyroscope (3), the accelerometer (4) and the barometric altimeter (5) acquired in the step one to obtain an observed value;
step three: inputting observed values of a magnetometer (2), a gyroscope (3) and an accelerometer (4) into an attitude measurement module for data processing, wherein when the attitude measurement module processes data, an extended Kalman filtering algorithm is adopted to carry out error correction on a processed attitude angle error and an attitude angle obtained by a quaternion method to obtain real-time attitude information of a positioning target, and the real-time attitude information of the positioning target is input into a central controller (1) through the attitude measurement module;
step four: inputting data collected by an airspeed meter (6) and a GPS (7) into a navigation measurement and control module for data processing, calculating an observed value of an air pressure altimeter (5) through air pressure temperature and obtaining a height value of a positioning target after error compensation, and entering an extended Kalman filter for calculating the observed values of a magnetometer (2), a gyroscope (3) and an accelerometer (4) after attitude calculation, static accurate measurement, zero angular velocity update and zero velocity update compensation, when the central processing unit (1) processes data, error compensation is carried out on sensor data by adopting a 15-element error state vector navigation algorithm, wherein the error compensation comprises an attitude error, an angular velocity error, a position error, a velocity error and an acceleration error, so as to obtain the three-dimensional position, attitude and velocity real-time information of a positioning target, and the information of the positioning target is input into the central processing unit (1);
step five: the central controller (1) outputs real-time position information, attitude information and speed information of the positioning target and transmits the information to the remote control center through the wireless communication module (9).
2. The three-dimensional positioning method based on the Kalman filtering algorithm according to claim 1, characterized in that: the method for performing error compensation on the data acquired by the magnetometer (2) in the second step comprises hard magnetic interference error compensation and soft magnetic interference error compensation, wherein the soft magnetic interference error compensation is calculated according to the following formula:
Figure FDA0002619874040000011
in the above formula, (m)x,my) Is magnetometer data not subjected to magnetic field interference, (m'x,m′y) For magnetometer data after error compensation, alpha is an included angle between a long axis of an ellipse changed by soft magnetic interference and a horizontal straight line, and h is a proportional coefficient corresponding to mx and my when the graph is a standard circle.
3. The kalman filter algorithm-based three-dimensional positioning method according to claim 2, wherein: the method for performing error compensation on the data acquired by the gyroscope (3) in the step two is drift compensation on the gyroscope, and the specific method for the drift compensation is as follows:
step 3.1: the navigation measurement and control module is horizontally and statically placed in an interference-free environment, and the central controller (1) collects sensor data in the navigation measurement and control module and stores the data;
step 3.2: set at a temperature T1Then, the zero value of the gyroscope is obtained as S01At a temperature T2Then, the zero value of the gyroscope is obtained as S02Get it
Figure FDA0002619874040000021
The zero drift value of each gyroscope and the change value of the temperature have the following proportional relation:
ΔS＝ΔT·c+e，
in the above formula: e is the residual error of the equation, and c is the proportional coefficient of the equation;
the data of the two sets of gyroscopes with respect to zero drift and temperature variation are calculated according to the following formula:
Figure FDA0002619874040000022
finally, the least square method is adopted for calculation to obtain
c＝(TT·T)-1·TTS, in the above formula: delta S0(n) is the zero difference of the gyroscope at the time of sampling at the nth time, delta T0And (n) is the temperature zero difference value at the nth sampling.
4. The Kalman filtering algorithm-based three-dimensional positioning method according to claim 3, characterized in that: the method for performing error compensation on the data acquired by the accelerometer (4) in the second step is specifically realized by a method for calibrating the zero offset and the scale coefficient error of the accelerometer (4), and comprises the following calibration steps:
step 4.1: mounting an accelerometer (4) on a rotating turntable, wherein the rotation axis of the turntable is horizontal and perpendicular to the sensitive axis of the accelerometer;
step 4.2: controlling the rotary turntable to rotate at an angular rate omega at a constant speed, so that a sensitive shaft of the accelerometer (4) rotates in a plumb surface;
step 4.3: the input of the components of the gravitational acceleration at different positions is represented as:
Figure FDA0002619874040000026
the output of the accelerometer is represented as:
Figure FDA0002619874040000023
general formula
Figure FDA0002619874040000027
Carry-in type
Figure FDA0002619874040000024
The following can be obtained:
Figure FDA0002619874040000025
in the above formula: g is gravity acceleration, theta is the output of a rotation angle sensor of the rotary turntable, Aout is the signal output of an accelerometer, k0Is the accelerometer zero offset, k1Is a scale factor, k2Is a quadratic non-linear error coefficient, k, of the scale factor3Is a cubic non-linear error coefficient of scale coefficients, wherein k is estimated using a recursive least squares method0、k1、k2、k3Value of (A)。
5. The Kalman filtering algorithm-based three-dimensional positioning method according to claim 4, characterized in that: the specific method for carrying out error compensation on the data collected by the barometric altimeter (5) in the step two is as follows:
the relationship between the atmospheric pressure P and the standard barometric altitude H at different altitudes, ranging from sea level H-0 m to H-11000 m, is defined as:
Figure FDA0002619874040000031
in the above formula P0Atmospheric pressure at sea level, T0Is the atmospheric temperature at sea level, beta1Is a temperature gradient, beta10.0065K/m, gn is gravity acceleration;
the compensated temperature value Temp is calculated according to the following formula,
Figure FDA0002619874040000032
in the above formula: d2Is a temperature value, C5As a reference temperature value, C6To measure the temperature coefficient;
and performing nonlinear compensation on the temperature value Temp after temperature compensation, specifically performing secondary correction on the temperature value and the pressure value through a second-order correction coefficient, wherein the calculation formula is as follows:
Temp＝Temp-T2
Figure FDA0002619874040000033
in the above formula: t is2As a temperature compensation constant, C1Is a pressure sensitivity value, C2Is a pressure offset value, C3Temperature coefficient of pressure sensitivity, C4Is a pressure-biased temperature coefficient, C5As a reference temperature value, C6For measuring temperature coefficient, D1Is a pressure value, D2As temperature value, TCS is pressure-sensitive temperature coefficient, SENS2For temperature compensation of sensitivity, OFF2The error is compensated for temperature.
6. The Kalman filtering algorithm-based three-dimensional positioning method according to claim 5, characterized in that: in the third step, the error correction of the processed attitude angle error and the attitude angle obtained by the quaternion method by adopting the extended Kalman filtering algorithm comprises the steps of correcting the integral angle error of a gyroscope (3) through data of an accelerometer (4) and a magnetometer (2), and resetting and compensating the static gait detection of the positioning target through logic calculation of the accelerometer (4) and the gyroscope (3), wherein the specific steps of the static gait detection are as follows:
step 6.1: acceleration data collected by the accelerometer (4) and angular velocity data collected by the gyroscope (3) are used as input signals of static gait detection and sent to the attitude measurement module;
step 6.2: the attitude measurement module sends a logic value of the X-axis acceleration signal and the Y-axis angular velocity signal after passing through the logical AND relationship to a central controller (1) as a basis for judging whether a positioning target is in a static state or a dynamic state, and the specific judgment steps of the logic value are as follows:
definition C1For the logic threshold of the X-axis acceleration signal, define C2Is a logic threshold of the Y-axis angular velocity signal, wherein
Figure FDA0002619874040000041
C＝C1&C2Wherein C is the static gait detection logic value.
7. The Kalman filtering algorithm-based three-dimensional positioning method according to claim 6, characterized in that: the error state vector navigation algorithm of 15 elements in step four comprises the following steps:
step 7.1: let 15 element error state vector be:
Figure FDA0002619874040000042
in the above formula
Figure FDA0002619874040000043
Is the three-dimensional attitude error at time k,
Figure FDA0002619874040000044
is the angular rate error at time k,rkis the position error at the time k,vkis the speed error at the time k and,
Figure FDA0002619874040000045
the acceleration error at the moment k;
step 7.2: will be a formula
Figure FDA0002619874040000046
The state equation obtained after linearization is: xk/k-1＝ΦkXk-1/k-1+wk-1Wherein X isk/k-1Is the prediction error value, Xk-1/k-1Is an optimal estimated value, w, of k-1 time Kalman filteringk-1Is variance of Qk＝E(wkwk T) Process noise of phikIs a state transition matrix of 15 x 15,
Φkthe formula of (c) is shown as follows:
Figure FDA0002619874040000047
in the above formula
Figure FDA0002619874040000048
Figure FDA0002619874040000049
For the output of the accelerometer after error compensation after coordinate transformation,
Figure FDA00026198740400000410
is based on a diagonally symmetric matrix of accelerometer measurements,
Figure FDA00026198740400000411
is calculated byThe following were used:
Figure FDA00026198740400000412
Figure FDA00026198740400000413
step 7.3: establishing an observation equation: zk＝HXk/k+nk，
Wherein: zkIs an error measurement, H is a measurement matrix, Xk/kFor filtering error estimates at time k, nkIs a variance of Rk＝E(nknk T) The noise of the measurement of (2) is,
the calculation formula of the measurement matrix H is as follows:
Figure FDA0002619874040000051
the observation vector corresponding to the measurement matrix H is mk，mkThe calculation formula of (2) is as follows:
mk＝[ΔAk，Δωk，Δrzk，Δvk]，
wherein: delta AkIs a three-dimensional attitude angle ofkFor three-dimensional angular velocity values, Δ rzkAs height value,. DELTA.vkIs a three-dimensional velocity value;
said Xk/kDerived from the previous moment by means of the Kalman filter state update equation, Xk/kThe calculation formula of (2) is as follows:
Xk/k＝Xk/k-1+Kk·[mk-HXk/k-1]，
wherein KkIs the Kalman filter gain, mkFor actual error measurements, Xk/k-1For one-step prediction of error state values, Kalman filter gain KkCalculated by the following formula:
Kk＝Pk/k-1HT(HPk/k-1HT+Rk)-1，
wherein: pk/k-1Estimating a covariance matrix for the error, calculated from the measurements at time k-1, said Pk/k-1The calculation formula of (2) is as follows:
Pk/k-1＝Φk-1Pk-1/k-1Φk-1 T+Qk-1；
step 7.4: obtaining a covariance matrix P according to the state equation obtained after linearization in the steps 7.1 and 7.2k/kThe calculation formula of (2) is as follows:
Pk/k＝(I15×15-KkH)Pk/k-1(I15×15-KkH)T+Rk，
wherein: i is15×1515 x 15 matrix.
Step 7.5: after determining the current speed, attitude and position information of the positioning target, the 15-element error state vector X is determinedk/kAnd setting 0, and continuously updating the state of the system by the observed quantity of the three-dimensional positioning system according to a Kalman filtering equation so as to obtain the real-time three-dimensional information of the positioning target.
8. A three-dimensional positioning system based on Kalman filtering algorithm is characterized in that: the device comprises an annular shell worn on a positioning target, wherein a circuit control board, an attitude measurement module and a navigation measurement and control module are arranged in the shell, a central controller (1) is integrated on the circuit control board, the attitude measurement module comprises a magnetometer (2), a gyroscope (3) and an accelerometer (4), and the navigation measurement and control module comprises an air pressure altimeter (5), an airspeed meter (6) and a GPS (7);
the central controller (1) is respectively connected with the clock module (8), the wireless communication module (9), the AD conversion module (10), the data storage module (11) and the power management module (12) through leads;
the AD conversion module (10) is respectively connected with the magnetometer (2), the gyroscope (3), the accelerometer (4), the barometric altimeter (5), the airspeed meter (6) and the GPS (7) through leads;
the casing outside is provided with SD card interface, USB interface, still be provided with display screen (13) on the casing, display screen (13) are connected with central controller (1) through the wire.
9. The kalman filter algorithm based three-dimensional positioning system according to claim 8, wherein: the wireless communication module (9) specifically comprises a short-range wireless module and a long-range wireless module, wherein the model of the short-range wireless module is nRF24L01, and the model of the long-range wireless module is Xtend.
CN202010780014.5A 2020-08-05 2020-08-05 Three-dimensional positioning method based on Kalman filtering algorithm and positioning system thereof Withdrawn CN111854762A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010780014.5A CN111854762A (en) 2020-08-05 2020-08-05 Three-dimensional positioning method based on Kalman filtering algorithm and positioning system thereof

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010780014.5A CN111854762A (en) 2020-08-05 2020-08-05 Three-dimensional positioning method based on Kalman filtering algorithm and positioning system thereof

Publications (1)

Publication Number Publication Date
CN111854762A true CN111854762A (en) 2020-10-30

Family

ID=72972129

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010780014.5A Withdrawn CN111854762A (en) 2020-08-05 2020-08-05 Three-dimensional positioning method based on Kalman filtering algorithm and positioning system thereof

Country Status (1)

Country Link
CN (1) CN111854762A (en)

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110567481A (en) * 2019-09-05 2019-12-13 上海凌泽信息科技有限公司 object displacement monitoring method
CN114138013A (en) * 2021-11-19 2022-03-04 武汉珈鹰智能科技有限公司 Unmanned aerial vehicle fixed-height flight method, device, equipment and storage medium
CN114383605A (en) * 2021-12-03 2022-04-22 理大产学研基地(深圳)有限公司 Indoor positioning and optimizing method based on MEMS sensor and sparse landmark points
CN114459292A (en) * 2021-12-28 2022-05-10 中国人民解放军国防科技大学 Trajectory calculation system for fire control correction
CN114636842A (en) * 2022-05-17 2022-06-17 成都信息工程大学 Atmospheric data estimation method and device for hypersonic aircraft
CN114719858A (en) * 2022-04-19 2022-07-08 东北大学秦皇岛分校 3-dimensional positioning method based on IMU and floor height target compensation
CN116108873A (en) * 2022-12-12 2023-05-12 天津大学 Motion posture assessment system based on RFID/IMU fusion

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2017063387A1 (en) * 2015-10-13 2017-04-20 上海华测导航技术股份有限公司 Method for updating all attitude angles of agricultural machine on the basis of nine-axis mems sensor
CN107289933A (en) * 2017-06-28 2017-10-24 东南大学 Double card Kalman Filtering guider and method based on MEMS sensor and VLC positioning fusions
CN111426318A (en) * 2020-04-22 2020-07-17 中北大学 Low-cost AHRS course angle compensation method based on quaternion-extended Kalman filtering

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2017063387A1 (en) * 2015-10-13 2017-04-20 上海华测导航技术股份有限公司 Method for updating all attitude angles of agricultural machine on the basis of nine-axis mems sensor
CN107289933A (en) * 2017-06-28 2017-10-24 东南大学 Double card Kalman Filtering guider and method based on MEMS sensor and VLC positioning fusions
CN111426318A (en) * 2020-04-22 2020-07-17 中北大学 Low-cost AHRS course angle compensation method based on quaternion-extended Kalman filtering

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
刘宇;吕玲;路永乐;向高林;: "自适应卡尔曼滤波在磁干扰姿态测量中的应用", 压电与声光, no. 06 *
翟瑞永: "基于MEMS传感器微型导航***的测量控制技术研究", 《中国博士学位论文全文数据库》, pages 1 - 125 *

Cited By (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110567481A (en) * 2019-09-05 2019-12-13 上海凌泽信息科技有限公司 object displacement monitoring method
CN114138013A (en) * 2021-11-19 2022-03-04 武汉珈鹰智能科技有限公司 Unmanned aerial vehicle fixed-height flight method, device, equipment and storage medium
CN114383605A (en) * 2021-12-03 2022-04-22 理大产学研基地(深圳)有限公司 Indoor positioning and optimizing method based on MEMS sensor and sparse landmark points
CN114383605B (en) * 2021-12-03 2024-04-02 理大产学研基地(深圳)有限公司 Indoor positioning and optimizing method based on MEMS sensor and sparse landmark point
CN114459292A (en) * 2021-12-28 2022-05-10 中国人民解放军国防科技大学 Trajectory calculation system for fire control correction
CN114719858A (en) * 2022-04-19 2022-07-08 东北大学秦皇岛分校 3-dimensional positioning method based on IMU and floor height target compensation
CN114719858B (en) * 2022-04-19 2024-05-07 东北大学秦皇岛分校 3-Dimensional positioning method based on IMU and floor height target compensation
CN114636842A (en) * 2022-05-17 2022-06-17 成都信息工程大学 Atmospheric data estimation method and device for hypersonic aircraft
CN116108873A (en) * 2022-12-12 2023-05-12 天津大学 Motion posture assessment system based on RFID/IMU fusion
CN116108873B (en) * 2022-12-12 2024-04-19 天津大学 Motion posture assessment system based on RFID/IMU fusion

Similar Documents

Publication Publication Date Title
CN111854762A (en) Three-dimensional positioning method based on Kalman filtering algorithm and positioning system thereof
CN107490378B (en) Indoor positioning and navigation method based on MPU6050 and smart phone
US8010308B1 (en) Inertial measurement system with self correction
Tanigawa et al. Drift-free dynamic height sensor using MEMS IMU aided by MEMS pressure sensor
US10704902B2 (en) Surveying pole
CN108759834B (en) Positioning method based on global vision
CN109269471A (en) A kind of novel GNSS receiver inclinometric system and method
Wahdan et al. Magnetometer calibration for portable navigation devices in vehicles using a fast and autonomous technique
US11035915B2 (en) Method and system for magnetic fingerprinting
CN111024070A (en) Inertial foot binding type pedestrian positioning method based on course self-observation
CN104880189B (en) A kind of antenna for satellite communication in motion low cost tracking anti-interference method
CN107976187B (en) Indoor track reconstruction method and system integrating IMU and vision sensor
CN108759815B (en) Information fusion integrated navigation method used in global visual positioning method
Suprem et al. Orientation and displacement detection for smartphone device based imus
CN110617795B (en) Method for realizing outdoor elevation measurement by using sensor of intelligent terminal
US10488432B2 (en) Systems and methods for compensating for the absence of a sensor measurement in a heading reference system
CN110986997A (en) Method and system for improving indoor inertial navigation precision
JP2019120587A (en) Positioning system and positioning method
CN110672095A (en) Pedestrian indoor autonomous positioning algorithm based on micro inertial navigation
Tao et al. Precise displacement estimation from time-differenced carrier phase to improve PDR performance
US20140249750A1 (en) Navigational and location determination system
RU2443978C1 (en) Method of determining spatial coordinates of mobile objects and integrated navigation system for realising said method
CN112284388B (en) Unmanned aerial vehicle multisource information fusion navigation method
CN115326062A (en) GNSS INS (Global navigation satellite System for inertial navigation System) integrated navigation method, device and medium based on bionic flapping-wing flying robot
CN103743379B (en) A kind of pipe detector attitude detecting method and its detection means

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
WW01 Invention patent application withdrawn after publication
WW01 Invention patent application withdrawn after publication

Application publication date: 20201030