CN105180936A - Servo loop decoupling method of four-axle inertial stabilization platform system - Google Patents

Abstract

A provided servo loop decoupling method of a four-axle inertial stabilization platform system comprises the following steps: 1, obtaining the angular velocity components at Xp axis, Yp axis and Zp axis according to the angular velocity of a gyroscope installed on a platform body; 2, measuring and obtaining the internal relative rotation angle and angular velocity of the four-axle inertial stabilization platform system; and 3, calculating the synthetic rotation angular velocity of the platform body, an internal frame, an outer frame and a servo frame. The method is capable realizing servo loop decoupling calculation without singular value under the circumstance that the platform system relative rotation angle is an optional value, thereby improving the full attitude adaptation capability of a carrier under the condition of no trajectory constraint.

Description

A kind of servo loop decoupling method of four axle inertially stabilized platform systems

Technical field

The present invention relates to inertial survey technique field, particularly a kind of servo loop decoupling method of four axle inertially stabilized platform systems, be mainly used in the full attitude high precision navigation in Aeronautics and Astronautics field.

Background technology

Because three-axis inertial platform system exists " framework locking " phenomenon, be difficult to the requirement meeting the motion of carrier high maneuver, therefore, create four axle Inertial Platform System.The relative three-axis inertial platform system of four axle Inertial Platform System, the basis of stage body, inner frame and outside framework adds servo-actuated framework, and servo-actuated framework is between platform outer gimbal and pedestal.

Traditional solution is as follows: following loop signal comes from inner frame angle, adopts secant resolver to carry out gain compensation.But the shortcoming of the method, when frame corners is 90 ° outside, there is singular value.See document " four axis platform servo-drive system Modeling Research, Chinese inertial technology journal Vol.10, No.5, in October, 2002 ", and " model analysis of four axis platform servomechanism and design, navigation and vehicle controL, the 4th phase in 2014 ".

At present, the way solving this problem is mainly: adopt framework locking when frame corners is 90 ° outside, once by this angle value, revert to again former servo-actuated scheme.See document " during four axis platform outside framework angle ± 90 ° Kinematic Characteristic Simulation, navigation and vehicle controL, the 2nd phase in 2009 ".But the shortcoming of the method is, when frame corners keeps 90 ° always outside, then four axis platform deteriorates to three-axis platform, still there is " framework locking " phenomenon, can not realize the full attitude function of carrier movement.

In a word, said method also exists with condition judgment to overcome the shortcoming of singular value, for this reason, needs to study a kind of without singular value, not by the decoupling method of track restriction.

Introduce the research conditions of currently available technology below:

First, the definition of five body coordinate system of four axle inertially stabilized platform systems as shown in Figure 1, therefrom can find out relation between each body coordinate system.In FIG, if for the relative angle speed of the relative stage body of inner frame, for the relative angle speed of the relative inner frame of outside framework, for the relative angle speed for the relative outside framework of pedestal (rocket body), for the relative angle speed of the relatively servo-actuated framework of pedestal (rocket body).

If for stage body (comprising gyroscope housing) is to X _p, Y _p, Z _pthe moment of inertia of axle; for inner frame is to X _p1, Y _p1, Z _p1the moment of inertia of axle; for outside framework is to X _p2, Y _p2, Z _p2the moment of inertia of axle; for servo-actuated framework is to X _p3, Y _p3, Z _p3the moment of inertia of axle.Definition for being folded to stage body axle X _pmoment of inertia, for being folded to stage body axle Y _pmoment of inertia; J _xy, J _yx, J _xz, J _yzfor the equivalent inertia of frame system amasss.Wherein:

$\begin{array}{c}{J}_{x_p}^{\′}=J_{x_p}+J_{x_{p1}}{\cos }^2{\mathrm{\β}}_{zk}+J_{y_{p1}}{\sin }^2{\mathrm{\β}}_{zk}+J_{x_{p2}}{\cos }^2{\mathrm{\β}}_{zk}{\cos }^2{\mathrm{\β}}_{yk}+J_{z_{p2}}{\cos }^2{\mathrm{\β}}_{zk}{\sin }^2{\mathrm{\β}}_{yk}\\ +J_{y_{p3}}{\cos }^2{\mathrm{\β}}_{zk}{\sin }^2{\mathrm{\β}}_{yk}{\sin }^2{\mathrm{\β}}_{xk}+J_{z_{p3}}{\cos }^2{\mathrm{\β}}_{zk}{\sin }^2{\mathrm{\β}}_{yk}{\cos }^2{\mathrm{\β}}_{xk}\\ +\frac{1}{4}\left(J_{z_{p3}}-J_{y_{p3}}\right)\sin 2{\mathrm{\β}}_{zk}{\mathrm{sin\β}}_{yk}\sin 2{\mathrm{\β}}_{xk}\end{array}---\left(1\right)$

$\begin{array}{c}{J}_{y_p}^{\′}=J_{y_p}+J_{x_{p1}}{\sin }^2{\mathrm{\β}}_{zk}+J_{y_{p1}}{\cos }^2{\mathrm{\β}}_{zk}+J_{x_{p2}}{\sin }^2{\mathrm{\β}}_{zk}{\cos }^2{\mathrm{\β}}_{yk}+J_{z_{p2}}{\sin }^2{\mathrm{\β}}_{zk}{\sin }^2{\mathrm{\β}}_{yk}\\ +J_{y_{p3}}{\sin }^2{\mathrm{\β}}_{zk}{\sin }^2{\mathrm{\β}}_{yk}{\sin }^2{\mathrm{\β}}_{xk}+J_{z_{p3}}{\sin }^2{\mathrm{\β}}_{zk}{\sin }^2{\mathrm{\β}}_{yk}{\cos }^2{\mathrm{\β}}_{xk}\\ -\frac{1}{4}\left(J_{z_{p3}}-J_{y_{p3}}\right)\sin 2{\mathrm{\β}}_{zk}{\mathrm{sin\β}}_{yk}\sin 2{\mathrm{\β}}_{xk}\end{array}---\left(2\right)$

$\begin{array}{c}{J}_{xy}=\frac{1}{2}\left(J_{x_{p1}}-J_{y_{p1}}+J_{x_{p2}}{\cos }^2{\mathrm{\β}}_{yk}+J_{z_{p2}}{\sin }^2{\mathrm{\β}}_{yk}+J_{y_{p3}}{\sin }^2{\mathrm{\β}}_{yk}{\sin }^2{\mathrm{\β}}_{xk}+J_{z_{p3}}{\sin }^2{\mathrm{\β}}_{yk}{\cos }^2{\mathrm{\β}}_{xk}\right)\sin 2{\mathrm{\β}}_{zk}\\ +\frac{1}{2}\left(J_{y_{p3}}-J_{z_{p3}}\right){\cos }^2{\mathrm{\β}}_{zk}{\mathrm{sin\β}}_{yk}\sin 2{\mathrm{\β}}_{xk}\end{array}---\left(3\right)$

$J_{xz}=\frac{1}{2}(J_{z_{p2}}-J_{x_{p2}}+J_{y_{p3}}{\sin }^2{\mathrm{\β}}_{xk}+J_{z_{p3}}{\cos }^2{\mathrm{\β}}_{xk})sin2{\mathrm{\β}}_{yk}{\mathrm{cos\β}}_{zk}---\left(4\right)$

$\begin{array}{c}{J}_{yx}=\frac{1}{2}\left(J_{x_{p1}}-J_{y_{p1}}+J_{x_{p2}}{\cos }^2{\mathrm{\β}}_{yk}+J_{z_{p2}}{\sin }^2{\mathrm{\β}}_{yk}+J_{y_{p3}}{\sin }^2{\mathrm{\β}}_{yk}{\sin }^2{\mathrm{\β}}_{xk}+J_{z_{p3}}{\sin }^2{\mathrm{\β}}_{yk}{\cos }^2{\mathrm{\β}}_{xk}\right)\sin 2{\mathrm{\β}}_{zk}\\ -\frac{1}{2}\left(J_{y_{p3}}-J_{z_{p3}}\right){\cos }^2{\mathrm{\β}}_{zk}{\mathrm{sin\β}}_{yk}\sin 2{\mathrm{\β}}_{xk}\end{array}---\left(5\right)$

$J_{yz}=\frac{1}{2}(J_{z_{p2}}-J_{x_{p2}}+J_{y_{p3}}{\sin }^2{\mathrm{\β}}_{xk}+J_{z_{p3}}{\cos }^2{\mathrm{\β}}_{xk})sin2{\mathrm{\β}}_{yk}{\mathrm{sin\β}}_{zk}---\left(6\right)$

If M _zpfor stage body axle disturbance torque, for stage body axle torque motor feedback moment; for input axis disturbance torque, for input axis torque motor feedback moment; for outside framework axle disturbance torque, for outside framework axle torque motor feedback moment; for the moment of face on servo-actuated gimbal axis, for servo-actuated gimbal axis torque motor feedback moment; Then each axle head moment loading of four axle Inertial Platform System is as follows to the resultant moment of stage body three axle:

$\left[\begin{array}{c}{M}_{z3}\\ M_{y3}\\ M_{x3}\end{array}\right]=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& {\mathrm{cos\β}}_{zk}& {\mathrm{cos\β}}_{yk}{\mathrm{sin\β}}_{zk}& {\mathrm{sin\β}}_{xk}{\mathrm{sin\β}}_{yk}{\mathrm{sin\β}}_{zk}\\ 0& -{\mathrm{sin\β}}_{zk}& {\mathrm{cos\β}}_{yk}{\mathrm{cos\β}}_{zk}& {\mathrm{sin\β}}_{xk}{\mathrm{sin\β}}_{yk}{\mathrm{cos\β}}_{zk}\end{array}\right]\left[\begin{array}{c}{M}_{zp}\\ M_{y_{p1}}\\ M_{y_{p2}}\\ M_{y_{p3}}\end{array}\right]---\left(7\right)$

$\left[\begin{array}{c}{M}_{D_{z3}}\\ M_{D_{y3}}\\ M_{D_{x3}}\end{array}\right]=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& {\mathrm{cos\β}}_{zk}& {\mathrm{cos\β}}_{yk}{\mathrm{sin\β}}_{zk}& {\mathrm{sin\β}}_{xk}{\mathrm{sin\β}}_{yk}{\mathrm{sin\β}}_{zk}\\ 0& -{\mathrm{sin\β}}_{zk}& {\mathrm{cos\β}}_{yk}{\mathrm{cos\β}}_{zk}& {\mathrm{sin\β}}_{xk}{\mathrm{sin\β}}_{yk}{\mathrm{cos\β}}_{zk}\end{array}\right]\left[\begin{array}{c}{M}_{D{zp}_{}}\\ M_{D_{y1}}\\ M_{D_{x2}}\\ M_{D_{y3}}\end{array}\right]---\left(8\right)$

be respectively stage body around x _p, y _p, z _pthe absolute angular velocities of axle, obtains by the orthogonal gyroscope survey being installed on stage body; Then the stage body kinetics equation of four axle Inertial Platform System is

$\left[\begin{array}{ccc}{J}_{x_p}^{\′}& J_{xy}& J_{xz}\\ J_{yx}& J_{y_p}^{\′}& J_{yz}\\ 0& 0& J_{z_p}^{\′}\end{array}\right]\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\ω}}}_{x_p}\\ {\stackrel{\·}{\mathrm{\ω}}}_{y_p}\\ {\stackrel{\·}{\mathrm{\ω}}}_{z_p}\end{array}\right]=\left[\begin{array}{c}{M}_{x3}\\ M_{y3}\\ M_{z3}\end{array}\right]-\left[\begin{array}{c}{M}_{D_{x3}}\\ M_{D_{y3}}\\ M_{D_{z3}}\end{array}\right]---\left(9\right)$

Can find out, at 3 gyroscope angular speeds when information is known, 4 are had to control to perform link servo loop is not exclusively controlled.Therefore, current solution increases servo antrol loop, makes inner frame angle β _ykbe approximately 0 °, now dynamic equation

$\begin{array}{c}\left(J_{y_{p2}}+J_{y_{p3}}{\cos }^2{\mathrm{\β}}_{xk}+J_{z_{p3}}{\sin }^2{\mathrm{\β}}_{xk}\right){\stackrel{\·\·}{\mathrm{\β}}}_{yk}+\frac{1}{2}\left(J_{y_{p3}}-J_{z_{p3}}\right){\stackrel{\·}{\mathrm{\ω}}}_{z_{p2}}\sin 2{\mathrm{\β}}_{xk}\\ =\left(M_{y_{p3}}-M_{D_{y3}}\right){\mathrm{cos\β}}_{xk}-M_{z_{p3}}{\mathrm{sin\β}}_{xk}\end{array}---\left(10\right)$

After stage body three axle kinetics equation, the kinetics equation of four axle Inertial Platform System is:

$\left[\begin{array}{cccc}{J}_{z_p}^{\′}& 0& 0& 0\\ J_{yz}& J_{y_p}^{\′}& J_{yx}& 0\\ J_{xz}& J_{xy}& J_{x_p}^{\′}& 0\\ 0& 0& 0& J_{y_{p2}}^{\′}\end{array}\right]\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\ω}}}_{z_p}\\ {\stackrel{\·}{\mathrm{\ω}}}_{y_p}\\ {\stackrel{\·}{\mathrm{\ω}}}_{x_p}\\ {\stackrel{\·\·}{\mathrm{\β}}}_{yk}\end{array}\right]=\left[\begin{array}{c}{M}_{z3}\\ M_{y3}\\ M_{x3}\\ M_{y2}\end{array}\right]-\left[\begin{array}{c}{M}_{D_{z3}}\\ M_{D_{y3}}\\ M_{D_{x3}}\\ M_{D_{y2}}\end{array}\right]---\left(11\right)$

Wherein,

$\left[\begin{array}{c}{M}_{z3}\\ M_{y3}\\ M_{x3}\\ M_{y2}\end{array}\right]=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& {\mathrm{cos\β}}_{zk}& {\mathrm{cos\β}}_{yk}{\mathrm{sin\β}}_{zk}& {\mathrm{sin\β}}_{xk}{\mathrm{sin\β}}_{yk}{\mathrm{sin\β}}_{zk}\\ 0& -{\mathrm{sin\β}}_{zk}& {\mathrm{cos\β}}_{yk}{\mathrm{cos\β}}_{zk}& {\mathrm{sin\β}}_{xk}{\mathrm{sin\β}}_{yk}{\mathrm{cos\β}}_{zk}\\ 0& 0& 0& {\mathrm{cos\β}}_{xk}\end{array}\right]\left[\begin{array}{c}{M}_{zp}\\ M_{y_{p1}}\\ M_{x_{p2}}\\ M_{y_{p3}}\end{array}\right]---\left(12\right)$

$J_{y_{p2}}^{\′}=J_{y_{p2}}+J_{y_{p3}}{\cos }^2{\mathrm{\β}}_{xk}+J_{z_{p3}}{\sin }^2{\mathrm{\β}}_{xk}$

Now, spatially decoupled matrix is at β _ykwhen being approximately 0, can be reduced to

$T=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& {\mathrm{cos\β}}_{zk}& -{\mathrm{sin\β}}_{zk}& 0\\ 0& {\mathrm{sec\β}}_{yk}{\mathrm{sin\β}}_{zk}& {\mathrm{sec\β}}_{yk}{\mathrm{cos\β}}_{zk}& -{\mathrm{tan\β}}_{xk}{\mathrm{tan\β}}_{yk}\\ 0& 0& 0& {\mathrm{sec\β}}_{xk}\end{array}\right]\stackrel{{\mathrm{\β}}_{yk}=0}{\≈}\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& {\mathrm{cos\β}}_{zk}& -{\mathrm{sin\β}}_{zk}& 0\\ 0& {\mathrm{sin\β}}_{zk}& {\mathrm{cos\β}}_{zk}& 0\\ 0& 0& 0& {\mathrm{sec\β}}_{xk}\end{array}\right]---\left(13\right)$

Existing decoupling zero servo loop theory diagram as shown in Figure 2, prior art, on the coordinate resolver basis of former three stable loops, increases secant resolver sec β _xk.I.e. handle as a link of following loop.But also find out, at β _xkbe tending towards ± 90 ° time, there is singular value, sec β _xkbe tending towards infinitely great.

Summary of the invention

The object of the invention is to overcome the deficiencies in the prior art, a kind of servo loop decoupling method of four axle inertially stabilized platform systems is provided, the method can when to relatively rotate angle be arbitrary value to plateform system, realize the servo loop decoupling computation without singular value, thus improve carrier without the full attitude adaptive faculty under trajectory constraint.

Above-mentioned purpose of the present invention is achieved through the following technical solutions:

A kind of servo loop decoupling method of four axle inertially stabilized platform systems, realize based on four axle inertially stabilized platform systems, described Stable Platform System comprises pedestal, servo-actuated framework, outside framework, inner frame and stage body, and corresponding body coordinate system is respectively base body coordinate system X ₁y ₁z ₁, servo-actuated frame coordinates system X _p3y _p3z _p3, outside framework body coordinate system X _p2y _p2z _p2, inner frame body coordinate system X _p1y _p1z _p1with stage body body coordinate system X _py _pz _p; The initial point of described five coordinate systems overlaps, and: the Z of stage body body coordinate system _pthe Z of axle and inner frame body coordinate system _p1axle overlaps, the Y of the body coordinate system of outside framework _p2the Y of axle and inner frame body coordinate system _p1axle overlaps, the X of servo-actuated frame body coordinate system _p3the X of axle and outside framework body coordinate system _p2axle overlaps, the X of base body coordinate system ₁axle overlaps with the Y-axis of servo-actuated frame body coordinate system; Wherein, pedestal and carrier are connected, described Stable Platform System carrier drive issue raw inside relatively rotate time, pedestal is around the Y of servo-actuated frame body coordinate system _p3axle rotates, and servo-actuated framework is around the X of outside framework body coordinate system _p2axle rotates, and outside framework is around the Y of inner frame body coordinate system _p1axle rotates, and inner frame is around the Z of stage body body coordinate system _paxle rotates;

Described four axle Inertial Platform System servo loop decoupling method performing steps are as follows:

(1) angular velocity, according to the gyroscope that stage body is installed exported, obtains stage body at X _paxle, Y _paxle and Z _pangular velocity component on axle

(2), measure obtain four axle inertially stabilized platform internal system angle and angular velocity in relative rotation, comprising: servo-actuated framework is around the X of outside framework body coordinate system _p2the angle beta that axle rotates _xk, outside framework is around the Y of inner frame body coordinate system _p1the angle beta that axle rotates _ykand angular velocity inner frame is around the Z of stage body body coordinate system _pthe angle beta that axle rotates _zkand angular velocity

(3), calculate the rotational angular velocity of stage body, inner frame, outside framework and servo-actuated framework, specific formula for calculation is as follows:

${\mathrm{\ω}}_z={\mathrm{\ω}}_{z_p};$

${\mathrm{\ω}}_y={\mathrm{\ω}}_{y_p}{\mathrm{cos\β}}_{zk}-{\mathrm{\ω}}_{x_p}{\mathrm{sin\β}}_{zk};$

${\mathrm{\ω}}_x={\mathrm{\ω}}_{y_p}{\mathrm{cos\β}}_{yk}{\mathrm{sin\β}}_{zk}+{\mathrm{\ω}}_{x_p}{\mathrm{cos\β}}_{yk}{\mathrm{cos\β}}_{zk}-{\stackrel{\·}{\mathrm{\β}}}_{zk}{\mathrm{sin\β}}_{yk};$

${\mathrm{\ω}}_{{\mathrm{yk}}^{\′}}={\mathrm{\ω}}_{y_p}{\mathrm{sin\β}}_{xk}{\mathrm{sin\β}}_{yk}{\mathrm{sin\β}}_{zk}+{\mathrm{\ω}}_{x_p}{\mathrm{sin\β}}_{xk}{\mathrm{sin\β}}_{yk}{\mathrm{cos\β}}_{zk}+{\stackrel{\·}{\mathrm{\β}}}_{yk}{\mathrm{cos\β}}_{xk}+{\stackrel{\·}{\mathrm{\β}}}_{zk}{\mathrm{sin\β}}_{xk}{\mathrm{cos\β}}_{yk};$

Wherein, ω _zfor stage body Z _pthe synthesis rotational angular velocity of axle; ω _yfor inner frame Y _p1the synthesis rotational angular velocity of axle; ω _xfor outside framework X _p2the synthesis rotational angular velocity of axle; ω _{yk '}for servo-actuated framework Y _p3the synthesis rotational angular velocity of axle.

The servo loop decoupling method of four above-mentioned axle inertially stabilized platform systems, in step (2), measurement obtains four axle inertially stabilized platform internal system and relatively rotates angle and angular velocity by the following method:

At the X of outside framework _p2setting angle sensor on axle, measures and obtains the X of servo-actuated framework around outside framework body coordinate system _p2the angle beta that axle rotates _xk; At the Y of inner frame _p1setting angle sensor on axle, measures and obtains the Y of outside framework around inner frame body coordinate system _p1the angle beta that axle rotates _ykand angular velocity at stage body Z _pon axle, sensor installation measures the angle beta that inner frame rotates around the Zp axle of stage body body coordinate system _zkand angular velocity

The servo loop decoupling method of four above-mentioned axle inertially stabilized platform systems, in step (2), rotational angle β _xk, β _yk, β _zkspan be 0 ~ 360 °.

The present invention compared with prior art has the following advantages:

(1), the decoupling computation formula that provides of the present invention, can relatively rotate within the scope of 0 ~ 360 °, angle in internal system and realize, without singular value calculating, overcoming prior art frame corners β outside _xk=± 90 °, inner frame angle β _yksingular value problem when=± 90 °; Compare existing decoupling method more accurately, applicability is wider;

(2), the present invention in decoupling computation, sine and cosine calculating is carried out on former relative angle speed basis, do not exist gain amplify problem, avoid gain in existing decoupling method and be tending towards infinitely-great problem.

Accompanying drawing explanation

Fig. 1 is the relation schematic diagram in four axle inertially stabilized platform systems between four body coordinate system;

Fig. 2 is dynamic tuning gyroscope four axle inertially stabilized platform servo loop theory diagram in the decoupling zero scheme adopted in prior art;

Fig. 3 is four axle Inertial Platform System servo loop decoupling methods of the present invention;

Fig. 4 is dynamic tuning gyroscope four axle inertially stabilized platform servo loop theory diagram in the decoupling zero scheme that adopts of the present invention.

Embodiment

Below in conjunction with the drawings and specific embodiments, the present invention is described in further detail:

The servo loop decoupling method of a kind of four axle inertially stabilized platform systems provided by the invention, realizes based on four axle inertially stabilized platform systems.This four-axis stable platform system comprises pedestal, servo-actuated framework, outside framework, inner frame and stage body, and corresponding body coordinate system is respectively base body coordinate system X ₁y ₁z ₁, servo-actuated frame coordinates system X _p3y _p3z _p3, outside framework body coordinate system X _p2y _p2z _p2, inner frame body coordinate system X _p1y _p1z _p1with stage body body coordinate system X _py _pz _p.

The relation schematic diagram of five coordinate systems as shown in Figure 1, the initial point of above-described five coordinate systems overlaps, and there is following relative restraint relation: the Z of stage body body coordinate system _pthe Z of axle and inner frame body coordinate system _p1axle overlaps, the Y of the body coordinate system of outside framework _p2the Y of axle and inner frame body coordinate system _p1axle overlaps, the X of servo-actuated frame body coordinate system _p3the X of axle and outside framework body coordinate system _p2axle overlaps, the X of base body coordinate system ₁axle overlaps with the Y-axis of servo-actuated frame body coordinate system.Wherein, pedestal and carrier are connected, described Stable Platform System carrier drive issue raw inside relatively rotate time: pedestal is around the Y of servo-actuated frame body coordinate system _p3axle rotates and rotational angle is β _{yk '}; Servo-actuated framework is around the X of outside framework body coordinate system _p2axle rotates and rotational angle is β _xk; Outside framework is around the Y of inner frame body coordinate system _p1axle rotates and rotational angle is β _yk, inner frame is around the Z of stage body body coordinate system _paxle rotates and rotational angle is β _zk.

Processing flow chart as shown in Figure 3, four axle Inertial Platform System servo loop decoupling method performing steps of the present invention are as follows:

(1) angular velocity, according to the gyroscope that stage body is installed exported, obtains stage body at X _paxle, Y _paxle and Z _pangular velocity component on axle

(2), measurement obtains four axle inertially stabilized platform internal system and relatively rotates angle and angular velocity by the following method:

At the X of outside framework _p2setting angle sensor on axle, measures and obtains the X of servo-actuated framework around outside framework body coordinate system _p2the angle beta that axle rotates _xk; At the Y of inner frame _p1setting angle sensor on axle, measures and obtains the Y of outside framework around inner frame body coordinate system _p1the angle beta that axle rotates _ykand angular velocity at stage body Z _pon axle, sensor installation measures the angle beta that inner frame rotates around the Zp axle of stage body body coordinate system _zkand angular velocity wherein, what above measurement obtained relatively rotates angle beta _xk, β _yk, β _zkspan be 0 ~ 360 °, namely the method is applicable to full Attitude Calculation.

(3), calculate the rotational angular velocity of stage body, inner frame, outside framework and servo-actuated framework, specific formula for calculation is as follows:

${\mathrm{\ω}}_z={\mathrm{\ω}}_{z_p};$

${\mathrm{\ω}}_y={\mathrm{\ω}}_{y_p}{\mathrm{cos\β}}_{zk}-{\mathrm{\ω}}_{x_p}{\mathrm{sin\β}}_{zk};$

${\mathrm{\ω}}_x={\mathrm{\ω}}_{y_p}{\mathrm{cos\β}}_{yk}{\mathrm{sin\β}}_{zk}+{\mathrm{\ω}}_{x_p}{\mathrm{cos\β}}_{yk}{\mathrm{cos\β}}_{zk}-{\stackrel{\·}{\mathrm{\β}}}_{zk}{\mathrm{sin\β}}_{yk};$

${\mathrm{\ω}}_{{\mathrm{yk}}^{\′}}={\mathrm{\ω}}_{y_p}{\mathrm{sin\β}}_{xk}{\mathrm{sin\β}}_{yk}{\mathrm{sin\β}}_{zk}+{\mathrm{\ω}}_{x_p}{\mathrm{sin\β}}_{xk}{\mathrm{sin\β}}_{yk}{\mathrm{cos\β}}_{zk}+{\stackrel{\·}{\mathrm{\β}}}_{yk}{\mathrm{cos\β}}_{xk}+{\stackrel{\·}{\mathrm{\β}}}_{zk}{\mathrm{sin\β}}_{xk}{\mathrm{cos\β}}_{yk};$

Wherein, ω _zfor stage body Z _pthe synthesis rotational angular velocity of axle; ω _yfor inner frame Y _p1the synthesis rotational angular velocity of axle; ω _xfor outside framework X _p2the synthesis rotational angular velocity of axle; ω _{yk '}for servo-actuated framework Y _p3the synthesis rotational angular velocity of axle.

According to four axle Inertial Platform System servo loop decoupling methods provided by the invention, the servo loop theory diagram in engineer applied as shown in Figure 4.

Embodiment 1:

In the present embodiment, utilize computing formula of the present invention to carry out decoupling computation, wherein impose a condition as follows: pedestal is around outside framework coordinate system X _p2the angle beta that axle rotates _xk=0; Outside framework is around inner frame coordinate system Y _p1the angle beta that axle rotates _yk=0; Inner frame is around stage body coordinate system Z _pthe angle beta that axle rotates _zk=0; Namely mutually vertical between three rotation axiss.

Can obtain according to the invention provides computing formula:

${\mathrm{\ω}}_z={\mathrm{\ω}}_{z_p};$

${\mathrm{\ω}}_y={\mathrm{\ω}}_{y_p};$

${\mathrm{\ω}}_x={\mathrm{\ω}}_{x_p};$

${\mathrm{\ω}}_{{\mathrm{yk}}^{\′}}={\stackrel{\·}{\mathrm{\β}}}_{yk};$

As can be seen from above result of calculation, stage body three-axis controller input quantity is consistent with respective gyrostatic measured value respectively, and servo-actuated framework controlled quentity controlled variable input is relevant with inner frame angular velocity.

Embodiment 2:

In the present embodiment, utilize computing formula of the present invention to carry out decoupling computation, wherein impose a condition as follows: pedestal is around outside framework coordinate system X _p2the angle beta that axle rotates _xk=90 °; Outside framework is around inner frame coordinate system Y _p1the angle beta that axle rotates _yk=0; Inner frame is around stage body coordinate system Z _pthe angle beta that axle rotates _zk=0.

Can obtain according to the invention provides computing formula:

${\mathrm{\ω}}_z={\mathrm{\ω}}_{z_p};$

${\mathrm{\ω}}_y={\mathrm{\ω}}_{y_p};$

${\mathrm{\ω}}_x={\mathrm{\ω}}_{x_p};$

${\mathrm{\ω}}_{{\mathrm{yk}}^{\′}}={\stackrel{\·}{\mathrm{\β}}}_{zk};$

As can be seen from above result of calculation, stage body three-axis controller input quantity is consistent with respective gyrostatic measured value respectively, and servo-actuated framework controlled quentity controlled variable input is relevant with stage body angular velocity.

Embodiment 3:

In the present embodiment, utilize computing formula of the present invention to carry out decoupling computation, wherein impose a condition as follows: pedestal is around outside framework coordinate system X _p2the angle beta that axle rotates _xk=90 °; Outside framework is around inner frame coordinate system Y _p1the angle beta that axle rotates _yk=90 °; Inner frame is around stage body coordinate system Z _pthe angle beta that axle rotates _zk=0.

Can obtain according to the invention provides computing formula:

${\mathrm{\ω}}_z={\mathrm{\ω}}_{z_p};$

${\mathrm{\ω}}_y={\mathrm{\ω}}_{y_p};$

${\mathrm{\ω}}_x=-{\stackrel{\·}{\mathrm{\β}}}_{zk};$

${\mathrm{\ω}}_{{\mathrm{yk}}^{\′}}={\mathrm{\ω}}_{x_p};$

As can be seen from above result of calculation, except stage body Y and Z axis controlled quentity controlled variable are consistent with respective gyrostatic measured value respectively, servo-actuated frame controller input quantity is relevant with X gyroscope, and outside framework input quantity is relevant with stage body shaft angle speed.

Above-mentioned three embodiments can verify that decoupling method of the present invention is correct.

The above; be only the present invention's embodiment, but protection scope of the present invention is not limited thereto, is anyly familiar with those skilled in the art in the technical scope that the present invention discloses; the change that can expect easily or replacement, all should be encompassed within protection scope of the present invention.

The content be not described in detail in instructions of the present invention belongs to the known technology of professional and technical personnel in the field.

Claims (3)

1. the servo loop decoupling method of an axle inertially stabilized platform system, it is characterized in that: realize based on four axle inertially stabilized platform systems, described Stable Platform System comprises pedestal, servo-actuated framework, outside framework, inner frame and stage body, and corresponding body coordinate system is respectively base body coordinate system X ₁y ₁z ₁, servo-actuated frame coordinates system X _p3y _p3z _p3, outside framework body coordinate system X _p2y _p2z _p2, inner frame body coordinate system X _p1y _p1z _p1with stage body body coordinate system X _py _pz _p; The initial point of described five coordinate systems overlaps, and: the Z of stage body body coordinate system _pthe Z of axle and inner frame body coordinate system _p1axle overlaps, the Y of the body coordinate system of outside framework _p2the Y of axle and inner frame body coordinate system _p1axle overlaps, the X of servo-actuated frame body coordinate system _p3the X of axle and outside framework body coordinate system _p2axle overlaps, the X of base body coordinate system ₁axle overlaps with the Y-axis of servo-actuated frame body coordinate system; Wherein, pedestal and carrier are connected, described Stable Platform System carrier drive issue raw inside relatively rotate time, pedestal is around the Y of servo-actuated frame body coordinate system _p3axle rotates, and servo-actuated framework is around the X of outside framework body coordinate system _p2axle rotates, and outside framework is around the Y of inner frame body coordinate system _p1axle rotates, and inner frame is around the Z of stage body body coordinate system _paxle rotates;

Described four axle Inertial Platform System servo loop decoupling method performing steps are as follows:

(1) angular velocity, according to the gyroscope that stage body is installed exported, obtains stage body at X _paxle, Y _paxle and Z _pangular velocity component on axle

(2), measure obtain four axle inertially stabilized platform internal system angle and angular velocity in relative rotation, comprising: servo-actuated framework is around the X of outside framework body coordinate system _p2the angle beta that axle rotates _xk, outside framework is around the Y of inner frame body coordinate system _p1the angle beta that axle rotates _ykand angular velocity inner frame is around the Z of stage body body coordinate system _pthe angle beta that axle rotates _zkand angular velocity

(3), calculate the rotational angular velocity of stage body, inner frame, outside framework and servo-actuated framework, specific formula for calculation is as follows:

${\mathrm{\ω}}_z={\mathrm{\ω}}_{z_p};$

${\mathrm{\ω}}_y={\mathrm{\ω}}_{y_p}{\mathrm{cos\β}}_{zk}-{\mathrm{\ω}}_{x_p}{\mathrm{sin\β}}_{zk};$

${\mathrm{\ω}}_x={\mathrm{\ω}}_{y_p}{\mathrm{cos\β}}_{yk}{\mathrm{sin\β}}_{zk}+{\mathrm{\ω}}_{x_p}{\mathrm{cos\β}}_{yk}{\mathrm{cos\β}}_{zk}-{\stackrel{\·}{\mathrm{\β}}}_{zk}{\mathrm{sin\β}}_{yk};$

${\mathrm{\ω}}_{{\mathrm{yk}}^{\′}}={\mathrm{\ω}}_{y_p}{\mathrm{sin\β}}_{xk}{\mathrm{sin\β}}_{yk}{\mathrm{sin\β}}_{zk}+{\mathrm{\ω}}_{x_p}{\mathrm{sin\β}}_{xk}{\mathrm{sin\β}}_{yk}{\mathrm{cos\β}}_{zk}+{\stackrel{\·}{\mathrm{\β}}}_{yk}{\mathrm{cos\β}}_{xk}+{\stackrel{\·}{\mathrm{\β}}}_{zk}{\mathrm{sin\β}}_{xk}{\mathrm{cos\β}}_{yk};$

Wherein, ω _zfor stage body Z _pthe synthesis rotational angular velocity of axle; ω _yfor inner frame Y _p1the synthesis rotational angular velocity of axle; ω _xfor outside framework X _p2the synthesis rotational angular velocity of axle; ω _{yk '}for servo-actuated framework Y _p3the synthesis rotational angular velocity of axle.

2. the servo loop decoupling method of a kind of four axle inertially stabilized platform systems according to claim 1, it is characterized in that: in step (2), measurement obtains four axle inertially stabilized platform internal system and relatively rotates angle and angular velocity by the following method:

At the X of outside framework _p2setting angle sensor on axle, measures and obtains the X of servo-actuated framework around outside framework body coordinate system _p2the angle beta that axle rotates _xk; At the Y of inner frame _p1setting angle sensor on axle, measures and obtains the Y of outside framework around inner frame body coordinate system _p1the angle beta that axle rotates _ykand angular velocity at stage body Z _pon axle, sensor installation measures the angle beta that inner frame rotates around the Zp axle of stage body body coordinate system _zkand angular velocity

3. the servo loop decoupling method of a kind of four axle inertially stabilized platform systems according to claim 1 and 2, is characterized in that: in step (2), rotational angle β _xk, β _yk, β _zkspan be 0 ~ 360 °.