CN110095118A - A kind of method for real-time measurement and system at body gesture angle - Google Patents
A kind of method for real-time measurement and system at body gesture angle Download PDFInfo
- Publication number
- CN110095118A CN110095118A CN201910476459.1A CN201910476459A CN110095118A CN 110095118 A CN110095118 A CN 110095118A CN 201910476459 A CN201910476459 A CN 201910476459A CN 110095118 A CN110095118 A CN 110095118A
- Authority
- CN
- China
- Prior art keywords
- angle
- matrix
- attitude
- differential equation
- quaternary number
- 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.)
- Pending
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 the method for real-time measurement and system at a kind of body gesture angle.The method for real-time measurement includes: the kinematic parameter obtained in vehicle driving；Direction cosine matrix is determined according to the kinematic parameter；Attitude matrix is determined according to the direction cosine matrix；Determine quaternion differential equation；The attitude matrix is described according to the quaternion differential equation, the attitude matrix after determining description；The attitude angle of different moments is determined according to the attitude matrix after the description；Obtain the acceleration in vehicle driving；Using the quaternary number of the quaternion differential equation as state variable, using the acceleration as observed quantity, and noise statistics information is added, correction is optimized to the attitude angle using Kalman filter, determines optimum posture angle.Vehicle attitude angular measurement precision can be improved using method for real-time measurement provided by the present invention and system, reduce calculation amount.
Description
Technical field
The present invention relates to body gesture angle real-time measurement fields, more particularly to a kind of real-time measurement side at body gesture angle
Method and system.
Background technique
Attitude angle can be understood as the rotation of coordinate system to another coordinate system, determines different coordinate systems, asks
The attitude angle of solution can difference.Carrier coordinate system and navigational coordinate system are generallyd use in the use of inertia device；Navigation is sat
It marks used coordinate system, majority when system is inertial navigation system solution navigational parameter and directlys adopt geographic coordinate system.Carrier
Posture and course are exactly the relationship between established carrier coordinate system and navigational coordinate system, it can be understood as the fixed point rotary of rigid body is managed
By usually being indicated with roll angle, pitch angle and course angle；Roll angle refers to tilt angle of the vehicle body around the longitudinal axis, and pitch angle refers to
For vehicle body around the tilt angle of horizontal axis, course angle refers to the angle in vehicular longitudinal velocity direction Yu geographic meridian north.
Inertial navigation system includes gimbaled inertial navigation system and strap-down inertial navigation system.Gimbaled inertial navigation
System is to use physical platform analogue navigation coordinate system, and attitude algorithm process is fairly simple, but because structure is complicated for physical platform, volume
Greatly, it is not appropriate for being installed on vehicle.Strap-down inertial navigation system does not have actual physical platform, small in size, posture square
Battle array is equivalent to the mathematical platform of simulation, so guaranteeing the accuracy of mathematical platform, optimization strapdown inertial is mainly asking for system
It inscribes, the real-time update of attitude matrix and navigation calculation are crucial in entire algorithm.The algorithm of the description attitude matrix differential equation has
Eulerian angles and direction cosines.The attitude matrix differential equation process of Eulerian angles description is simple and clear, it can be readily appreciated that and acquiring
Attitude matrix be it is orthogonal, after being coordinately transformed to information be not present non-orthogonal errors, but its description equation in deposit
In trigonometric function, calculating is become difficult, and when pitch angle is 90 °, singular point occurs in the differential equation, make the movement model of carrier
Enclose and be restricted, thus for full posture work such method of carrier and be not suitable for.The differential side of direction cosine matrix description
Journey can make carrier work at 90 °, realize full attitude measurement, but solving nine equations simultaneously makes calculation amount become larger, to practical work
For journey and it is not suitable for.
Summary of the invention
The object of the present invention is to provide the method for real-time measurement and system at a kind of body gesture angle, to solve vehicle attitude angle
Measure low, the computationally intensive problem of precision.
To achieve the above object, the present invention provides following schemes:
A kind of method for real-time measurement at body gesture angle, comprising:
Obtain the kinematic parameter in vehicle driving；The kinematic parameter includes roll angle, pitch angle and course angle；
Direction cosine matrix is determined according to the kinematic parameter；
Attitude matrix is determined according to the direction cosine matrix；The attitude matrix is carrier coordinate system and navigational coordinate system
Transition matrix；
Determine quaternion differential equation；
The attitude matrix is described according to the quaternion differential equation, the attitude matrix after determining description；
The attitude angle of different moments is determined according to the attitude matrix after the description；
Obtain the acceleration in vehicle driving；
Using the quaternary number of the quaternion differential equation as state variable, using the acceleration as observed quantity, and add
Enter noise statistics information, correction is optimized to the attitude angle using Kalman filter, determines optimum posture angle.
Optionally, described that direction cosine matrix is determined according to the kinematic parameter, it specifically includes:
According to formulaReally
Determine direction cosine matrix；Wherein, ψ is roll angle；θ is pitch angle；γ is course angle；For direction cosine matrix.
Optionally, the determining quaternion differential equation, specifically includes:
According to formulaDetermine quaternary fractional differentiation side
Journey；Wherein, q0(k+1)、q1(k+1)、q2(k+1)、q3It (k+1) is the value of (k+1) moment quaternary number, q0(k)、q1(k)、q2(k)、
q3It (k) is the value of k moment quaternary number, Δ θxFor the rotation angle in the unit time in the direction of the x axis；ΔθyFor in the y-axis direction
Rotation angle in unit time；ΔθzFor the rotation angle in the unit time in the z-axis direction.
Optionally, described that the attitude matrix is described according to the quaternion differential equation, the posture square after determining description
Battle array, specifically includes:
Quaternary number is solved according to the quaternion differential equation；
Obtain three axis angular rates；
Quaternary number is updated according to three axis angular rate, determines updated quaternary number；
Attitude matrix according to attitude matrix described in the updated quaternion representation, after determining description.
A kind of real-time measurement system at body gesture angle, comprising:
Kinematic parameter obtains module, for obtaining the kinematic parameter in vehicle driving；The kinematic parameter include roll angle,
Pitch angle and course angle；
Direction cosine matrix determining module, for determining direction cosine matrix according to the kinematic parameter；
Attitude matrix determining module, for determining attitude matrix according to the direction cosine matrix；The attitude matrix is
The transition matrix of carrier coordinate system and navigational coordinate system；
Quaternion differential equation determining module, for determining quaternion differential equation；
Describing module describes the attitude matrix according to the quaternion differential equation, the attitude matrix after determining description；
Attitude angle determining module determines the attitude angle of different moments according to the attitude matrix after the description；
Attitude angle determining module, for determining the attitude angle of different moments according to the quaternion differential equation；
Acceleration obtains module, for obtaining the acceleration in vehicle driving；
Optimum posture angle determining module, for using the quaternary number of the quaternion differential equation as state variable, with institute
Acceleration is stated as observed quantity, and noise statistics information is added, school is optimized to the attitude angle using Kalman filter
Just, optimum posture angle is determined.
Optionally, the direction cosine matrix determining module specifically includes:
Direction cosine matrix determination unit, for according to formulaDetermine direction cosine matrix；
Wherein, ψ is roll angle；θ is pitch angle, γ is course angle；For direction cosine matrix.
Optionally, the quaternion differential equation determining module specifically includes:
Quaternion differential equation determination unit, for according to formulaDetermine quaternion differential equation；Wherein, q0(k+
1)、q1(k+1)、q2(k+1)、q3It (k+1) is the value of (k+1) moment quaternary number, q0(k)、q1(k)、q2(k)、q3It (k) is the k moment
The value of quaternary number, Δ θxFor the rotation angle in the unit time in the direction of the x axis；ΔθyFor in the unit time in the y-axis direction
Rotate angle；ΔθzFor the rotation angle in the unit time in the z-axis direction.
Optionally, the attitude angle determining module specifically includes:
Quaternary number solves unit, for solving quaternary number according to the quaternion differential equation；
Three axis angular rate acquiring units, for obtaining three axis angular rates；
Quaternary number updating unit determines updated quaternary number for updating quaternary number according to three axis angular rate；
Attitude angle determination unit is used for the attitude matrix according to the updated quaternion representation, after determining description
Attitude matrix.
The specific embodiment provided according to the present invention, the invention discloses following technical effects: the present invention provides one kind
The method for real-time measurement and system at body gesture angle, by describing attitude matrix using Quaternion Algorithm, compared to Euler's horn cupping
And direction cosine method, Quaternion Method only need to solve the differential equation of four unknown quantitys, calculate simply, convenient for operation, drop significantly
Low calculation amount；Again using the quaternary number of the quaternion differential equation as state variable, using the acceleration as observed quantity,
And noise statistics information is added, correction is optimized to the attitude angle using Kalman filter, determines optimum posture angle, from
And noise statistics information has been filtered out, the convergence rate at optimum posture angle is improved, calculating error is reduced, improves vehicle attitude
Angular measurement precision.
Detailed description of the invention
It in order to more clearly explain the embodiment of the invention or the technical proposal in the existing technology, below will be to institute in embodiment
Attached drawing to be used is needed to be briefly described, it should be apparent that, the accompanying drawings in the following description is only some implementations of the invention
Example, for those of ordinary skill in the art, without any creative labor, can also be according to these attached drawings
Obtain other attached drawings.
Fig. 1 is the method for real-time measurement flow chart at body gesture angle provided by the present invention；
Fig. 2 is the real-time measurement system structure chart at body gesture angle provided by the present invention；
Fig. 3 is static roll angle contrast curve chart provided by the present invention；
Fig. 4 is static pitch angle contrast curve chart provided by the present invention；
Fig. 5 is dynamic roll angle contrast curve chart provided by the present invention；
Fig. 6 is dynamic pitch angle contrast curve chart provided by the present invention；
Fig. 7 is roll angle error curve diagram provided by the present invention；
Fig. 8 is pitch angle error curve diagram provided by the present invention.
Specific embodiment
Following will be combined with the drawings in the embodiments of the present invention, and technical solution in the embodiment of the present invention carries out clear, complete
Site preparation description, it is clear that described embodiments are only a part of the embodiments of the present invention, instead of all the embodiments.It is based on
Embodiment in the present invention, it is obtained by those of ordinary skill in the art without making creative efforts every other
Embodiment shall fall within the protection scope of the present invention.
The object of the present invention is to provide the method for real-time measurement and system at a kind of body gesture angle, can be improved vehicle attitude
Angular measurement precision reduces calculation amount.
In order to make the foregoing objectives, features and advantages of the present invention clearer and more comprehensible, with reference to the accompanying drawing and specific real
Applying mode, the present invention is described in further detail.
Fig. 1 is the method for real-time measurement flow chart at body gesture angle provided by the present invention, as shown in Figure 1, a kind of vehicle body
The method for real-time measurement of attitude angle, comprising:
Step 101: obtaining the kinematic parameter in vehicle driving；The kinematic parameter includes roll angle, pitch angle and boat
To angle.
Step 102: direction cosine matrix is determined according to the kinematic parameter.
Step 103: attitude matrix is determined according to the direction cosine matrix；The attitude matrix be carrier coordinate system with lead
The transition matrix of boat coordinate system.
Transformational relation between new coordinate system and original coordinate system is direction cosine matrix C, and attitude matrix represents carrier seat
Mark system arrives the transition matrix of navigational coordinate system, is the transposition of direction cosine matrix.As long as obtaining attitude matrix, according to each in matrix
The relationship of element can acquire Eulerian angles.According to the rotation relationship between two coordinate systems, it is as follows to establish direction cosine matrix:
Wherein, CγFor around the postrotational direction cosine matrix of z-axis, CθFor around the postrotational direction cosine matrix of y-axis, CψFor
Direction cosine matrix after rotating around x axis, ψ, θ, γ respectively represent roll angle, pitch angle and course angle.
Attitude matrixFor the transposition of direction cosine matrix:
Formula (2) are indicated are as follows:
By analysis mode (2) it is found that if all inner elements of attitude matrix can be obtained, attitude angle calculates as follows:
Step 104: determining quaternion differential equation.
Step 105: the attitude matrix being described according to the quaternion differential equation, the attitude matrix after determining description.
Step 106: the attitude angle of different moments is determined according to the attitude matrix after the description.
Rotation for three-dimensional space, i.e., with three variables go to indicate object towards when to will appear universal joint deadlock existing
As；Universal joint deadlock situation refers to that different coordinate representations occurs in same position in three dimensions, and Quaternion Algorithm can
Effectively avoid this phenomenon；Quaternary number, which is equivalent to, is introduced into system in more higher dimensional space, plural number can be analogized to, if handle
If plural number regards two-dimensional space as, quaternary number is equivalent to space-time.Quaternary number is made of three imaginary parts and a real part.
The ξ it is assumed that axis that a certain vector bypasses origin o rotates a certain angle, then between the coordinate system and reference frame
Transformation quaternary number indicate are as follows:
New point is indicated with quaternary number are as follows:
For vehicle carrier coordinate system and navigational coordinate system, the pass between it is described with quaternary number and direction cosine matrix
System are as follows:
Write as the form of quaternionic matrix are as follows:
q(Rb)=M (q*)M*(q)q(Rn) (9)
Wherein: q (Rb)=[0 xb yb zb]T, q (Rn)=[0 xn yn zn]T
Transformational relation between the final carrier coordinate system and navigational coordinate system obtained with quaternion representation are as follows:
The expression formula of quaternion differential equation are as follows:
Wherein,For along the angular speed of carrier.
The accurate solution of the differential equation is solved with approximatioss are as follows:
Above formula is carried out being unfolded only to choose single order item, obtains an order algorithm of quaternion differential equation are as follows:
Wherein, Δ θxFor the rotation angle in the unit time in the direction of the x axis；ΔθyFor in the unit time in the y-axis direction
Rotation angle；ΔθzFor the rotation angle in the unit time in the z-axis direction；I is unit matrix.
Compared to Euler's horn cupping and direction cosine method, Quaternion Method only needs to solve the differential equation of four unknown quantitys, meter
It calculates simply, convenient for operation.
Step 107: obtaining the acceleration in vehicle driving.
Step 108: using the quaternary number of the quaternion differential equation as state variable, using the acceleration as observation
Amount, and noise statistics information is added, correction is optimized to the attitude angle using Kalman filter, determines optimum posture
Angle.
ADIS16445 sensor directly exports acceleration and angular speed information, and angular velocity carries out integrating available angle
Angle value, since gyroscope generates drift error vulnerable to the influence of temperature and destabilizing moment, and with the growth of time, accidentally
Difference can be increasing, so magnitude of angular velocity is credible in short term；The survey that influences short time in of the accelerometer vulnerable to state of motion of vehicle
Magnitude does not have credibility, therefore exclusive use accelerometer or gyroscope are unable to calculate optimum attitude angle；Due to outer
The interference of portion's noise makes the Eulerian angles finally resolved there is very big error, and convergence rate is slow；Therefore it is added in the entire system
Extended Kalman filter.
Kalman filter is a kind of linear minimum-variance estimation method, and state space is introduced into random theory, whole
White noise is considered as input by a linear system, describes input/output relation with state equation；Algorithm is recursion, is utilized
Information be in time domain, so can not only estimate stable unidimensional system, can also estimate non-stationary, it is more
The random process of dimension.
Since entire inertial navigation system is nonlinear system, Extended Kalman filter is introduced.Spreading kalman
Filtering is the processing method for linearizing the error of whole system, and nonlinear function is launched into Taylor's grade around filter value
Number omits higher order term, essence or Kalman filtering.The advantages of algorithm is need not to precalculate nominal trajectory, but it is only fitted
For the lesser situation of filtering error.
The state equation and observational equation established for different control systems gap, in the present system with quaternary
Number is used as state variable, using the acceleration as observed quantity, and the statistical information of system noise is added.By the acceleration of measurement
As the input of filter, constantly system is corrected.
Wherein Δ ω is Attitude rate estimator value, and δ is error, Δ ω=ω-δ.
δ=[δx δy δz]T (17)
It is obtained after above formula is unfolded:
Using carrier three axis directions acceleration as observed quantity:
Z=[ax ay az]Τ (19)
Under geographic coordinate system, 3-axis acceleration immobilizes, and is acceleration of gravity in z-axis direction, other two axis are 0.
Acceleration is indicated under carrier coordinate system:
The state equation and observational equation finally obtained is as follows:
Extended Kalman filter (ExtendedKalman Filter, EKF) includes two calculating process on the whole: when
Between update and measurement updaue.It includes the prior state value and mistake that current time is estimated according to the calculated value of last moment that time, which updates,
Poor covariance.Measurement updaue, which refers to, works as according to the observation at current time, prior state estimated value and error covariance to calculate
State value, filtering gain matrix and the error matrix at preceding moment.Entire algorithm belongs to the process of continuous circular recursion, as long as can
The estimated value and the observation at current time that obtain last moment state variable can calculate the state variable at current time
Estimated value.Extended Kalman filter controller effectively eliminates the interference of external noise, so that system has been reached good and follows
Effect.Recursive Formulas of Kalman Filter:
(1) state one-step prediction equation
Xk,k-1=Φk,k-1Xk-1 (23)
(2) one-step prediction mean square error matrix
Pk,k-1=Φk,k-1Pk-1Φk,k-1 Τ+Γk-1Qk-1Γk-1 Τ (24)
(3) filtering gain matrix
(4) state estimation accounting equation
Xk=Xk,k-1+Kk(Zk-HkXk,k-1) (26)
(5) estimate Square Error matrix
Pk=(I-KkHk)Pk,k-1 (27)
Fig. 2 is the real-time measurement system structure chart at body gesture angle provided by the present invention, as shown in Fig. 2, a kind of vehicle body
The real-time measurement system of attitude angle, comprising:
Kinematic parameter obtains module 201, for obtaining the kinematic parameter in vehicle driving；The kinematic parameter includes roll
Angle, pitch angle and course angle.
Direction cosine matrix determining module 202, for determining direction cosine matrix according to the kinematic parameter.
The direction cosine matrix determining module 202 specifically includes: direction cosine matrix determination unit, for according to formulaDetermine direction cosine matrix；
Wherein, ψ is roll angle；θ is pitch angle, γ is course angle；For direction cosine matrix.
Attitude matrix determining module 203, for determining attitude matrix according to the direction cosine matrix；The attitude matrix
For the transition matrix of carrier coordinate system and navigational coordinate system.
Quaternion differential equation determining module 204, for determining quaternion differential equation.
The quaternion differential equation determining module 204 specifically includes: quaternion differential equation determination unit, is used for basis
FormulaDetermine quaternion differential equation；Wherein, q0
(k+1)、q1(k+1)、q2(k+1)、q3It (k+1) is the value of (k+1) moment quaternary number, q0(k)、q1(k)、q2(k)、q3(k) be k when
Carve the value of quaternary number, Δ θxFor the rotation angle in the unit time in the direction of the x axis；ΔθyFor in the unit time in the y-axis direction
Rotation angle；ΔθzFor the rotation angle in the unit time in the z-axis direction.
Describing module 205, for describing the attitude matrix according to the quaternion differential equation, the appearance after determining description
State matrix.
Attitude angle determining module 206, for determining the attitude angle of different moments according to the attitude matrix after the description.
The attitude angle determining module 206 specifically includes: quaternary number solves unit, for according to the quaternary fractional differentiation side
Journey solves quaternary number；Three axis angular rate acquiring units, for obtaining three axis angular rates；Quaternary number updating unit, for according to institute
It states three axis angular rates and updates quaternary number, determine updated quaternary number；Attitude angle determination unit, for according to described updated
Attitude matrix described in quaternion representation, the attitude matrix after determining description.
Acceleration obtains module 207, for obtaining the acceleration in vehicle driving.
Optimum posture angle determining module 208, for using the quaternary number of the quaternion differential equation as state variable, with
Noise statistics information is added as observed quantity in the acceleration, is optimized using Kalman filter to the attitude angle
Correction, determines optimum posture angle.
After the completion of whole system is resolved, in order to verify the accuracy at final carriage angle, by ADIS16445 and MTi-
30 sensors compare verifying.MTi-30 is attitude heading reference system (AHRS), includes Embedded attitude algorithm unit, can
Export rift-free roll, pitch angle.In static state, the typical error of roll angle and pitch angle is 0.2 °, and worst error is no more than
0.4 °, typical error is no more than 0.5 ° when dynamic, as shown in table 1- table 2.
Table 1
Table 2
Table 1 is gyroscope performance comparison table provided by the present invention, and table 2 is accelerometer performance pair provided by the present invention
Than table, the specifications parameter that Tables 1 and 2 is ADIS16445 and MTi-30 is compared, the gyroscope and accelerometer gamut of MTi-30
Larger, the movement of vehicle is in lower-speed state, so this experiment sensor gamut can satisfy requirement.Gyroscope it is non-linear
Refer within the scope of turning rate input, the ratio between the maximum deviation of the fitting a straight line of output quantity and gamut, it is same non-linear
When, this experiment sensor error is smaller.In contrast the noise intensity of gyroscope, g- sensitivity, operation bias stability and partially
The performance of poor repeatability is very close.The accelerometer of ADIS16445 is in contrast in non-linear, noise intensity, operation deviation
There is better performance in terms of stability.
Experimental result is verified, carries out the comparison of static and dynamic angle respectively.
Table 3 is static pitching angle error contrast table provided by the present invention, and table 4 is static pitch angle provided by the present invention
MTi-30 and ADIS16445 are fixed, are rotated a certain angle as shown in table 3- table 4 by error contrast table, using MTi-30 as base
Standard, the angular error of ADIS16445 and MTi-30 when measurement is static.
Table 3
Table 4
Fig. 3 is static roll angle contrast curve chart provided by the present invention, and Fig. 4 is static pitch angle provided by the present invention
Contrast curve chart, Fig. 3-Fig. 4 are a period of time inner sensor acquisition data result after given special angle, as can be seen from the figure
Static system angular error is smaller, and has good stationarity；Fig. 5 is dynamic roll angle correlation curve provided by the present invention
Figure, Fig. 6 is dynamic pitch angle contrast curve chart provided by the present invention, as shown in Fig. 5-Fig. 6.
Angular error is maintained within 0.5 ° when can be seen that the two dynamic by Fig. 7 and Fig. 8, and passes through two kinds of sensings of comparison
The specifications parameter of device, ADIS16445 have better performance in some aspects, can satisfy actual demand, while also demonstrating
The feasibility of algorithm.
Each embodiment in this specification is described in a progressive manner, the highlights of each of the examples are with other
The difference of embodiment, the same or similar parts in each embodiment may refer to each other.For system disclosed in embodiment
For, since it is corresponded to the methods disclosed in the examples, so being described relatively simple, related place is said referring to method part
It is bright.
Used herein a specific example illustrates the principle and implementation of the invention, and above embodiments are said
It is bright to be merely used to help understand method and its core concept of the invention；At the same time, for those skilled in the art, foundation
Thought of the invention, there will be changes in the specific implementation manner and application range.In conclusion the content of the present specification is not
It is interpreted as limitation of the present invention.
Claims (8)
1. a kind of method for real-time measurement at body gesture angle characterized by comprising
Obtain the kinematic parameter in vehicle driving；The kinematic parameter includes roll angle, pitch angle and course angle；
Direction cosine matrix is determined according to the kinematic parameter；
Attitude matrix is determined according to the direction cosine matrix；The attitude matrix is the mistake of carrier coordinate system and navigational coordinate system
Cross matrix；
Determine quaternion differential equation；
The attitude matrix is described according to the quaternion differential equation, the attitude matrix after determining description；
The attitude angle of different moments is determined according to the attitude matrix after the description；
Obtain the acceleration in vehicle driving；
Using the quaternary number of the quaternion differential equation as state variable, using the acceleration as observed quantity, and it is added and makes an uproar
Sound statistical information optimizes correction to the attitude angle using Kalman filter, determines optimum posture angle.
2. the method for real-time measurement at body gesture angle according to claim 1, which is characterized in that described according to the movement
Parameter determines direction cosine matrix, specifically includes:
According to formulaDetermination side
To cosine matrix；Wherein, ψ is roll angle；θ is pitch angle；γ is course angle；For direction cosine matrix.
3. the method for real-time measurement at body gesture angle according to claim 1, which is characterized in that the determining quaternary number is micro-
Equation is divided to specifically include:
According to formulaDetermine quaternion differential equation；Its
In, q0(k+1)、q1(k+1)、q2(k+1)、q3It (k+1) is the value of (k+1) moment quaternary number, q0(k)、q1(k)、q2(k)、q3(k)
For the value of k moment quaternary number, Δ θxFor the rotation angle in the unit time in the direction of the x axis；ΔθyFor unit in the y-axis direction
Rotation angle in time；ΔθzFor the rotation angle in the unit time in the z-axis direction.
4. the method for real-time measurement at body gesture angle according to claim 1, which is characterized in that described according to the quaternary
Fractional differentiation equation describes the attitude matrix, and the attitude matrix after determining description specifically includes:
Quaternary number is solved according to the quaternion differential equation；
Obtain three axis angular rates；
Quaternary number is updated according to three axis angular rate, determines updated quaternary number；
Attitude matrix according to attitude matrix described in the updated quaternion representation, after determining description.
5. a kind of real-time measurement system at body gesture angle characterized by comprising
Kinematic parameter obtains module, for obtaining the kinematic parameter in vehicle driving；The kinematic parameter includes roll angle, pitching
Angle and course angle；
Direction cosine matrix determining module, for determining direction cosine matrix according to the kinematic parameter；
Attitude matrix determining module, for determining attitude matrix according to the direction cosine matrix；The attitude matrix is carrier
The transition matrix of coordinate system and navigational coordinate system；
Quaternion differential equation determining module, for determining quaternion differential equation；
Describing module describes the attitude matrix according to the quaternion differential equation, the attitude matrix after determining description；
Attitude angle determining module determines the attitude angle of different moments according to the attitude matrix after the description；
Attitude angle determining module, for determining the attitude angle of different moments according to the quaternion differential equation；
Acceleration obtains module, for obtaining the acceleration in vehicle driving；
Optimum posture angle determining module, for being added with described using the quaternary number of the quaternion differential equation as state variable
Noise statistics information is added as observed quantity in speed, optimizes correction to the attitude angle using Kalman filter, really
Determine optimum posture angle.
6. the real-time measurement system at body gesture angle according to claim 5, which is characterized in that the direction cosine matrix
Determining module specifically includes:
Direction cosine matrix determination unit, for according to formulaDetermine direction cosine matrix；
Wherein, ψ is roll angle；θ is pitch angle；γ is course angle；For direction cosine matrix.
7. the real-time measurement system at body gesture angle according to claim 5, which is characterized in that the quaternary fractional differentiation side
Journey determining module specifically includes:
Quaternion differential equation determination unit, for according to formulaDetermine quaternion differential equation；Wherein, q0(k+
1)、q1(k+1)、q2(k+1)、q3It (k+1) is the value of (k+1) moment quaternary number, q0(k)、q1(k)、q2(k)、q3It (k) is the k moment
The value of quaternary number, Δ θxFor the rotation angle in the unit time in the direction of the x axis；ΔθyFor in the unit time in the y-axis direction
Rotate angle；ΔθzFor the rotation angle in the unit time in the z-axis direction.
8. the real-time measurement system at body gesture angle according to claim 5, which is characterized in that the attitude angle determines mould
Block specifically includes:
Quaternary number solves unit, for solving quaternary number according to the quaternion differential equation；
Three axis angular rate acquiring units, for obtaining three axis angular rates；
Quaternary number updating unit determines updated quaternary number for updating quaternary number according to three axis angular rate；
Attitude angle determination unit is used for the attitude matrix according to the updated quaternion representation, the appearance after determining description
State matrix.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910476459.1A CN110095118A (en) | 2019-06-03 | 2019-06-03 | A kind of method for real-time measurement and system at body gesture angle |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910476459.1A CN110095118A (en) | 2019-06-03 | 2019-06-03 | A kind of method for real-time measurement and system at body gesture angle |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN110095118A true CN110095118A (en) | 2019-08-06 |
Family
ID=67450083
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910476459.1A Pending CN110095118A (en) | 2019-06-03 | 2019-06-03 | A kind of method for real-time measurement and system at body gesture angle |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110095118A (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114981616A (en) * | 2020-01-29 | 2022-08-30 | 舍弗勒技术股份两合公司 | Clutch actuator, detection system and method for detecting the angular position of a rotating part |
| CN115406405A (en) * | 2022-04-15 | 2022-11-29 | 汉博来自控科技(上海)有限公司 | Solution for level sensor of forklift truck |
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 |
| CN106643737A (en) * | 2017-02-07 | 2017-05-10 | 大连大学 | Four-rotor aircraft attitude calculation method in wind power interference environments |
| CN106979779A (en) * | 2017-05-22 | 2017-07-25 | 深圳市靖洲科技有限公司 | A kind of unmanned vehicle real-time attitude measuring method |
| CN106979780A (en) * | 2017-05-22 | 2017-07-25 | 深圳市靖洲科技有限公司 | A kind of unmanned vehicle real-time attitude measuring method |
-
2019
- 2019-06-03 CN CN201910476459.1A patent/CN110095118A/en active Pending
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 |
| CN106643737A (en) * | 2017-02-07 | 2017-05-10 | 大连大学 | Four-rotor aircraft attitude calculation method in wind power interference environments |
| CN106979779A (en) * | 2017-05-22 | 2017-07-25 | 深圳市靖洲科技有限公司 | A kind of unmanned vehicle real-time attitude measuring method |
| CN106979780A (en) * | 2017-05-22 | 2017-07-25 | 深圳市靖洲科技有限公司 | A kind of unmanned vehicle real-time attitude measuring method |
Non-Patent Citations (2)
| Title |
|---|
| 刘琼: "微型航姿系统的设计与姿态解算算法研究", 《中国优秀硕士学位论文全文数据库信息科技辑》 * |
| 吕辰: "基于MARG无人机姿态测量系统的实时融合算法研究", 《中国优秀硕士学位论文全文数据库工程科技Ⅱ辑》 * |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114981616A (en) * | 2020-01-29 | 2022-08-30 | 舍弗勒技术股份两合公司 | Clutch actuator, detection system and method for detecting the angular position of a rotating part |
| CN114981616B (en) * | 2020-01-29 | 2024-03-15 | 舍弗勒技术股份两合公司 | Clutch actuator, detection system and method for detecting an angular position of a rotating component |
| CN115406405A (en) * | 2022-04-15 | 2022-11-29 | 汉博来自控科技(上海)有限公司 | Solution for level sensor of forklift truck |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN105180968B (en) | A kind of IMU/ magnetometers installation misalignment filters scaling method online | |
| CN107525503B (en) | Adaptive cascade Kalman filtering method based on combination of dual-antenna GPS and MIMU | |
| CN106979780B (en) | A kind of unmanned vehicle real-time attitude measurement method | |
| CN106052685B (en) | A kind of posture and course estimation method of two-stage separation fusion | |
| CN105806363B (en) | The underwater large misalignment angle alignment methods of SINS/DVL based on SRQKF | |
| CN107478223A (en) | A kind of human body attitude calculation method based on quaternary number and Kalman filtering | |
| CN103822633B (en) | A kind of low cost Attitude estimation method measuring renewal based on second order | |
| CN106052716B (en) | Gyro error online calibration method based on starlight information auxiliary under inertial system | |
| CN103512584A (en) | Navigation attitude information output method, device and strapdown navigation attitude reference system | |
| CN110398257A (en) | The quick initial alignment on moving base method of SINS system of GPS auxiliary | |
| CN108318038A (en) | A kind of quaternary number Gaussian particle filtering pose of mobile robot calculation method | |
| CN106482746B (en) | Lever arm calibration and compensation method in a kind of accelerometer for hybrid inertial navigation system | |
| CN105910606A (en) | Direction adjustment method based on angular velocity difference | |
| CN106370178B (en) | Attitude measurement method and device of mobile terminal equipment | |
| CN108458714A (en) | The Eulerian angles method for solving of acceleration of gravity is free of in a kind of attitude detection system | |
| CN107402007A (en) | A kind of method for improving miniature AHRS modules precision and miniature AHRS modules | |
| CN111895988A (en) | Unmanned aerial vehicle navigation information updating method and device | |
| CN105865490B (en) | A kind of inertially stabilized platform fixed pedestal multiposition is from method of sight | |
| CN107167131A (en) | A kind of depth integration of micro-inertia measuring information and the method and system of real-Time Compensation | |
| CN107782309A (en) | Noninertial system vision and double tops instrument multi tate CKF fusion attitude measurement methods | |
| CN105841698A (en) | AUV rudder angle precise real-time measurement system without zero setting | |
| Ayub et al. | Pedestrian direction of movement determination using smartphone | |
| CN109945895A (en) | Inertial navigation Initial Alignment Method based on the smooth structure changes filtering that fades | |
| CN108627152A (en) | A kind of air navigation aid of the miniature drone based on Fusion | |
| Bistrov | Performance analysis of alignment process of MEMS IMU |
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 | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20190806 |
|
| RJ01 | Rejection of invention patent application after publication |