CN103727939A - Biaxial rotating attitude measurement system and measuring method thereof - Google Patents

Abstract

The invention discloses a biaxial rotating attitude measurement system and a measuring method thereof, and belongs to the technical field of inertial navigation. The device comprises an inertial measurement unit, a first rotating platform, a second rotating platform, a drive circuit, a controller, a computing unit and the like. According to the device, the inertial measurement unit is controlled to rotate by the first rotating platform, and then the drift of a gyro is estimated by using a sip angle change difference valve calculated by the gyro and an accelerometer under different attitudes before and after rotating. Meanwhile, the direction of a rotating shaft of the first rotating platform can be changed by the second rotating platform, so that a defect that when the rotating shaft of the first rotating platform is coincided with the gravity direction thereof, the dip angle can not be changed by the rotating of the first rotating platform and thus drift estimation fails is overcome, and singular points existing when an attitude description method implemented by using euler angles can be avoided. According to the invention, attitude measuring information can be provided for application objects such as robots in working environments with indoor and strong magnetic interference and the like.

Description

A kind of biaxial rotated attitude measurement system and measuring method thereof

Technical field

The present invention relates to a kind of system and method that utilizes Inertial Measurement Unit to carry out attitude measurement, particularly a kind of biaxial rotated attitude measurement system and measuring method thereof, belong to inertial navigation technology field.

Background technology

Inertial navigation system based on Inertial Measurement Unit is a kind of autonomous navigation system that does not rely on external information, and its subject matter is that long attitude measurement can be dispersed because gyrostatic drift accumulates.Existing attitude method of estimation, need to be revised and compensate attitude error by outside aiding sensors such as magnetometer or GPS.The weak point of these methods is: when magnetometer in operating environment and gps signal are because strong magnetic interference, the factor such as indoor are when unreliable, the application of these additional sensors and relevant error correcting method is restricted.

Research is found to change Inertial Measurement Unit (Inertial Measurement Unit by rotation, be called for short IMU) the method at inclination angle (comprising roll angle and the angle of pitch in attitude angle), can realize not relying on extraneous sensor and estimate gyroscopic drift and reduce positioning error.But utilize single-shaft-rotation to change the method existing problems at Inertial Measurement Unit inclination angle: when special circumstances that turning axle direction and gravity direction overlap, rotation is because losing efficacy at the inclination angle that cannot change Inertial Measurement Unit.In addition the attitude angle singular problem existing in the attitude angle computing method of expressing based on Eulerian angle, also needs to solve.

The device and method of existing single shaft or multiaxis rotatory inertia device, as patent " a kind of twin shaft spin fiber strapdown inertial navigation device " (CN102980578), by controlling inertia device, with rotating mechanism reciprocating rotation, error is modulated into the form that the cycle changes, in navigation calculation process, utilize integral operation error counteracting to be reduced to the accumulation of systematic error, navigation accuracy while improving the long boat of inertial navigation system.The weak point of this class device and method is: the function without drift estimate, when changing, inertial navigation system self attitude makes to rotatablely move when drift cannot be cancelled each other, device and method application is restricted, and does not also have the function that overcomes attitude angle singular problem simultaneously.Patent " Inertial measurement system with self correction " (US8010308) has proposed by the rotation around transverse axis, utilizes accelerometer to estimate the method for the drift motion correction course angle of gyro when vertical.The method need to guarantee turning axle around horizontal direction with guarantee revise precision, be not suitable for the situation of IMU attitude angle in arbitrary value.

Summary of the invention

The object of the invention is the weak point for prior art, a kind of biaxial rotated attitude measurement system and measuring method thereof are provided, improve under the environment such as indoor, magnetic interference attitude estimated accuracy for a long time, overcome the attitude angle singular problem in Eulerian angle computation process simultaneously.

For achieving the above object, technical scheme of the present invention is as follows:

The biaxial rotated attitude measurement system of one of the present invention, is characterized in that: comprise Inertial Measurement Unit, the first rotation platform, the second rotation platform, driving circuit, controller and computing unit; The first described rotation platform comprises the first pedestal, swivel mount and the first rotating electromagnetic equipment; Inertial Measurement Unit is fixed on the swivel mount of the first rotation platform, and described swivel mount is fixed on the turning axle of the first rotating electromagnetic equipment, and described rotating electromagnetic equipment is fixed on the first pedestal; The second described rotation platform comprises the second pedestal and the second rotating electromagnetic equipment; The first described pedestal is fixed on the turning axle of the second rotating electromagnetic equipment, and the second described rotating electromagnetic equipment is fixed on the second pedestal; The first described rotating electromagnetic equipment is connected with the signal output part of controller by driving circuit with the second rotating electromagnetic equipment; Described controller is realized Inertial Measurement Unit rotatablely moving around the turning axle of the first rotating electromagnetic equipment by driving circuit control the first rotating electromagnetic equipment; Described controller is realized the first rotation platform rotatablely moving around the turning axle of the second rotating electromagnetic equipment by driving circuit control the second rotating electromagnetic equipment; The output terminal of described Inertial Measurement Unit is connected by the input end of signal wire and computing unit, and described computing unit can obtain the motor message of Inertial Measurement Unit collection and do real-time calculating.

Controller of the present invention and computing unit adopt computing machine or the microcontroller with relevant interface circuit.

Attitude measurement method of the present invention, is characterized in that the method comprises the following steps:

A) by integrated gyroscope and accelerometer measures angular speed and the acceleration of Inertial Measurement Unit; By the variation numerical value at the inclination angle of Inertial Measurement Unit in angular speed interval of delta t computing time of gyroscope survey:

${\left(\begin{array}{c}\mathrm{\Δ\φ}\\ \mathrm{\Δ\θ}\end{array}\right)}_g=\left(\begin{array}{ccc}\sin \mathrm{\φ}\tan \mathrm{\θ}& 1& -\cos \mathrm{\φ}\tan \mathrm{\θ}\\ \cos \mathrm{\φ}& 0& -\cos \mathrm{\φ}\sec \mathrm{\θ}\end{array}\right)\left(\begin{array}{c}{\mathrm{\ω}}_x\\ {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z\end{array}\right)\mathrm{\Δt}$

In formula $\left(\begin{array}{c}{\mathrm{\ω}}_x\\ {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z\end{array}\right)$ Be the angular speed that gyroscope survey obtains, φ is roll angle, and θ is the angle of pitch, ${\left(\begin{array}{c}\mathrm{\Δ\φ}\\ \mathrm{\Δ\θ}\end{array}\right)}_g$ For the change of pitch angle value of calculating by angular speed;

B) by the acceleration of accelerometer measures, utilize formula (2) to calculate the inclination angle of Inertial Measurement Unit, and obtain the variation of time interval Δ t leaning angle ${\left(\begin{array}{c}\mathrm{\Δ\φ}\\ \mathrm{\Δ\θ}\end{array}\right)}_a:$

$\left(\begin{array}{c}\mathrm{\φ}\\ \mathrm{\θ}\end{array}\right)=\left(\begin{array}{c}a\tan 2({-A}_{\mathrm{mx}},{-A}_{\mathrm{mz}})\\ \arcsin \left(\frac{A_{\mathrm{my}}}{\left|\right|A_m\left|\right|}\right)\end{array}\right)---\left(2\right)$

In formula, be A _mx, A _my, A _mzbe respectively the acceleration that three axis accelerometer is measured, A _m=[A _mxa _mya _mz], ${\left(\begin{array}{c}\mathrm{\Δ\φ}\\ \mathrm{\Δ\θ}\end{array}\right)}_a$ For passing through the change of pitch angle value of acceleration calculation; ;

C) calculate change of pitch angle b) obtaining ${\left(\begin{array}{c}\mathrm{\Δ\φ}\\ \mathrm{\Δ\θ}\end{array}\right)}_g$ And c) obtain change of pitch angle ${\left(\begin{array}{c}\mathrm{\Δ\φ}\\ \mathrm{\Δ\θ}\end{array}\right)}_a$ Difference r:

$r={\left(\begin{array}{c}\mathrm{\Δ\φ}\\ \mathrm{\Δ\θ}\end{array}\right)}_g-{\left(\begin{array}{c}\mathrm{\Δ\φ}\\ \mathrm{\Δ\θ}\end{array}\right)}_a---\left(3\right)$

D) determine that roll angle is that φ, the angle of pitch are under θ, the relationship of the difference r of change of pitch angle and gyroscopic drift b:

r=Vb (4)

In formula, $V=\left(\begin{array}{ccc}\sin \mathrm{\φ}\tan \mathrm{\θ}& 1& -\cos \mathrm{\φ}\tan \mathrm{\θ}\\ \sin \mathrm{\φ}\sec \mathrm{\θ}& 0& \cos \mathrm{\φ}\sec \mathrm{\θ}\end{array}\right);$

E) control Inertial Measurement Unit rotation, rotating axial vector of unit length is u, and the anglec of rotation is Δ α, and the roll angle under postrotational new attitude is that φ ', the angle of pitch are θ ';

F) repeating step 1)～5), determine that roll angle is that φ ', the angle of pitch are under θ ', the relationship of the difference r ' of change of pitch angle and gyroscopic drift b:

r′=V′b (5)

In formula, $V^{\′}=\left(\begin{array}{ccc}\sin {\mathrm{\φ}}^{\′}\tan {\mathrm{\θ}}^{\′}& 1& -\cos {\mathrm{\φ}}^{\′}\tan {\mathrm{\θ}}^{\′}\\ \sin {\mathrm{\φ}}^{\′}\sec {\mathrm{\θ}}^{\′}& 0& \cos {\mathrm{\φ}}^{\′}\sec {\mathrm{\θ}}^{\′}\end{array}\right);$

G) relationship under different attitudes before and after simultaneous rotation, the drift by least square method computing gyroscope also compensates gyroscope angular speed, the attitude measurement result after output calibration;

H) when the turning axle direction of the first rotation platform and gravity direction approach when parallel, by the second rotation platform by the first rotation platform together with 45 °～90 ° of the Inertial Measurement Unit anglecs of rotation, after making the turning axle of the first rotation platform and gravity direction in an angle, repeating step a)～h), output attitude measurement result.

Attitude measurement method of the present invention, is characterized in that described Eulerian angle adopt as given a definition:

Navigation coordinate system is defined as o _nx _ny _nz _n; Inertial Measurement Unit Coordinate system definition is o _bx _by _bz _b; Navigation coordinate is o _nx _ny _nz _naround z _naxle turns (ψ) angle and obtains ox ₃y ₃z ₃, ox ₃y ₃z ₃around x ₃axle turns θ angle and obtains ox ₂y ₂z ₂, ox ₂y ₂z ₂again around y ₂axle turns φ angle, obtains Inertial Measurement Unit coordinate system o _bx _by _bz _b; The transition matrix R that is tied to Inertial Measurement Unit coordinate system from navigation coordinate is exactly the attitude matrix of Inertial Measurement Unit:

$R=\left[\begin{array}{ccc}\cos \mathrm{\φ}\cos \mathrm{\ψ}+\sin \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& -\cos \mathrm{\φ}\sin \mathrm{\ψ}+\sin \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& -\sin \mathrm{\φ}\cos \mathrm{\θ}\\ \cos \mathrm{\θ}\sin \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\θ}\\ \sin \mathrm{\φ}\cos \mathrm{\ψ}-\cos \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& -\sin \mathrm{\φ}\sin \mathrm{\ψ}-\cos \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\φ}\cos \mathrm{\θ}\end{array}\right]---\left(5\right)$

In formula, φ is roll angle, and θ is the angle of pitch, and ψ is course angle.

Attitude measurement method of the present invention, is characterized in that the turning axle of the first described slewing is set to overlap with the yb axle of Inertial Measurement Unit coordinate system.

The present invention compared with prior art, has the following advantages and marked improvement:

This system is utilized the scheme of twin shaft rotatory inertia measuring unit only to realize just to estimate gyroscopic drift and revise attitude with the integrated gyroscope of Inertial Measurement Unit and accelerometer, compare the conventional method with magnetometer or the auxiliary correction of GPS attitude estimation error, be not subject to the environmental limits such as indoor, magnetic interference, and can avoid calculating the singular point in attitude with Eulerian angle describing method.

Accompanying drawing explanation

Fig. 1 is the schematic three dimensional views of embodiment provided by the invention.

Fig. 2 is the connection diagram between each parts of embodiment provided by the invention.

Fig. 3 is the schematic flow sheet of attitude measurement method of the present invention.

In Fig. 1 to Fig. 3:

1-Inertial Measurement Unit, 2-the first rotation platform, 3-the second rotation platform,

4-driving circuit, 21-the first pedestal, 22-swivel mount,

23-the first rotating electromagnetic equipment, 31-the second pedestal, 32-the second rotating electromagnetic equipment,

231-turning axle, 23-rotating electromagnetic equipment, 31-driving circuit,

5-controller, 6-computing unit.

Embodiment

Below in conjunction with drawings and Examples, further describe the content of principle of work of the present invention, concrete structure.

The embodiment of biaxial rotated attitude measurement system provided by the invention, as shown in Figure 1 and Figure 2, this system comprises Inertial Measurement Unit 1, the first rotation platform 2, the second rotation platform 3, driving circuit 4, controller 5 and computing unit 6; The first described rotation platform comprises the first pedestal 21, swivel mount 22 and the first rotating electromagnetic equipment 23; Inertial Measurement Unit is fixed on the swivel mount of the first rotation platform, and described swivel mount is fixed on the turning axle 231 of the first rotating electromagnetic equipment, and described rotating electromagnetic equipment is fixed on the first pedestal; The second described rotation platform comprises the second pedestal 31 and the second rotating electromagnetic equipment 32; The first described pedestal is fixed on the turning axle 321 of the second rotating electromagnetic equipment, and the second described rotating electromagnetic equipment is fixed on the second pedestal; The first described rotating electromagnetic equipment is connected with the signal output part of controller by driving circuit with the second rotating electromagnetic equipment; Described controller is realized Inertial Measurement Unit rotatablely moving around the turning axle of the first rotating electromagnetic equipment by driving circuit control the first rotating electromagnetic equipment; Described controller is realized the first rotation platform rotatablely moving around the turning axle of the second rotating electromagnetic equipment by driving circuit control the second rotating electromagnetic equipment; The output terminal of described Inertial Measurement Unit is connected by the input end of signal wire and computing unit, and described computing unit can obtain the motor message of Inertial Measurement Unit collection and do real-time calculating.

In the present embodiment, described controller and computing unit adopt computing machine or the microcontroller with relevant interface circuit.

An attitude measurement method that uses said system, the method comprises the following steps:

A) by the integrated three-axis gyroscope of Inertial Measurement Unit and three axis accelerometer, measure respectively angular speed and the acceleration of motion.

B) by the variation numerical value at the inclination angle of Inertial Measurement Unit in angular speed interval of delta t computing time of gyroscope survey:

${\left(\begin{array}{c}\mathrm{\Δ\φ}\\ \mathrm{\Δ\θ}\end{array}\right)}_g=\left(\begin{array}{ccc}\sin \mathrm{\φ}\tan \mathrm{\θ}& 1& -\cos \mathrm{\φ}\tan \mathrm{\θ}\\ \cos \mathrm{\φ}& 0& -\cos \mathrm{\φ}\sec \mathrm{\θ}\end{array}\right)\left(\begin{array}{c}{\mathrm{\ω}}_x\\ {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z\end{array}\right)\mathrm{\Δt}---\left(1\right)$

In formula $\left(\begin{array}{c}{\mathrm{\ω}}_x\\ {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z\end{array}\right)$ Be the angular speed that gyroscope survey obtains, φ is roll angle, and θ is the angle of pitch, ${\left(\begin{array}{c}\mathrm{\Δ\φ}\\ \mathrm{\Δ\θ}\end{array}\right)}_g$ For the change of pitch angle value of calculating by angular speed;

C) by the acceleration of accelerometer measures, utilize formula (2) to calculate the inclination angle of Inertial Measurement Unit, and obtain the variation of time interval Δ t leaning angle ${\left(\begin{array}{c}\mathrm{\Δ\φ}\\ \mathrm{\Δ\θ}\end{array}\right)}_a:$

$\left(\begin{array}{c}\mathrm{\φ}\\ \mathrm{\θ}\end{array}\right)=\left(\begin{array}{c}a\tan 2({-A}_{\mathrm{mx}},{-A}_{\mathrm{mz}})\\ \arcsin \left(\frac{A_{\mathrm{my}}}{\left|\right|A_m\left|\right|}\right)\end{array}\right)---\left(2\right)$

In formula, be A _mx, A _my, A _mzbe respectively the acceleration that three axis accelerometer is measured, A _m=[A _mxa _mya _mz], ${\left(\begin{array}{c}\mathrm{\Δ\φ}\\ \mathrm{\Δ\θ}\end{array}\right)}_a$ For passing through the change of pitch angle value of acceleration calculation;

D) calculate change of pitch angle b) obtaining ${\left(\begin{array}{c}\mathrm{\Δ\φ}\\ \mathrm{\Δ\θ}\end{array}\right)}_g$ And c) obtain change of pitch angle ${\left(\begin{array}{c}\mathrm{\Δ\φ}\\ \mathrm{\Δ\θ}\end{array}\right)}_a$ Difference r:

$r={\left(\begin{array}{c}\mathrm{\Δ\φ}\\ \mathrm{\Δ\θ}\end{array}\right)}_g-{\left(\begin{array}{c}\mathrm{\Δ\φ}\\ \mathrm{\Δ\θ}\end{array}\right)}_a---\left(3\right)$

E) determine that roll angle is that φ, the angle of pitch are under θ, the relationship of the difference r of change of pitch angle and gyroscopic drift b:

r=Vb (4)

In formula, $V=\left(\begin{array}{ccc}\sin \mathrm{\φ}\tan \mathrm{\θ}& 1& -\cos \mathrm{\φ}\tan \mathrm{\θ}\\ \sin \mathrm{\φ}\sec \mathrm{\θ}& 0& \cos \mathrm{\φ}\sec \mathrm{\θ}\end{array}\right);$

F) control Inertial Measurement Unit around definite angle delta α of single-shaft-rotation, rotating axial vector of unit length is u, and the roll angle under postrotational new attitude is that φ ', the angle of pitch are θ ';

G) repeating step 1)～5), determine that roll angle is that φ ', the angle of pitch are under θ ', the relationship of the difference r ' of change of pitch angle and gyroscopic drift b:

r′=V′b (5)

In formula, $V^{\′}=\left(\begin{array}{ccc}\sin {\mathrm{\φ}}^{\′}\tan {\mathrm{\θ}}^{\′}& 1& -\cos {\mathrm{\φ}}^{\′}\tan {\mathrm{\θ}}^{\′}\\ \sin {\mathrm{\φ}}^{\′}\sec {\mathrm{\θ}}^{\′}& 0& \cos {\mathrm{\φ}}^{\′}\sec {\mathrm{\θ}}^{\′}\end{array}\right);$

H) relationship under different attitudes before and after simultaneous rotation, obtains a system of equations take gyrostatic drift b as unknown quantity:

$\hat{r}=\hat{V}b---\left(6\right)$

In formula, $\hat{r}={\left[\begin{array}{ccc}{\mathrm{<figure-callout\; id="1"\; label="r"\; filenames="HDA0000452060100000011.png"\; state="\{\{state\}\}">r</figure-callout>}}_1& ...& r_n\end{array}\right]}^T,\hat{V}={\left[\begin{array}{ccc}{\mathrm{<figure-callout\; id="1"\; label="V"\; filenames="HDA0000452060100000011.png"\; state="\{\{state\}\}">V</figure-callout>}}_1& ...& V_n\end{array}\right]}^T;$

Because gyrostatic drift is a stochastic process slowly changing, in the b short time, can think constant, therefore (6) are overdetermined equation group, drift that can computing gyroscope by least square method:

$b={\left({\hat{V}}^T\hat{V}\right)}^{-1}{\hat{V}}^T\hat{r}---\left(7\right)$

Then by estimated drift, gyroscope angular speed is compensated:

$\left(\begin{array}{c}{\mathrm{\&omega;}}_x^{\&prime;}\\ {\mathrm{\&omega;}}_y^{\&prime;}\\ {\mathrm{\&omega;}}_z^{\&prime;}\end{array}\right)=\left(\begin{array}{c}{\mathrm{\&omega;}}_x\\ {\mathrm{\&omega;}}_y\\ {\mathrm{\&omega;}}_z\end{array}\right)-b---\left(8\right)$

In formula $\left(\begin{array}{c}{\mathrm{\&omega;}}_x^{\&prime;}\\ {\mathrm{\&omega;}}_y^{\&prime;}\\ {\mathrm{\&omega;}}_z^{\&prime;}\end{array}\right)$ It is the angular speed after compensation;

Utilize the angular speed after compensation, according to the Eulerian angle after formula (9) calculation correction:

$\left(\begin{array}{c}\stackrel{.}{\mathrm{\&phi;}}\\ \stackrel{.}{\mathrm{\&theta;}}\\ \stackrel{.}{\mathrm{\&psi;}}\end{array}\right)=\left(\begin{array}{ccc}\sin \mathrm{\&phi;}\tan \mathrm{\&theta;}& 1& -\cos \mathrm{\&phi;}\tan \mathrm{\&theta;}\\ \cos \mathrm{\&phi;}& 0& \sin \mathrm{\&phi;}\\ \sin \mathrm{\&phi;}\sec \mathrm{\&theta;}& 0& -\cos \mathrm{\&phi;}\sec \mathrm{\&theta;}\end{array}\right)\left(\begin{array}{c}{\mathrm{\&omega;}}_x^{\&prime;}\\ {\mathrm{\&omega;}}_y^{\&prime;}\\ {\mathrm{\&omega;}}_z^{\&prime;}\end{array}\right)---\left(9\right)$

Attitude measurement result after output calibration.

I) when the turning axle direction of the first rotation platform and gravity direction approach when parallel, by the second rotation platform by the first rotation platform together with 45 °～90 ° of the Inertial Measurement Unit anglecs of rotation, after making the turning axle of the first rotation platform and gravity direction in an angle, repeating step a)～g), output attitude measurement result.

Fig. 3 is the schematic flow sheet of attitude measurement method of the present invention.

Eulerian angle described in the present embodiment adopt as give a definition:

Navigation coordinate system is defined as o _nx _ny _nz _n; Inertial Measurement Unit Coordinate system definition is o _bx _by _bz _b; Navigation coordinate is o _nx _ny _nz _naround z _naxle turns (ψ) angle and obtains ox ₃y ₃z ₃, ox ₃y ₃z ₃around x ₃axle turns θ angle and obtains ox ₂y ₂z ₂, ox ₂y ₂z ₂again around y ₂axle turns φ angle, obtains Inertial Measurement Unit coordinate system o _bx _by _bz _b; The transition matrix R that is tied to Inertial Measurement Unit coordinate system from navigation coordinate is exactly the attitude matrix of Inertial Measurement Unit:

$R=\left[\begin{array}{ccc}\cos \mathrm{\&phi;}\cos \mathrm{\&psi;}+\sin \mathrm{\&phi;}\sin \mathrm{\&theta;}\sin \mathrm{\&psi;}& -\cos \mathrm{\&phi;}\sin \mathrm{\&psi;}+\sin \mathrm{\&phi;}\sin \mathrm{\&theta;}\cos \mathrm{\&psi;}& -\sin \mathrm{\&phi;}\cos \mathrm{\&theta;}\\ \cos \mathrm{\&theta;}\sin \mathrm{\&psi;}& \cos \mathrm{\&theta;}\cos \mathrm{\&psi;}& \sin \mathrm{\&theta;}\\ \sin \mathrm{\&phi;}\cos \mathrm{\&psi;}-\cos \mathrm{\&phi;}\sin \mathrm{\&theta;}\sin \mathrm{\&psi;}& -\sin \mathrm{\&phi;}\sin \mathrm{\&psi;}-\cos \mathrm{\&phi;}\sin \mathrm{\&theta;}\cos \mathrm{\&psi;}& \cos \mathrm{\&phi;}\cos \mathrm{\&theta;}\end{array}\right]---\left(5\right)$

In formula, φ is roll angle, and θ is the angle of pitch, and ψ is course angle.

In the present embodiment, the turning axle of the first described slewing is set to overlap with the yb axle of Inertial Measurement Unit coordinate system, therefore when the turning axle direction of the first rotation platform and gravity direction approach when parallel, the singular point (roll angle θ ≈ ± 90 °) of computation method for attitude of describing in Eulerian angle, now the rotatablely moving of second slewing of step in h) can make Inertial Measurement Unit avoid singular point.

The present embodiment carries out the method for attitude measurement, for the criterion of the first turning axle direction and gravity direction coincidence, is: || θ |-90 ° |≤5 °.

In the present embodiment, described Inertial Measurement Unit adopts the MEMS Inertial Measurement Unit MTI-100 of Xsens company.

In the present embodiment, described the first slewing and the second slewing all adopt rotary magnet, and controller adopts DSP, by square-wave signal drive magnetic, realize crankmotion, the first slewing movement angle is 90 °, and the second slewing movement angle is 45 °.

Because the present invention is auxiliary without external sensors such as magnetometer or GPS, the attitude of only utilizing the first rotation platform to change Inertial Measurement Unit estimates that gyrostatic drift motion reduces attitude positioning error; By the second rotation platform, can change the direction of the turning axle of the first rotation platform simultaneously, solve that when the turning axle of the first rotation platform and gravity direction overlap its rotation can not change inclination angle and the problem that lost efficacy; And second rotary freedom can be that Inertial Measurement Unit is avoided the singular point (θ=± 90 °) of inertial navigation method in calculating, be therefore applicable to the attitude positioning requirements of the applications such as robot under the operating environments such as indoor, strong magnetic interference.

Above-described embodiment is only for illustrating the present invention; different with volume according to the quality of Inertial Measurement Unit; the realization of the structure of twin shaft rotation platform, the selection of slewing and rotation mode all can change to some extent; every distortion and improvement of making in the technology of the present invention design, all should belong to protection scope of the present invention.

Claims (4)

1. a biaxial rotated attitude measurement system, is characterized in that: comprise Inertial Measurement Unit (1), the first rotation platform (2), the second rotation platform (3), driving circuit (4), controller (5) and computing unit (6); The first described rotation platform comprises the first pedestal (21), swivel mount (22) and the first rotating electromagnetic equipment (23); Inertial Measurement Unit is fixed on the swivel mount of the first rotation platform, and described swivel mount is fixed on the turning axle (231) of the first rotating electromagnetic equipment, and described rotating electromagnetic equipment is fixed on the first pedestal; The second described rotation platform comprises the second pedestal (31) and the second rotating electromagnetic equipment (32); The first described pedestal is fixed on the turning axle (321) of the second rotating electromagnetic equipment, and the second described rotating electromagnetic equipment is fixed on the second pedestal; The first described rotating electromagnetic equipment is connected with the signal output part of controller by driving circuit with the second rotating electromagnetic equipment; Described controller is realized Inertial Measurement Unit by driving circuit control the first rotating electromagnetic equipment and is rotated around the turning axle of the first rotating electromagnetic equipment; Described controller is realized the first rotation platform by driving circuit control the second rotating electromagnetic equipment and is rotated around the turning axle of the second rotating electromagnetic equipment; The output terminal of described Inertial Measurement Unit is connected by the input end of signal wire and computing unit, and described computing unit can obtain the motor message of Inertial Measurement Unit collection and do real-time calculating.

2. adopt an attitude measurement method for system as claimed in claim 1, it is characterized in that the method comprises the following steps:

A) by integrated gyroscope and accelerometer measures angular speed and the acceleration of Inertial Measurement Unit;

B) by the variation numerical value at the inclination angle of Inertial Measurement Unit in angular speed interval of delta t computing time of gyroscope survey ${\left(\begin{array}{c}\mathrm{\&Delta;\&phi;}\\ \mathrm{\&Delta;\&theta;}\end{array}\right)}_g:$

${\left(\begin{array}{c}\mathrm{\&Delta;\&phi;}\\ \mathrm{\&Delta;\&theta;}\end{array}\right)}_g=\left(\begin{array}{ccc}\sin \mathrm{\&phi;}\tan \mathrm{\&theta;}& 1& -\cos \mathrm{\&phi;}\tan \mathrm{\&theta;}\\ \cos \mathrm{\&phi;}& 0& -\cos \mathrm{\&phi;}\sec \mathrm{\&theta;}\end{array}\right)\left(\begin{array}{c}{\mathrm{\&omega;}}_x\\ {\mathrm{\&omega;}}_y\\ {\mathrm{\&omega;}}_z\end{array}\right)\mathrm{\&Delta;t}---\left(1\right)$

In formula $\left(\begin{array}{c}{\mathrm{\&omega;}}_x\\ {\mathrm{\&omega;}}_y\\ {\mathrm{\&omega;}}_z\end{array}\right)$ Be the angular speed that gyroscope survey obtains, φ is roll angle, and θ is the angle of pitch, ${\left(\begin{array}{c}\mathrm{\&Delta;\&phi;}\\ \mathrm{\&Delta;\&theta;}\end{array}\right)}_g$ For the change of pitch angle value of calculating by angular speed;

C) by the acceleration of accelerometer measures, utilize formula (2) to calculate the inclination angle of Inertial Measurement Unit, and obtain the variation of time interval Δ t leaning angle ${\left(\begin{array}{c}\mathrm{\&Delta;\&phi;}\\ \mathrm{\&Delta;\&theta;}\end{array}\right)}_a:$

$\left(\begin{array}{c}\mathrm{\&phi;}\\ \mathrm{\&theta;}\end{array}\right)=\left(\begin{array}{c}a\tan 2({-A}_{\mathrm{mx}},{-A}_{\mathrm{mz}})\\ \arcsin \left(\frac{A_{\mathrm{my}}}{\left|\right|A_m\left|\right|}\right)\end{array}\right)---\left(2\right)$

In formula, be A _mx, A _my, A _mzbe respectively the acceleration that three axis accelerometer is measured, A _m=[A _mxa _mya _mz], ${\left(\begin{array}{c}\mathrm{\&Delta;\&phi;}\\ \mathrm{\&Delta;\&theta;}\end{array}\right)}_a$ For passing through the change of pitch angle value of acceleration calculation;

D) calculate change of pitch angle b) obtaining ${\left(\begin{array}{c}\mathrm{\&Delta;\&phi;}\\ \mathrm{\&Delta;\&theta;}\end{array}\right)}_g$ And c) obtain change of pitch angle ${\left(\begin{array}{c}\mathrm{\&Delta;\&phi;}\\ \mathrm{\&Delta;\&theta;}\end{array}\right)}_a$ Difference r:

$r={\left(\begin{array}{c}\mathrm{\&Delta;\&phi;}\\ \mathrm{\&Delta;\&theta;}\end{array}\right)}_g-{\left(\begin{array}{c}\mathrm{\&Delta;\&phi;}\\ \mathrm{\&Delta;\&theta;}\end{array}\right)}_a---\left(3\right)$

E) determine that roll angle is φ, the angle of pitch difference r of change of pitch angle and the relationship of gyroscopic drift b while being θ:

r=Vb (4)

In formula, $V=\left(\begin{array}{ccc}\sin \mathrm{\&phi;}\tan \mathrm{\&theta;}& 1& -\cos \mathrm{\&phi;}\tan \mathrm{\&theta;}\\ \sin \mathrm{\&phi;}\sec \mathrm{\&theta;}& 0& \cos \mathrm{\&phi;}\sec \mathrm{\&theta;}\end{array}\right);$

F) control Inertial Measurement Unit rotation, rotating axial vector of unit length is u, and the anglec of rotation is Δ α, and the roll angle under postrotational new attitude is that φ ', the angle of pitch are θ ';

G) repeating step 1)～5), determine roll angle be φ ', the angle of pitch be θ ' time change of pitch angle difference r ' and the relationship of gyroscopic drift b:

r′=V′b (5)

In formula, $V^{\&prime;}=\left(\begin{array}{ccc}\sin {\mathrm{\&phi;}}^{\&prime;}\tan {\mathrm{\&theta;}}^{\&prime;}& 1& -\cos {\mathrm{\&phi;}}^{\&prime;}\tan {\mathrm{\&theta;}}^{\&prime;}\\ \sin {\mathrm{\&phi;}}^{\&prime;}\sec {\mathrm{\&theta;}}^{\&prime;}& 0& \cos {\mathrm{\&phi;}}^{\&prime;}\sec {\mathrm{\&theta;}}^{\&prime;}\end{array}\right);$

H) relationship under different attitudes before and after simultaneous rotation, the drift by least square method computing gyroscope also compensates gyroscope angular speed, the attitude measurement result after output calibration;

I) when the turning axle direction of the first rotation platform and gravity direction approach when parallel, by the second rotation platform by the first rotation platform together with 45 °～90 ° of the Inertial Measurement Unit anglecs of rotation, after making the turning axle of the first rotation platform and gravity direction in an angle, repeating step a)～h), output attitude measurement result.

3. attitude measurement method as claimed in claim 2, is characterized in that described Eulerian angle adopt as given a definition:

Navigation coordinate system is defined as o _nx _ny _nz _n; Inertial Measurement Unit Coordinate system definition is o _bx _by _bz _b; Navigation coordinate is o _nx _ny _nz _naround z _naxle turns (ψ) angle and obtains ox ₃y ₃z ₃, ox ₃y ₃z ₃around x ₃axle turns θ angle and obtains ox ₂y ₂z ₂, ox ₂y ₂z ₂again around y ₂axle turns φ angle, obtains Inertial Measurement Unit coordinate system o _bx _by _bz _b; The transition matrix R that is tied to Inertial Measurement Unit coordinate system from navigation coordinate is exactly the attitude matrix of Inertial Measurement Unit:

$R=\left[\begin{array}{ccc}\cos \mathrm{\&phi;}\cos \mathrm{\&psi;}+\sin \mathrm{\&phi;}\sin \mathrm{\&theta;}\sin \mathrm{\&psi;}& -\cos \mathrm{\&phi;}\sin \mathrm{\&psi;}+\sin \mathrm{\&phi;}\sin \mathrm{\&theta;}\cos \mathrm{\&psi;}& -\sin \mathrm{\&phi;}\cos \mathrm{\&theta;}\\ \cos \mathrm{\&theta;}\sin \mathrm{\&psi;}& \cos \mathrm{\&theta;}\cos \mathrm{\&psi;}& \sin \mathrm{\&theta;}\\ \sin \mathrm{\&phi;}\cos \mathrm{\&psi;}-\cos \mathrm{\&phi;}\sin \mathrm{\&theta;}\sin \mathrm{\&psi;}& -\sin \mathrm{\&phi;}\sin \mathrm{\&psi;}-\cos \mathrm{\&phi;}\sin \mathrm{\&theta;}\cos \mathrm{\&psi;}& \cos \mathrm{\&phi;}\cos \mathrm{\&theta;}\end{array}\right]---\left(5\right)$

In formula, φ is roll angle, and θ is the angle of pitch, and ψ is course angle.

4. attitude measurement method claimed in claim 3, is characterized in that: the turning axle of the first described slewing is set to the y with Inertial Measurement Unit coordinate system _baxle overlaps.

Images