A kind of indoor orientation method based on micro-inertia sensorTechnical field
The present invention relates to a kind of indoor orientation method, particularly a kind of indoor orientation method based on micro-inertia sensor.
Background technology
At present, because global position system navigation signal cannot cover indoor, therefore indoor positioning is all the localization method based on radio-frequency (RF) identification.Indoor locating system based on radio-frequency identification method comprises: with reference to electronic tag, frequency read/write, main frame and database, electronic tag and read write line realize exchanges data by wireless network, read write line is connected to database by wired or wireless form, its positioning precision is high, but need in advance electronic label array to be accurately arranged in indoor environment, simultaneously in order to ensure that high positioning precision also needs to improve the quantity of electronic tag, this just means the raising of cost, but also by producing larger interference, affect the intensity of signal.Therefore, existing indoor orientation method, due to the restriction of a variety of causes, makes indoor orientation method cannot popularize on a large scale always.
Summary of the invention
The object of the invention is to provide a kind of indoor orientation method based on micro-inertia sensor, solves satnav and there is blind area and the problem that location method information collecting work amount is large, algorithm is complicated, data processing is difficult, hardware device cost is high based on radio-frequency (RF) identification in indoor.
Based on an indoor orientation method for micro-inertia sensor, its concrete steps are:
The first step builds micro-inertia indoor locating system
Micro-inertia indoor locating system, comprising: micro-inertia sensor, magnetic resistance electronic compass, Information Collecting & Processing device, navigation calculation processor, display control device, communication bus A, communication bus B and communication bus C; Wherein micro-inertia sensor comprises: three axle micro-inertial acceleration meters and the micro-inertial gyroscope of three axles; Navigation calculation processor comprises: error compensation module, quiet dynamic measuring data recognition module, initial alignment module and strap-down inertial resolve module.Micro-inertia sensor and magnetic resistance electronic compass are packaged in one, are all connected with Information Collecting & Processing device by communication bus A; Information Collecting & Processing device is connected with navigation calculation processor by communication bus B; Navigation calculation processor is connected with display control device by communication bus C; Display control device is used for man-machine interaction.
The micro-inertia indoor locating system start of second step
After the indoor carrier needing location installs micro-inertia indoor locating system, control display control device to power up to micro-inertia indoor locating system, micro-inertia sensor and magnetic resistance electronic compass start to gather carrier data and be transferred to navigation calculation processor after data acquisition processing device process.
3rd step error compensation module for compensating error of measured data
After navigation calculation processor receives the measurement data of micro-inertia sensor and magnetic resistance electronic compass, carry out error compensation by error compensation module:
Consider zero inclined, alignment error, Random Drift Error item of the micro-inertial acceleration meter of three axles, ignore more than second order dynamically Small errors, the error model setting up the micro-inertial acceleration meter of three axles is:
（1）
In formula,it is three axles micro-inertial acceleration meter output valve;be that three axles micro-inertial acceleration meter zero is inclined;it is three axles micro-inertial acceleration meter scaling ratio;it is the micro-inertial acceleration meter of three axlesaxle pairthe quadrature error coefficient of axle;for carrier movement input acceleration;it is three axles micro-inertial acceleration meter stochastic error.
Consider zero inclined, alignment error, quadrature error and Random Drift Error of three axle gyroscopes, ignore more than second order dynamically Small errors, the error model setting up three axle gyroscopes is:
（2）
In formula,be three axle gyroscope output valves;be the zero inclined of three axle gyroscopes;it is the scaling ratio of three axle gyroscopes;for carrier movement input angular velocity;be three axle gyroscope stochastic errors;be three axle gyroscopesaxle pairthe alignment error coefficient of axle;for the once item error coefficient relevant with acceleration.
The error model setting up magnetic resistance electronic compass is:
（3）
In formula,for the output valve of magnetic resistance electronic compass;for the carrier heading pre-entered;,,,,for the penalty coefficient of magnetic resistance electronic compass.
Error compensation module is transferred to quiet dynamic measuring data recognition module after carrying out error compensation to measurement data.
4th step quiet dynamic measuring data recognition module identification carrier static and dynamic Status data
Quiet dynamic measuring data recognition module receives after the measurement data of error compensation, picks out the static and dynamic Status of carrier, and measurement data is decomposed into static data and dynamic data according to the situation of change of micro-inertia sensor (1) measurement data.Wherein, static data is used for the initial alignment at attitude of carrier angle, and dynamic data is used for the posture renewal of carrier, speed upgrades and location updating.
5th step initial alignment module resolves carrier initial attitude angle
The initial alignment that initial alignment module carries out attitude of carrier angle according to the static data of quiet dynamic measuring data recognition module is resolved.
The angle of pitch of the acceleration static information determination carrier exported by three-axis micro accelerometerand roll angle, formula is:
（4）
（5）
In formula,,,for the output valve of accelerometer,for acceleration of gravity.
By the course angle of magnetic resistance electronic compass determination carrier:
（6）
Through type (4), formula (5), formula (6) obtain the initial attitude angle under carrier initial rest state.
The strap-down inertial that the initial attitude angle information of carrier sends in navigation calculation processor is resolved module by initial alignment module.
6th step strap-down inertial resolves module and resolves carrier locating information
Strap-down inertial resolves dynamic data that module goes out according to quiet Dynamic Identification module identification and the carrier initial attitude angle that initial alignment module obtains, strap inertial navigation algorithm is adopted to carry out continuous integration process to result, and be transformed in navigational coordinate system, first calculate the attitude angle information of carrier: the angle of pitch, course angleand roll angle; Calculate the velocity information of carrier again: transverse velocity, forward speedand longitudinal velocity; Finally calculate the positional information transversal displacement of carrier, forward direction displacementand length travel.
The attitude angle information of carrier is resolved:
Carrier coordinate systemto navigational coordinate systemattitude matrixfor:
（7）
The initial attitude angle obtained by initial alignment module obtains original state matrix by formula (7), for the posture renewal of carrier provides initial value, obtained the value of initial hypercomplex number by original state matrix.
（8）
（9）
With the angular velocity that the three axle gyroscopes through error compensation are measured,,, quadravalence dragon lattice-Ku Tafa is carried out to formula (8) and resolves, and do normalized according to formula (9), realize the real-time update of hypercomplex number.
（10）
After formula (8) and formula (9) calculate hypercomplex number in real time, completed the renewal of attitude matrix by formula (10), and the attitude angle information of carrier can be obtained according to formula (7) and formula (10).
The velocity information of carrier is resolved:
Through the ratio force information that the three-axis micro accelerometer of over-compensation is measured,,, pass through attitude matrixwith the transformation of indoor coordinate system to indoor coordinate system, the speed of carrying out carrier in indoor coordinate system by integration upgrades.
（11）
In conjunction with the attitude angle information of the carrier calculated above, second order Runge-Kutta method is carried out to formula (8) and resolves, obtain the velocity information of carrier.
The positional information of carrier is resolved:
（13）
Carry out an integration again to formula (13) to resolve, obtain the positional information of carrier.
Navigation calculation processor exports the attitude angle information of carrier, comprising: the angle of pitch, course angleand roll angle, velocity information, comprising: transverse velocity, forward speedand longitudinal velocity, positional information, comprising: transversal displacement, forward direction displacementand length travel.Resolve the carrier locating information obtained and be transferred to display control device by communication bus C.
7th step display control device exports carrier locating information
Attitude angle information, velocity information, the positional information output display of carrier that display control device will receive, thus complete the indoor positioning based on micro-inertia sensor.
Indoor orientation method based on micro-inertia sensor of the present invention solves satnav and there is blind area, other problem that location technology information acquisition workload is large, algorithm is complicated, data processing is difficult, hardware device cost is high in indoor.
Accompanying drawing explanation
Micro-inertia indoor locating system schematic diagram of a kind of indoor orientation method based on micro-inertia sensor of Fig. 1.
1. quiet dynamic measuring data recognition module 13. initial alignment module 14. strap-down inertial of micro-inertia sensor 2. magnetic resistance electronic compass 3. Information Collecting & Processing device 4. navigation calculation processor 5. display control device 6. communication bus A7. communication bus B8. communication bus C9. tri-axle micro-inertial gyroscope 10. 3 axle micro-inertial acceleration meter 11. error compensation module 12. resolves module.
Embodiment
Based on an indoor orientation method for micro-inertia sensor, its concrete steps are:
The first step builds micro-inertia indoor locating system
Micro-inertia indoor locating system, comprising: micro-inertia sensor 1, magnetic resistance electronic compass 2, Information Collecting & Processing device 3, navigation calculation processor 4, display control device 5, communication bus A6, communication bus B7 and communication bus C8; Wherein micro-inertia sensor 1 comprises: three axles micro-inertial acceleration meter 9 and the micro-inertial gyroscope 10 of three axles; Navigation calculation processor 4 comprises: error compensation module 11, quiet dynamic measuring data recognition module 12, initial alignment module 13 and strap-down inertial resolve module 14.Micro-inertia sensor 1 and magnetic resistance electronic compass 2 are packaged in one, are all connected with Information Collecting & Processing device 3 by communication bus A6; Information Collecting & Processing device 3 is connected with navigation calculation processor 4 by communication bus B7; Navigation calculation processor 4 is connected with display control device 5 by communication bus C8; Display control device 5 is for man-machine interaction.
The micro-inertia indoor locating system start of second step
After the indoor carrier needing location installs micro-inertia indoor locating system, control display control device 5 to power up to micro-inertia indoor locating system, micro-inertia sensor 1 and magnetic resistance electronic compass 2 start to gather carrier data and be transferred to navigation calculation processor 4 after data acquisition processing device 3 process.
3rd step error compensation module 11 compensating measure data error
After navigation calculation processor 4 receives the measurement data of micro-inertia sensor 1 and magnetic resistance electronic compass 2, carry out error compensation by error compensation module 11:
Consider zero inclined, alignment error, Random Drift Error item of the micro-inertial acceleration meter 9 of three axles, ignore more than second order dynamically Small errors, the error model setting up the micro-inertial acceleration meter 9 of three axles is:
（1）
In formula,it is three axles micro-inertial acceleration meter 9 output valve;be that three axles micro-inertial acceleration meter 9 zero is inclined;it is three axles micro-inertial acceleration meter 9 scaling ratio;it is the micro-inertial acceleration meter 9 of three axlesaxle pairthe quadrature error coefficient of axle;for carrier movement input acceleration;it is three axles micro-inertial acceleration meter 9 stochastic error.
Consider zero inclined, alignment error, quadrature error and Random Drift Error of three axle gyroscopes 10, ignore more than second order dynamically Small errors, the error model setting up three axle gyroscopes 10 is:
（2）
In formula,be three axle gyroscope 10 output valves;be the zero inclined of three axle gyroscopes 10;it is the scaling ratio of three axle gyroscopes 10;for carrier movement input angular velocity;be three axle gyroscope 10 stochastic errors;be three axle gyroscopes 10axle pairthe alignment error coefficient of axle;for the once item error coefficient relevant with acceleration.
The error model setting up magnetic resistance electronic compass 2 is:
（3）
In formula,for the output valve of magnetic resistance electronic compass 2;for the carrier heading pre-entered;,,,,for the penalty coefficient of magnetic resistance electronic compass 2.
Quiet dynamic measuring data recognition module 12 is transferred to after error compensation module 11 pairs of measurement data carry out error compensation.
4th step quiet dynamic measuring data recognition module 12 identification carrier static and dynamic Status data
Quiet dynamic measuring data recognition module 12 receives after the measurement data of error compensation, picks out the static and dynamic Status of carrier, and measurement data is decomposed into static data and dynamic data according to the situation of change of micro-inertia sensor 1 measurement data.Wherein, static data is used for the initial alignment at attitude of carrier angle, and dynamic data is used for the posture renewal of carrier, speed upgrades and location updating.
5th step initial alignment module 13 resolves carrier initial attitude angle
The initial alignment that initial alignment module 13 carries out attitude of carrier angle according to the static data of quiet dynamic measuring data recognition module 12 is resolved.
The angle of pitch of the acceleration static information determination carrier exported by three-axis micro accelerometer 9and roll angle, formula is:
（4）
（5）
In formula,,,for the output valve of accelerometer,for acceleration of gravity.
The course angle of carrier is determined by magnetic resistance electronic compass 2:
（6）
Through type (4), formula (5), formula (6) obtain the initial attitude angle under carrier initial rest state.
The initial attitude angle information strap-down inertial sent in navigation calculation processor 4 of carrier is resolved module 14 by initial alignment module 13.
6th step strap-down inertial resolves module 14 and resolves carrier locating information
Strap-down inertial resolves dynamic data that module 14 picks out according to quiet Dynamic Identification module 12 and the carrier initial attitude angle that initial alignment module 13 obtains, strap inertial navigation algorithm is adopted to carry out continuous integration process to result, and be transformed in navigational coordinate system, first calculate the attitude angle information of carrier: the angle of pitch, course angleand roll angle; Calculate the velocity information of carrier again: transverse velocity, forward speedand longitudinal velocity; Finally calculate the positional information transversal displacement of carrier, forward direction displacementand length travel.
The attitude angle information of carrier is resolved:
Carrier coordinate systemto navigational coordinate systemattitude matrixfor:
（7）
The initial attitude angle obtained by initial alignment module 13 obtains original state matrix by formula (7), for the posture renewal of carrier provides initial value, obtained the value of initial hypercomplex number by original state matrix.
（8）
（9）
With the angular velocity that the three axle gyroscopes 10 through error compensation are measured,,, quadravalence dragon lattice-Ku Tafa is carried out to formula (8) and resolves, and do normalized according to formula (9), realize the real-time update of hypercomplex number.
（10）
After formula (8) and formula (9) calculate hypercomplex number in real time, completed the renewal of attitude matrix by formula (10), and the attitude angle information of carrier can be obtained according to formula (7) and formula (10).
The velocity information of carrier is resolved:
Through the ratio force information that the three-axis micro accelerometer 9 of over-compensation is measured,,, pass through attitude matrixwith the transformation of indoor coordinate system to indoor coordinate system, the speed of carrying out carrier in indoor coordinate system by integration upgrades.
（11）
In conjunction with the attitude angle information of the carrier calculated above, second order Runge-Kutta method is carried out to formula (8) and resolves, obtain the velocity information of carrier.
The positional information of carrier is resolved:
（13）
Carry out an integration again to formula (13) to resolve, obtain the positional information of carrier.
Navigation calculation processor 4 exports the attitude angle information of carrier, comprising: the angle of pitch, course angleand roll angle, velocity information, comprising: transverse velocity, forward speedand longitudinal velocity, positional information, comprising: transversal displacement, forward direction displacementand length travel.Resolve the carrier locating information obtained and be transferred to display control device 5 by communication bus C8.
7th step display control device 5 exports carrier locating information
Attitude angle information, velocity information, the positional information output display of carrier that display control device 5 will receive, thus complete the indoor positioning based on micro-inertia sensor.