CN106931965A - A kind of method and device for determining terminal attitude - Google Patents
A kind of method and device for determining terminal attitude Download PDFInfo
- Publication number
- CN106931965A CN106931965A CN201511028441.3A CN201511028441A CN106931965A CN 106931965 A CN106931965 A CN 106931965A CN 201511028441 A CN201511028441 A CN 201511028441A CN 106931965 A CN106931965 A CN 106931965A
- Authority
- CN
- China
- Prior art keywords
- terminal
- attitude
- angle
- value
- angular velocity
- 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.)
- Granted
Links
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/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
Abstract
The invention discloses a kind of method and apparatus for determining terminal attitude, in current sample period, the sampling magnitude of angular velocity of the gyroscope output of terminal is determined, and compensate the sampling magnitude of angular velocity of determination；According to the sampling magnitude of angular velocity and initial attitude matrix of compensation, it is determined that updating attitude matrix；According to the sampling magnitude of angular velocity and the renewal attitude matrix for determining compensation, the attitude optimal estimation value of the terminal is determined by Kalman filtering.The method and apparatus of determination terminal attitude provided in an embodiment of the present invention, improve the precision of low-speed motion navigation, reducing the error of terminal Attitude estimation.
Description
Technical field
The present invention relates to automated computer field, more particularly to a kind of method and device for determining terminal attitude.
Background technology
With the continuous lifting of the level of hardware of smart mobile phone, many smart mobile phones have been equipped with acceleration biography
The sensors such as sensor, gyroscope, magnetometer, so as to possess sense accelerations, angular speed, magnetic field intensity
Ability.However, because the sensor accuracy in smart mobile phone is low, and indoor environment complexity, intelligence
The precision at mobile phone spatial attitude angle is influenceed by following many factors：
(1) sensor error, including Systematic Errors in itself and random error.Systematic Errors are constant value,
Random error is different from each sensor start-up course.
(2) accumulated error.Using the measurement data of gyroscope, intelligence can be obtained by attitude updating algorithm
The attitude angle of mobile phone, precision is higher in its short time, but due to the integral characteristic of gyroscope, its precision with
Time declines.
(3) magnetic interference.Due to magnetic interference, cause magnetic sensor data exception, band is calculated to attitude angle
Carry out error, the attitude angle that system can be caused to export when serious deviates normal value；After magnetic disturbance is eliminated, it is necessary to
It is lot more time to attitude angle is returned to normal value.
The spatial attitude algorithm of existing intelligent mobile phone sensor is：The state of smart mobile phone is divided into quick fortune
Dynamic state with close to inactive state；Mobile phone is calculated to time integral using gyroscope under quick motion state
Spatial attitude, close to the space for calculating mobile phone in the state of static with electronic compass using acceleration transducer
Appearance angle, and current spatial attitude is modified and error concealment.Outdoor high-speed motion is mainly used in lead
Boat, because high-speed motion is less demanding to spatial attitude accuracy, and can be carried out using location parameter to result
Amendment, therefore practical application effect is preferable.
However, the low-speed motion location navigation based on intelligent mobile phone sensor, the i.e. pedestrian based on smart mobile phone
Navigation is subject to the sensor accuracy in smart mobile phone low and the factor such as complexity of indoor environment is influenceed, application
Effect is unsatisfactory.
The content of the invention
The present invention provides a kind of method and device for determining terminal attitude, is used to solve intelligent hand in the prior art
The spatial attitude algorithm of machine sensor is not suitable for the problem of low-speed motion location navigation.
The embodiment of the present invention provides a kind of method for determining terminal attitude, including：
In current sample period, the sampling magnitude of angular velocity of the gyroscope output of terminal is determined, and compensate determination
The sampling magnitude of angular velocity；
According to the sampling magnitude of angular velocity and initial attitude matrix of compensation, it is determined that updating attitude matrix；
According to the sampling magnitude of angular velocity and the renewal attitude matrix for determining compensation, by Kalman filtering
Determine the attitude optimal estimation value of the terminal.
Optionally, the initial attitude matrix determines according to following methods：
According to the systematic error of acceleration transducer, the angle of pitch and roll angle of terminal are determined；
According to the angle of pitch and roll angle of the terminal, the initial attitude matrix of the terminal is determined.
Optionally, the systematic error according to acceleration transducer, determines the angle of pitch and roll angle of terminal,
Including：
When sensor-triggered instruction is received, the initial acceleration angle value of the acceleration transducer output is obtained；
According to the initial acceleration angle value and the systematic error of the acceleration transducer, the angle of pitch and institute are determined
State roll angle；
The angle of pitch and roll angle according to the terminal, determines the initial attitude matrix of the terminal, bag
Include：
When sensor-triggered instruction is received, the azimuth of Magnetic Sensor output is obtained；
The initial attitude matrix of the terminal is determined according to the azimuth, the angle of pitch and the roll angle.
Optionally, the sampling magnitude of angular velocity and the renewal attitude matrix according to the compensation, by card
Kalman Filtering determines the attitude optimal estimation value of the terminal, including：
Sampling magnitude of angular velocity according to the compensation determines the predicted value of Kalman filter, according to the renewal
Attitude matrix determines the measuring value of the Kalman filter；
The predicted value according to the measuring value amendment for determining, obtains the optimum attitude estimate of the terminal.
Optionally, the optimum attitude of the terminal is estimated to include the random error of gyroscope；
The sampling magnitude of angular velocity that the compensation determines, including：
The random error of the gyroscope in being estimated according to a upper sampling period optimum attitude, and gyro
The systematic error of instrument, the sampling magnitude of angular velocity of compensation current sample period gyroscope output.
It is optionally, described to determine the sampling magnitude of angular velocity of the gyroscope output of terminal in current sample period,
And before compensating the sampling magnitude of angular velocity of determination, also include：
When sensor calibration instruction is received, the verification acceleration of the acceleration transducer output is obtained
Value, the value according to the verification acceleration determines the systematic error of the acceleration transducer.
It is optionally, described to determine the sampling magnitude of angular velocity of the gyroscope output of terminal in current sample period,
And before compensating the sampling magnitude of angular velocity of determination, also include：
When sensor calibration instruction is received, the value of the verification angular speed of the gyroscope output, root are obtained
The systematic error of the gyroscope is determined according to the value of the verification angular speed.
The embodiment of the present invention also provides a kind of device for determining terminal attitude, including：
Compensating module：For in current sample period, determining the sampling magnitude of angular velocity of the gyroscope output of terminal,
And compensate the sampling magnitude of angular velocity of determination；
Attitude matrix module：For sampling magnitude of angular velocity and initial attitude matrix according to compensation, it is determined that updating
Attitude matrix；
Estimate determining module：For according to the sampling magnitude of angular velocity and the renewal attitude for determining compensation
Matrix, the attitude optimal estimation value of the terminal is determined by Kalman filtering.
Optionally, the initial attitude matrix determines according to following methods：
According to the systematic error of acceleration transducer, the angle of pitch and roll angle of terminal are determined；
According to the angle of pitch and roll angle of the terminal, the initial attitude matrix of the terminal is determined.
Optionally, the attitude matrix module, specifically for：
When sensor-triggered instruction is received, the initial acceleration angle value of the acceleration transducer output is obtained；
According to the initial acceleration angle value and the systematic error of the acceleration transducer, the angle of pitch is determined
With the roll angle；
When sensor-triggered instruction is received, the azimuth of Magnetic Sensor output is obtained；
The initial attitude matrix of the terminal is determined according to the azimuth, the angle of pitch and the roll angle.
Optionally, the estimate determining module, specifically for：
Sampling magnitude of angular velocity according to the compensation determines the predicted value of Kalman filter, according to the renewal
Attitude matrix determines the measuring value of the Kalman filter；
The predicted value according to the measuring value amendment for determining, obtains the optimum attitude estimate of the terminal.
Optionally, the optimum attitude of the terminal is estimated to include the random error of gyroscope；
The compensating module, specifically for：
The random error of the gyroscope in being estimated according to a upper sampling period optimum attitude, and gyro
The systematic error of instrument, the sampling magnitude of angular velocity of compensation current sample period gyroscope output.
Optionally, the compensating module, is additionally operable to：
When sensor calibration instruction is received, the verification acceleration of the acceleration transducer output is obtained
Value, the value according to the verification acceleration determines the systematic error of the acceleration transducer.
Optionally, the compensating module, is additionally operable to：
When sensor calibration instruction is received, the value of the verification angular speed of the gyroscope output, root are obtained
The systematic error of the gyroscope is determined according to the value of the verification angular speed.
The invention provides a kind of method and apparatus for determining terminal attitude, in current sample period, it is determined that eventually
The sampling magnitude of angular velocity of the gyroscope output at end, and compensate the sampling magnitude of angular velocity of determination；According to compensation
Sampling magnitude of angular velocity and initial attitude matrix, it is determined that update attitude matrix；Determine adopting for compensation according to described
Sample magnitude of angular velocity and the renewal attitude matrix, determine that the attitude of the terminal is optimal and estimate by Kalman filtering
Evaluation.The method and apparatus for determining terminal attitude provided in an embodiment of the present invention, improve low-speed motion navigation
Precision, the error for reducing terminal Attitude estimation.
Brief description of the drawings
Technical scheme in order to illustrate more clearly the embodiments of the present invention, institute in being described to embodiment below
The accompanying drawing for needing to use is briefly introduced, it should be apparent that, drawings in the following description are only of the invention
Some embodiments, for one of ordinary skill in the art, are not paying the premise of creative labor
Under, other accompanying drawings can also be obtained according to these accompanying drawings.
Fig. 1 is a kind of method flow schematic diagram for determining terminal attitude provided in an embodiment of the present invention；
Fig. 2 is a kind of navigational coordinate system schematic diagram provided in an embodiment of the present invention；
Fig. 3 is a kind of end coordinates system schematic diagram provided in an embodiment of the present invention；
Fig. 4 is a kind of method flow diagram of terminal attitude provided in an embodiment of the present invention；
Fig. 5 is a kind of apparatus structure schematic diagram for determining terminal attitude provided in an embodiment of the present invention.
Specific embodiment
In order that the object, technical solutions and advantages of the present invention are clearer, below in conjunction with accompanying drawing to this hair
It is bright to be described in further detail, it is clear that described embodiment is only some embodiments of the invention,
Rather than whole embodiments.Based on the embodiment in the present invention, those of ordinary skill in the art are not doing
Go out all other embodiment obtained under the premise of creative work, belong to the scope of protection of the invention.
Terminal in the embodiment of the present invention is to refer to the equipment for providing a user with voice and/or data connectivity
, including wireless terminal or catv terminal (device).Wireless terminal can be with the hand-held of wireless connecting function
Formula equipment is connected to other processing equipments of radio modem, through wireless access network and one or many
The mobile terminal that individual core net is communicated.For example, it (or is " honeycomb " that wireless terminal can be mobile phone
Phone) and computer with mobile terminal.And for example, wireless terminal can also be portable, pocket,
Hand-held, built-in computer or vehicle-mounted mobile device.Determination terminal attitude in the embodiment of the present invention
Method and apparatus be applied in terminal guidance system.
The embodiment of the present invention provides a kind of method for determining terminal attitude, as shown in figure 1, for the present invention is implemented
A kind of method flow schematic diagram of determination terminal attitude that example is provided, including：
Step 101：In current sample period, the sampling magnitude of angular velocity of the gyroscope output of terminal is determined, and mend
Repay the sampling magnitude of angular velocity of determination；
Step 102：According to the sampling magnitude of angular velocity and initial attitude matrix of compensation, it is determined that updating attitude matrix；
Step 103：According to the sampling magnitude of angular velocity and renewal attitude matrix that determine compensation, by Kalman filtering
Determine the attitude optimal estimation value of terminal.
Specifically, before step 101, terminal carry out school using meeting to all the sensors in terminal first
Test, the sensor in embodiment of the present invention terminal comprises at least acceleration transducer, gyroscope and Magnetic Sensor.
Make terminal remains stationary for a period of time such as 30 seconds.When sensor calibration instruction is received, obtain acceleration and pass
The value of the verification acceleration of sensor output, and each is passed to determine acceleration transducer according to the value of verification acceleration
Feel the systematic error of axle；The value of the verification angular speed of the gyroscope output is obtained simultaneously, according to verification angle speed
The value of degree determines the systematic error of each sensing shaft of gyroscope.
It should be noted that the verification of sensor can be carried out once when only terminal is used for the first time, obtain
The systematic error of acceleration transducer and the systematic error of gyroscope, it is also possible to verified manually when needing.
In step 102, initial attitude matrix determines according to following methods：According to the system of acceleration transducer
Error, determines the angle of pitch and roll angle of terminal；According to the angle of pitch and roll angle of terminal, terminal is determined
Initial attitude matrix.
In each navigator fix, make terminal remains stationary for a period of time such as 5 seconds, according to acceleration transducer,
The output data of Magnetic Sensor carries out the initialization of attitude, calculates initial attitude matrix.Specifically, connecing
When receiving sensor-triggered instruction, the initial acceleration angle value of acceleration transducer output is obtained；Added according to initial
The systematic error of velocity amplitude and acceleration transducer, determines the angle of pitch and the roll angle；Additionally,
When receiving sensor-triggered instruction, the azimuth of Magnetic Sensor output is obtained；According to azimuth, the angle of pitch
With the initial attitude matrix that roll angle determines the terminal.
In step 103, the sampling magnitude of angular velocity first according to compensation determines the predicted value of Kalman filter,
Then the measuring value of the Kalman filter is determined according to renewal attitude matrix；Finally according to the measurement for determining
Value amendment predicted value, obtains the optimum attitude estimate of terminal.Wherein, optimum attitude is estimated to include acceleration
The random error of sensor, the random error of gyroscope and attitude error.Additionally, being adopted according to upper one
Sample cycle optimum attitude estimate in gyroscope random error, and gyroscope systematic error, compensation work as
The sampling magnitude of angular velocity of preceding sampling period gyroscope output.
The invention provides a kind of method for determining terminal attitude, in current sample period, the top of terminal is determined
The sampling magnitude of angular velocity of spiral shell instrument output, and compensate the sampling magnitude of angular velocity of determination；According to the sampling of compensation
Magnitude of angular velocity and initial attitude matrix, it is determined that updating attitude matrix；According to the sampling angle speed for determining compensation
Angle value and the renewal attitude matrix, the attitude optimal estimation value of the terminal is determined by Kalman filtering.
The method for determining terminal attitude provided in an embodiment of the present invention, improves the precision of low-speed motion navigation, reduces
The error of terminal Attitude estimation.
Below by specific process of calculation analysis to the method for determining terminal attitude provided in an embodiment of the present invention
It is described in detail.
Sensor in embodiment of the present invention terminal comprises at least acceleration transducer, gyroscope and Magnetic Sensor.
Terminal carry out verification to all the sensors in terminal using meeting first makes terminal remains stationary for a period of time such as 30
Second, the output data of gyroscope and acceleration transducer is obtained, and calculate the xyz of gyroscope in 30 seconds
Three axles output angular speed average beThe acceleration of the axles of xyz tri- output of acceleration transducer in 30 seconds
Spending average isThe systematic error of gyroscope is then obtained according to formula (1)：
In formula, bsx,bsy,bszRepresent three axle system errors of gyroscope, the i.e. bias of three axle constant value zero of gyroscope.
The systematic error of acceleration transducer is obtained according to formula (2)：
In formula, dsx,dsy,dszRepresent three axle system errors of acceleration transducer, i.e. three axles of acceleration transducer
The bias of constant value zero；G represents the value of acceleration of gravity.
It should be noted that the verification of sensor can be carried out once when only terminal is used for the first time, obtain,
Can also manually be verified when needing.
Before the initial attitude matrix of computing terminal, it is necessary first to determine the direction of coordinate system, coordinate system bag
Navigational coordinate system and end coordinates system are included, wherein navigational coordinate system is defined as L coordinate system, and end coordinates system determines
Justice is d coordinate systems.
Fig. 2 is a kind of navigational coordinate system schematic diagram provided in an embodiment of the present invention, as shown in Fig. 2 navigation is sat
Mark system origin is located at earth surface, and three axles point to as follows：X-axis points to east along prime vertical, and y-axis refers to along meridian circle
Northwards, z-axis points to zenith along normal.Fig. 3 is a kind of end coordinates system schematic diagram provided in an embodiment of the present invention,
As shown in figure 3, the origin of end coordinates system is located at equipment center, x-axis sensing equipment right side, y-axis is pointed to and set
Standby front, z-axis is upward.
Rotation relationship between navigational coordinate system and end coordinates system can represent with a matrix, referred to as attitude
Matrix, such as formula (3)：
In formula,Attitude matrix is represented, y, p, r represents attitude angle, wherein, y represents deflection (Yaw),
P represents the angle of pitch (Pitch), and r represents roll angle (Roll).
In each navigator fix, make terminal remains stationary for a period of time such as 5 seconds, according to initial acceleration angle value
With the systematic error of acceleration transducer, angle of pitch p and roll angle r, such as formula (4) are determined：
In formula,Represent the average of accelerometer x-axis and y-axis output acceleration in 5 seconds quiescent times.
With reference to the deflection y that Magnetic Sensor is exported, formula (4) is substituted into formula (3), that is, obtain the initial of terminal
Attitude matrix.
During navigator fix, the output data according to gyroscope carries out posture renewal, obtains each sampling
The attitude in cycle, such as formula (5)：
In formula,The attitude matrix in each sampling period is represented,Make difficulties title battle array, and its element is by gyro
The output data of instrument is constituted, and represents angular velocity of rotation of the end coordinates system relative to navigational coordinate system.According to
Formula (6) determines：
In formula, ωx,ωy,ωzFor gyroscope output angular speed by gyroscope systematic error and gyroscope
Value after random back-off.
The output data of gyroscope is generally discrete, is exported according to certain sampling period.Sampling period can
To configure, such as 0.02s, 0.05s, 0.1s etc..Therefore formula (6) is substituted into formula (5), obtains updating appearance
Shown in state matrix such as formula (7)：
In formula, k, k+1 represent the moment in former and later two sampling periods of discretization respectively, and Δ t represents the sampling period.
Below by the optimal estimation for using Kalman filtering to carry out attitude.The state vector of Kalman filter
It is 9 ranks, including 3 attitude errors, 3 axle gyroscope random errors, 3 axle acceleration sensors are with chance error
Difference.Using the error equation of gyroscope attitude angle as the state equation of wave filter, using attitude error as filter
The measuring value of ripple device.
Specifically, state vector is determined according to formula (8)：
X=(δ p δ r δ y bx by bz dx dy dz)T (8)
In formula, X represents state vector, and δ p, δ r, δ y represent the angle of pitch, roll angle, deflection respectively
Attitude error, bx, by, bzRepresent three axle random errors of gyroscope, dxdydzRepresent acceleration transducer
Three axle random errors.
Specifically, state equation is determined according to formula (9)：
Xk+1=Fk·Xk+Gk·Wk (9)
In formula, Xk, Xk+1The state value in sampling period before and after representing respectively；FkRepresent state-transition matrix；Gk
Represent that noise drives matrix, its value is taken as unit matrix；WkThe dynamic noise of expression system.FkAccording to formula (10)
It is determined that：
In formula,The unit matrix of 3 × 3 ranks is represented,Represent the null matrix of 3 × 3 ranks.
WkDetermined according to formula (11)：
In formula, E, D represent mathematic expectaion and variance respectively.Qk+1Represent the variance of dynamic noise.It is general to pass
The dynamic noise of each axle can be provided in the explanation document of sensor.
Specifically, the measurement equation of Kalman filter is determined according to formula (12)：
Zk+1=Hk+1·Xk+1+Vk+1 (12)
In formula, Zk+1It is measuring value, Hk+1It is measurement matrix, Vk+1To measure noise.
Specifically, the measuring value Z in formula (12)k+1Determined according to formula (13)：
In formula, pg,rg,ygThree attitude angles by being obtained after posture renewal, p are represented respectivelyam,ramDifference table
Show the renewal angle of pitch and update roll angle, y that acceleration transducer output data calculatesmExpression is currently adopted
The renewal deflection of sample cycle Magnetic Sensor output.pg,rg,ygDetermined according to formula (14)：
In formula, Rij(i=1,2,3；J=1,2,3) the element in correspondence formula (3).
pam,ramDetermined according to formula (15)：
Specifically, the measuring value noise V in formula (12) is determined according to formula (16)k+1：
In formula, Rk+1The variance of observation noise is represented, the variance R of observation noise is determined according to formula (17)k+1:
In above formula,Represent a reference value of observation noise variance, ythreIt is the threshold value of direction angle error.Specifically,
In Kalman filtering process, according to the compatibility between Kalman filtering measuring value and predicted value, dynamic is adjusted
The variance R of whole observation noisek+1。
Specifically, the measurement matrix H in formula (12) is determined according to formula (18)k+1：
So far, the model of Kalman filtering has been set up, and Kalman filtering process is divided into two steps：Time update and
Measure and update.The equation that wherein time updates determines according to formula (19)：
It is to be determined according to formula (20) to measure the equation for updating：
In formula (20)Be by the attitude optimal estimation after Kalman filtering, including attitude error,
Gyroscope random error, acceleration transducer random error.The attitude optimal estimation of current sample period can be with
The posture renewal and filtering of next cycle are fed back to, i.e., is estimated according to current sample period optimum attitude
In the gyroscope random error, and gyroscope systematic error, compensate next sampling period top
The sampling magnitude of angular velocity of spiral shell instrument output.
In the embodiment of the present invention, constantly posture renewal matrix and karr are computed repeatedly during navigator fix
The step of graceful filtering, you can for each sampling period provides optimal attitude angle information.
Below by specific method flow, to a kind of side for determining terminal attitude provided in an embodiment of the present invention
Method is described in detail, and Fig. 4 is a kind of method flow diagram for determining terminal attitude provided in an embodiment of the present invention,
Including：
Step 401：Verification acceleration transducer and gyroscope, obtain the systematic error and top of acceleration transducer
The systematic error of spiral shell instrument.
Specifically, terminal be verified using meeting to all the sensors in terminal first, the embodiment of the present invention
Sensor in terminal comprises at least acceleration transducer, gyroscope and Magnetic Sensor.Make terminal remains stationary
Such as 30 seconds a period of time.When sensor calibration instruction is received, the verification of acceleration transducer output is obtained
The value of acceleration, and the systematic error of each sensing shaft of acceleration transducer is determined according to the value of verification acceleration；
The value of the verification angular speed of the gyroscope output is obtained simultaneously, and the value according to verification angular speed determines gyroscope
The systematic error of each sensing shaft.
It should be noted that the verification of sensor can be carried out once when only terminal is used for the first time, obtain
The systematic error of acceleration transducer and the systematic error of gyroscope, it is also possible to verified manually when needing.
Step 402：Determine the initial attitude matrix of terminal.
Specifically, according to the systematic error of acceleration transducer, the angle of pitch and roll angle of terminal are determined；Root
According to the angle of pitch and roll angle of terminal, the initial attitude matrix of terminal is determined.
In each navigator fix, make terminal remains stationary for a period of time such as 5 seconds, according to acceleration transducer,
The output data of Magnetic Sensor carries out the initialization of attitude, calculates initial attitude matrix.Specifically, connecing
When receiving sensor-triggered instruction, the initial acceleration angle value of acceleration transducer output is obtained；Added according to initial
The systematic error of velocity amplitude and acceleration transducer, determines the angle of pitch and the roll angle；Additionally,
When receiving sensor-triggered instruction, the azimuth of Magnetic Sensor output is obtained；According to azimuth, the angle of pitch
With the initial attitude matrix that roll angle determines the terminal.
Step 403：The sampling magnitude of angular velocity of the gyroscope output of terminal is determined in current sample period, and is compensated
The sampling magnitude of angular velocity of determination.
Specifically, the random error of the gyroscope in being estimated according to a upper sampling period optimum attitude, and
The systematic error of gyroscope, the sampling magnitude of angular velocity of compensation current sample period gyroscope output.
Step 404：According to the sampling magnitude of angular velocity and initial attitude matrix of compensation, it is determined that updating attitude matrix.
Step 405：According to the sampling magnitude of angular velocity and renewal attitude matrix that determine compensation, by Kalman filtering
Determine the attitude optimal estimation value of terminal.
Specifically, the sampling magnitude of angular velocity according to compensation determines the predicted value of Kalman filter, according to renewal
Attitude matrix determines the measuring value of the Kalman filter；According to the measuring value amendment predicted value for determining, obtain
To the optimum attitude estimate of terminal.Wherein, optimum attitude estimate include acceleration transducer random error,
The random error and attitude error of gyroscope.Additionally, in being estimated according to current sample period optimum attitude
Gyroscope random error, and gyroscope systematic error, compensate next sampling period gyroscope defeated
The sampling magnitude of angular velocity for going out.
Based on same inventive concept, the embodiment of the present invention also provides a kind of device for determining terminal attitude, such as
It is a kind of device for determining terminal attitude provided in an embodiment of the present invention shown in Fig. 5, including：
Compensating module 501：For in current sample period, determining the sampling angular speed of the gyroscope output of terminal
Value, and compensate the sampling magnitude of angular velocity of determination；
Attitude matrix module 502：For sampling magnitude of angular velocity and initial attitude matrix according to compensation, it is determined that more
New attitude matrix；
Estimate determining module 503：For according to the sampling magnitude of angular velocity and the renewal appearance for determining compensation
State matrix, the attitude optimal estimation value of the terminal is determined by Kalman filtering.
Optionally, the initial attitude matrix determines according to following methods：
According to the systematic error of acceleration transducer, the angle of pitch and roll angle of terminal are determined；
According to the angle of pitch and roll angle of the terminal, the initial attitude matrix of the terminal is determined.
Optionally, the attitude matrix module 502, specifically for：
When sensor-triggered instruction is received, the initial acceleration angle value of the acceleration transducer output is obtained；
According to the initial acceleration angle value and the systematic error of the acceleration transducer, the angle of pitch is determined
With the roll angle；
When sensor-triggered instruction is received, the azimuth of Magnetic Sensor output is obtained；
The initial attitude matrix of the terminal is determined according to the azimuth, the angle of pitch and the roll angle.
Optionally, the estimate determining module 503, specifically for：
Sampling magnitude of angular velocity according to the compensation determines the predicted value of Kalman filter, according to the renewal
Attitude matrix determines the measuring value of the Kalman filter；
The predicted value according to the measuring value amendment for determining, obtains the optimum attitude estimate of the terminal.
Optionally, the optimum attitude of the terminal is estimated to include the random error of gyroscope；
The compensating module 501, specifically for：
The random error of the gyroscope in being estimated according to a upper sampling period optimum attitude, and gyro
The systematic error of instrument, the sampling magnitude of angular velocity of compensation current sample period gyroscope output.
Optionally, the compensating module 501, is additionally operable to：
When sensor calibration instruction is received, the verification acceleration of the acceleration transducer output is obtained
Value, the value according to the verification acceleration determines the systematic error of the acceleration transducer.
Optionally, the compensating module 501, is additionally operable to：
When sensor calibration instruction is received, the value of the verification angular speed of the gyroscope output, root are obtained
The systematic error of the gyroscope is determined according to the value of the verification angular speed.
The invention provides a kind of device for determining terminal attitude, in current sample period, the top of terminal is determined
The sampling magnitude of angular velocity of spiral shell instrument output, and compensate the sampling magnitude of angular velocity of determination；According to the sampling of compensation
Magnitude of angular velocity and initial attitude matrix, it is determined that updating attitude matrix；According to the sampling angle speed for determining compensation
Angle value and the renewal attitude matrix, the attitude optimal estimation value of the terminal is determined by Kalman filtering.
The device for determining terminal attitude provided in an embodiment of the present invention, improves the precision of low-speed motion navigation, reduces
The error of terminal Attitude estimation.
The present invention is with reference to method according to embodiments of the present invention, equipment (system) and computer program product
Flow chart and/or block diagram describe.It should be understood that can by computer program instructions realize flow chart and/
Or flow in each flow and/or square frame and flow chart and/or block diagram in block diagram and/or
The combination of square frame.These computer program instructions to all-purpose computer, special-purpose computer, embedded can be provided
The processor of processor or other programmable data processing devices is producing a machine so that by computer
Or the instruction of the computing device of other programmable data processing devices is produced for realizing in one stream of flow chart
The system of the function of being specified in journey or multiple one square frame of flow and/or block diagram or multiple square frames.
These computer program instructions may be alternatively stored in can guide computer or other programmable data processing devices
In the computer-readable memory for working in a specific way so that storage is in the computer-readable memory
Instruction is produced includes the manufacture of instruction system, and the instruction system is realized in one flow of flow chart or multiple streams
The function of being specified in one square frame of journey and/or block diagram or multiple square frames.
These computer program instructions can be also loaded into computer or other programmable data processing devices, made
Obtain and series of operation steps is performed on computer or other programmable devices to produce computer implemented place
Reason, so as to the instruction performed on computer or other programmable devices is provided for realizing in flow chart one
The step of function of being specified in flow or multiple one square frame of flow and/or block diagram or multiple square frames.
, but those skilled in the art once know base although preferred embodiments of the present invention have been described
This creative concept, then can make other change and modification to these embodiments.So, appended right will
Ask and be intended to be construed to include preferred embodiment and fall into having altered and changing for the scope of the invention.
Obviously, those skilled in the art can carry out various changes and modification without deviating from this hair to the present invention
Bright spirit and scope.So, if it is of the invention these modification and modification belong to the claims in the present invention and
Within the scope of its equivalent technologies, then the present invention is also intended to comprising these changes and modification.
Claims (14)
1. it is a kind of determine terminal attitude method, it is characterised in that including：
In current sample period, the sampling magnitude of angular velocity of the gyroscope output of terminal is determined, and compensate determination
The sampling magnitude of angular velocity；
According to the sampling magnitude of angular velocity and initial attitude matrix of compensation, it is determined that updating attitude matrix；
According to the sampling magnitude of angular velocity and the renewal attitude matrix for determining compensation, by Kalman filtering
Determine the attitude optimal estimation value of the terminal.
2. the method for claim 1, it is characterised in that the initial attitude matrix is according to lower section
Method determines：
According to the systematic error of acceleration transducer, the angle of pitch and roll angle of terminal are determined；
According to the angle of pitch and roll angle of the terminal, the initial attitude matrix of the terminal is determined.
3. method as claimed in claim 2, it is characterised in that the system according to acceleration transducer
Error, determines the angle of pitch and roll angle of terminal, including：
When sensor-triggered instruction is received, the initial acceleration angle value of the acceleration transducer output is obtained；
According to the initial acceleration angle value and the systematic error of the acceleration transducer, the angle of pitch is determined
With the roll angle；
The angle of pitch and roll angle according to the terminal, determines the initial attitude matrix of the terminal, bag
Include：
When sensor-triggered instruction is received, the azimuth of Magnetic Sensor output is obtained；
The initial attitude matrix of the terminal is determined according to the azimuth, the angle of pitch and the roll angle.
4. the method for claim 1, it is characterised in that the sampling angle speed according to the compensation
Angle value and the renewal attitude matrix, the attitude optimal estimation value of the terminal is determined by Kalman filtering,
Including：
Sampling magnitude of angular velocity according to the compensation determines the predicted value of Kalman filter, according to the renewal
Attitude matrix determines the measuring value of the Kalman filter；
The predicted value according to the measuring value amendment for determining, obtains the optimum attitude estimate of the terminal.
5. the method for claim 1, it is characterised in that the optimum attitude of the terminal is estimated to include
The random error of gyroscope；
The sampling magnitude of angular velocity that the compensation determines, including：
The random error of the gyroscope in being estimated according to a upper sampling period optimum attitude, and gyro
The systematic error of instrument, the sampling magnitude of angular velocity of compensation current sample period gyroscope output.
6. method as claimed in claim 2, it is characterised in that described in current sample period, it is determined that eventually
The sampling magnitude of angular velocity of the gyroscope output at end, and before compensating the sampling magnitude of angular velocity of determination, also wrap
Include：
When sensor calibration instruction is received, the verification acceleration of the acceleration transducer output is obtained
Value, the value according to the verification acceleration determines the systematic error of the acceleration transducer.
7. method as claimed in claim 5, it is characterised in that described in current sample period, it is determined that eventually
The sampling magnitude of angular velocity of the gyroscope output at end, and before compensating the sampling magnitude of angular velocity of determination, also wrap
Include：
When sensor calibration instruction is received, the value of the verification angular speed of the gyroscope output, root are obtained
The systematic error of the gyroscope is determined according to the value of the verification angular speed.
8. it is a kind of determine terminal attitude device, it is characterised in that including：
Compensating module：For in current sample period, determining the sampling magnitude of angular velocity of the gyroscope output of terminal,
And compensate the sampling magnitude of angular velocity of determination；
Attitude matrix module：For sampling magnitude of angular velocity and initial attitude matrix according to compensation, it is determined that updating
Attitude matrix；
Estimate determining module：For according to the sampling magnitude of angular velocity and the renewal attitude for determining compensation
Matrix, the attitude optimal estimation value of the terminal is determined by Kalman filtering.
9. device as claimed in claim 8, it is characterised in that the initial attitude matrix is according to lower section
Method determines：
According to the systematic error of acceleration transducer, the angle of pitch and roll angle of terminal are determined；
According to the angle of pitch and roll angle of the terminal, the initial attitude matrix of the terminal is determined.
10. device as claimed in claim 9, it is characterised in that the attitude matrix module, it is specific to use
In：
When sensor-triggered instruction is received, the initial acceleration angle value of the acceleration transducer output is obtained；
According to the initial acceleration angle value and the systematic error of the acceleration transducer, the angle of pitch is determined
With the roll angle；
When sensor-triggered instruction is received, the azimuth of Magnetic Sensor output is obtained；
The initial attitude matrix of the terminal is determined according to the azimuth, the angle of pitch and the roll angle.
11. devices as claimed in claim 8, it is characterised in that the estimate determining module, specifically
For：
Sampling magnitude of angular velocity according to the compensation determines the predicted value of Kalman filter, according to the renewal
Attitude matrix determines the measuring value of the Kalman filter；
The predicted value according to the measuring value amendment for determining, obtains the optimum attitude estimate of the terminal.
12. devices as claimed in claim 8, it is characterised in that the optimum attitude of the terminal estimates bag
Include the random error of gyroscope；
The compensating module, specifically for：
The random error of the gyroscope in being estimated according to a upper sampling period optimum attitude, and gyro
The systematic error of instrument, the sampling magnitude of angular velocity of compensation current sample period gyroscope output.
13. devices as claimed in claim 9, it is characterised in that the compensating module, are additionally operable to：
When sensor calibration instruction is received, the verification acceleration of the acceleration transducer output is obtained
Value, the value according to the verification acceleration determines the systematic error of the acceleration transducer.
14. devices as claimed in claim 12, it is characterised in that the compensating module, are additionally operable to：
When sensor calibration instruction is received, the value of the verification angular speed of the gyroscope output, root are obtained
The systematic error of the gyroscope is determined according to the value of the verification angular speed.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201511028441.3A CN106931965B (en) | 2015-12-31 | 2015-12-31 | Method and device for determining terminal posture |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201511028441.3A CN106931965B (en) | 2015-12-31 | 2015-12-31 | Method and device for determining terminal posture |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN106931965A true CN106931965A (en) | 2017-07-07 |
| CN106931965B CN106931965B (en) | 2021-04-13 |
Family
ID=59442018
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201511028441.3A Active CN106931965B (en) | 2015-12-31 | 2015-12-31 | Method and device for determining terminal posture |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN106931965B (en) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108534744A (en) * | 2018-01-30 | 2018-09-14 | 歌尔科技有限公司 | A kind of attitude angle acquisition methods, device and handle |
| CN109085564A (en) * | 2018-08-31 | 2018-12-25 | 北京邮电大学 | A kind of localization method and device |
| CN109550219A (en) * | 2018-11-30 | 2019-04-02 | 歌尔科技有限公司 | A kind of determination method, system and the mobile device of motion information |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103822633A (en) * | 2014-02-11 | 2014-05-28 | 哈尔滨工程大学 | Low-cost attitude estimation method based on second-order measurement update |
| CN104501796A (en) * | 2014-12-16 | 2015-04-08 | 重庆邮电大学 | Indoor WLAN/MEMS fusion cross-stair three-dimensional positioning method |
| CN104697526A (en) * | 2015-03-26 | 2015-06-10 | 上海华测导航技术股份有限公司 | Strapdown inertial navitation system and control method for agricultural machines |
| CN105203098A (en) * | 2015-10-13 | 2015-12-30 | 上海华测导航技术股份有限公司 | Whole attitude angle updating method applied to agricultural machinery and based on nine-axis MEMS (micro-electromechanical system) sensor |
-
2015
- 2015-12-31 CN CN201511028441.3A patent/CN106931965B/en active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103822633A (en) * | 2014-02-11 | 2014-05-28 | 哈尔滨工程大学 | Low-cost attitude estimation method based on second-order measurement update |
| CN104501796A (en) * | 2014-12-16 | 2015-04-08 | 重庆邮电大学 | Indoor WLAN/MEMS fusion cross-stair three-dimensional positioning method |
| CN104697526A (en) * | 2015-03-26 | 2015-06-10 | 上海华测导航技术股份有限公司 | Strapdown inertial navitation system and control method for agricultural machines |
| CN105203098A (en) * | 2015-10-13 | 2015-12-30 | 上海华测导航技术股份有限公司 | Whole attitude angle updating method applied to agricultural machinery and based on nine-axis MEMS (micro-electromechanical system) sensor |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108534744A (en) * | 2018-01-30 | 2018-09-14 | 歌尔科技有限公司 | A kind of attitude angle acquisition methods, device and handle |
| CN109085564A (en) * | 2018-08-31 | 2018-12-25 | 北京邮电大学 | A kind of localization method and device |
| CN109550219A (en) * | 2018-11-30 | 2019-04-02 | 歌尔科技有限公司 | A kind of determination method, system and the mobile device of motion information |
Also Published As
| Publication number | Publication date |
|---|---|
| CN106931965B (en) | 2021-04-13 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN104567931B (en) | A kind of heading effect error cancelling method of indoor inertial navigation positioning | |
| CN107478223A (en) | A kind of human body attitude calculation method based on quaternary number and Kalman filtering | |
| US10197587B2 (en) | Device and method for using time rate of change of sensor data to determine device rotation | |
| CN104969030B (en) | Inertial device, methods and procedures | |
| CN110398245B (en) | Indoor pedestrian navigation attitude estimation method based on foot-worn inertial measurement unit | |
| CN109163721A (en) | Attitude measurement method and terminal device | |
| CN105674984B (en) | A method of acquisition for mobile terminal pedestrian advancing direction is used without constraint | |
| CN108458714B (en) | Euler angle solving method without gravity acceleration in attitude detection system | |
| WO2014134710A1 (en) | Method and apparatus for fast magnetometer calibration | |
| CN106370178B (en) | Attitude measurement method and device of mobile terminal equipment | |
| CN103776446A (en) | Pedestrian autonomous navigation calculation algorithm based on MEMS-IMU | |
| US10627237B2 (en) | Offset correction apparatus for gyro sensor, recording medium storing offset correction program, and pedestrian dead-reckoning apparatus | |
| CN107830861A (en) | Based on adaptive gain complementary filter moving object attitude measurement method and device | |
| CN107255474B (en) | PDR course angle determination method integrating electronic compass and gyroscope | |
| CN103017763A (en) | State estimation apparatus and offset update method | |
| CN106705959A (en) | Method and device for detecting course of mobile terminal | |
| CN103512584A (en) | Navigation attitude information output method, device and strapdown navigation attitude reference system | |
| CN104266648A (en) | Indoor location system based on Android platform MARG sensor | |
| CN109916395A (en) | A kind of autonomous Fault-tolerant Integrated navigation algorithm of posture | |
| CN108426573A (en) | A kind of the pedestrian's gait detection method and terminal device of terminal device | |
| JP5809416B2 (en) | Mobile devices and autonomous navigation calculation | |
| CN108318027A (en) | The determination method and apparatus of the attitude data of carrier | |
| CN104296780B (en) | A kind of SINS autoregistrations based on gravity apparent motion and latitude computational methods | |
| CN106931965A (en) | A kind of method and device for determining terminal attitude | |
| US10466054B2 (en) | Method and system for estimating relative angle between headings |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |