CN105698793A - Servo loop decoupling method of four-axis inertial stable platform system - Google Patents

Abstract

The invention provides a servo loop decoupling method of a four-axis inertial stable platform system. The servo loop decoupling method comprises the following steps: 1, obtaining angular speed components of a platform body in Xp axis, Yp axis and Zp axis according to an angular speed output by a gyroscope mounted on the platform body; 2, measuring to obtain relatively rotary angle and angular speed in the four-axis inertial stable platform system; and 3, calculating a synthesized rotary angular speed of the platform body, an inner framework, an outer framework and a follow-up framework. According to the method provided by the invention, servo loop decoupling calculation without singular values is realized under the condition that the relative rotary angle of the platform system is an arbitrary value, so that all-attitude adaptability under a carrier track-free constraint condition is improved.

Description

Servo loop decoupling method of four-axis inertially stabilized platform system

Technical Field

The invention relates to the technical field of inertial measurement, in particular to a servo loop decoupling method of a four-axis inertially stabilized platform system, which is mainly used for full-attitude high-precision navigation in the fields of aviation and aerospace.

Background

Because the three-axis inertial platform system has the phenomenon of frame locking, the requirement of large maneuvering motion of the carrier is difficult to meet, and therefore, the four-axis inertial platform system is generated. The four-axis inertial platform system is additionally provided with a follow-up frame on the basis of the platform body, the inner frame and the outer frame relative to the three-axis inertial platform system, and the follow-up frame is positioned between the outer frame and the base of the platform.

The traditional solution is as follows: the follow-up loop signal comes from the inner gimbal angle, and a secant resolver is adopted for gain compensation. However, this method has a disadvantage that singular values exist at an outer frame angle of 90 °. See the literature, "four-axis platform servo system modeling research, chinese technical literature report of inertia vol.10, No.5, 10 months 2002", and "model analysis and design, navigation and control of four-axis platform servo system, 4 th year 2014".

At present, the main solutions to this problem are: and when the angle of the outer frame is 90 degrees, the frame is locked, and once the angle value is passed, the original follow-up scheme is restored. See the literature "motion characteristic simulation analysis, navigation and control at a four-axis platform outer frame angle of ± 90 °, stage 2 of 2009". However, the method has the defects that when the angle of the outer frame is always kept at 90 degrees, the four-axis platform is degenerated into a three-axis platform, the phenomenon of frame locking still exists, and the full-posture function of carrier motion cannot be realized.

In summary, the above method has the disadvantage of overcoming singular values by conditional judgment, and for this reason, a decoupling method which is free of singular values and is not limited by a trajectory needs to be researched.

The following describes the current state of the art:

first, five body coordinate systems of a four-axis inertially stabilized platform system are defined as shown in fig. 1, and the relationship between the body coordinate systems can be seen. In FIG. 1, letIs the relative angular velocity of the inner frame relative to the table body,the relative angular velocity of the outer frame relative to the inner frame,is the relative angular velocity of the base (arrow) relative to the outer frame,is the relative angular velocity of the base (arrow) relative to the follower frame.

Is provided withIs a table body (including a gyroscope shell) pair X_p、Y_p、Z_pThe rotational inertia of the shaft;is an inner frame pair X_p1、Y_p1、Z_p1The rotational inertia of the shaft;is an outer frame pair X_p2、Y_p2、Z_p2The rotational inertia of the shaft;for the following frame pair X_p3、Y_p3、Z_p3The moment of inertia of the shaft. Definition ofFor folding to table body axis X_pThe moment of inertia of the rotor (c),to be folded to the table body axis Y_pThe moment of inertia of (a); j. the design is a square_xy、J_yx、J_xz、J_yzIs the equivalent product of inertia of the frame system. 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{\β}}_{yk}{\cos }^2{\mathrm{\β}}_{xk}\\ +\frac{1}{4}(J_{z_{p3}}-J_{y_{p3}})\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}(J_{z_{p3}}-J_{y_{p3}})\sin 2{\mathrm{\β}}_{zk}{\mathrm{sin\β}}_{yk}\sin 2{\mathrm{\β}}_{xk}\end{array}---\left(2\right)$

$\begin{array}{c}{J}_{xy}=\frac{1}{2}(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})\sin 2{\mathrm{\β}}_{zk}\\ +\frac{1}{2}(J_{y_{p3}}-J_{z_{p3}}){\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}}^2{\mathrm{cos\β}}_{xk})sin2{\mathrm{\β}}_{yk}{\mathrm{cos\β}}_{zk}---\left(4\right)$

$\begin{array}{c}{J}_{yx}=\frac{1}{2}(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})\sin 2{\mathrm{\β}}_{zk}\\ -\frac{1}{2}(J_{y_{p3}}-J_{z_{p3}}){\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)$

let M_zpIs the interference torque of the table body shaft,the moment is fed back by a platform shaft moment motor;in order to disturb the moment for the inner frame shaft,feeding back torque for the torque motor of the inner frame shaft;in order to disturb the moment of the outer frame shaft,is an outer frame shaft torque motorFeeding back a torque;in order to follow up the external moment on the frame shaft,feeding back the torque for the follow-up frame shaft torque motor; the resultant moment of each shaft end of the four-shaft inertial platform system acting on the three shafts of the platform body is as follows:

$\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_{x_{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)$

respectively being a table body wound with x_p、y_p、z_pThe absolute angular velocity of the shaft can be obtained by measuring through a gyroscope orthogonally arranged on the table body; the dynamic equation of the platform body of the four-axis 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)$

It can be seen that at 3 gyroscope angular ratesWhen the information is known, there are 4 control execution linksThe servo loop is not fully controllable, therefore, the current solution is to add a servo control loop such that the inner gimbal angle β_ykIs approximately 0 DEG, and has a dynamic equation

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

After combining the three-axis dynamic equation of the platform body, the dynamic equation of the four-axis inertial platform system is as follows:

$\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_{y3}\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}$

at this point, the spatial decoupling matrix is at β_ykWhen the value is approximately 0, the operation can be simplified 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)$

As shown in the schematic block diagram of the prior art decoupling servo loop in FIG. 2, the prior art adds a secant resolver sec β to a coordinate resolver of the original three stable loops_xk. Namely the handleAs a part of the follow-up loop, but is also seen at β_xkTowards ± 90 °, singular values, sec β, exist_xkTending to infinity.

Disclosure of Invention

The invention aims to overcome the defects of the prior art and provides a servo loop decoupling method of a four-axis inertially stabilized platform system, which can realize the decoupling calculation of a servo loop without singular values under the condition that the relative rotation angle of the platform system is any value, thereby improving the full-attitude adaptability of a carrier under the condition of no-track constraint.

The above object of the present invention is achieved by the following technical solutions:

a servo loop decoupling method of a four-axis inertially stabilized platform system is achieved based on the four-axis inertially stabilized platform system, and the stabilized platform system comprises a base, a follow-up frame, an outer frame, an inner frame, a platform body and corresponding servo loopsThe body coordinate system is respectively a base body coordinate system X₁Y₁Z₁And a following frame coordinate system X_p3Y_p3Z_p3Outer frame body coordinate system X_p2Y_p2Z_p2Inner frame body coordinate system X_p1Y_p1Z_p1And table body coordinate system X_pY_pZ_p(ii) a The origins of the five coordinate systems coincide, and: z of table body coordinate system_pZ of axis and inner frame body coordinate system_p1Y of body coordinate system of axis coincidence and outer frame_p2Y of axis and inner frame body coordinate system_p1X of the coordinate system of the axis coincidence, follow-up frame body_p3X of axis and outer frame body coordinate system_p2X of axis coincident, base body coordinate system₁The axis is superposed with the Y axis of the servo frame body coordinate system; wherein the base is fixedly connected with the carrier, and when the stable platform system rotates relatively internally under the driving of the carrier, the base rotates around the Y of the coordinate system of the follow-up frame body_p3The axis rotates, and the follow-up frame rotates around the X of the coordinate system of the outer frame body_p2The shaft rotates, the outer frame rotates around the Y of the coordinate system of the inner frame body_p1Z of coordinate system of axis rotation and internal frame around table body_pRotating the shaft;

the servo loop decoupling method of the four-axis inertial platform system comprises the following steps:

(1) obtaining the angular velocity of the table body X according to the output angular velocity of the gyroscope arranged on the table body_pAxis, Y_pAxis and Z_pComponent of angular velocity on the shaft

(2) The inside relative pivoted angle and the angular velocity of four-axis inertially stabilized platform system are obtained in the measurement, include: x of following frame around outer frame body coordinate system_p2Angle of rotation β of shaft_xkY of coordinate system of outer frame around inner frame body_p1Angle of rotation β of shaft_ykAnd angular velocityZ of internal frame winding table body coordinate system_pAngle of rotation β of shaft_zkAnd angular velocity

(3) And calculating the rotating angular speeds of the table body, the inner frame, the outer frame and the follow-up frame, wherein the specific calculation formula 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, ω is_zIs a table body Z_pThe resultant rotational angular velocity of the shaft; omega_yIs an inner frame Y_p1The resultant rotational angular velocity of the shaft; omega_xIs an outer frame X_p2The resultant rotational angular velocity of the shaft; omega_yk′Is a follow-up frame Y_p3The resultant rotational angular velocity of the shaft.

In the servo loop decoupling method of the four-axis inertially stabilized platform system, in the step (2), the relative rotation angle and the angular velocity of the inner part of the four-axis inertially stabilized platform system are measured by the following method:

at X of the outer frame_p2An angle sensor is arranged on the shaft, and the X of the servo frame around the outer frame body coordinate system is obtained through measurement_p2Angle of rotation β of shaft_xk(ii) a Y of the inner frame_p1An angle sensor is arranged on the shaft, and the Y of the coordinate system of the outer frame around the inner frame body is obtained through measurement_p1Angle of rotation β of shaft_ykAnd angular velocityOn the table body Z_pThe on-axis sensor measures the rotation angle β of the inner frame around the Zp axis of the table body coordinate system_zkAnd angular velocity

In the servo loop decoupling method of the four-axis inertially stabilized platform system, in the step (2), the rotation angle β_xk、β_yk、β_zkThe value range of (a) is 0-360 degrees.

Compared with the prior art, the invention has the following advantages:

(1) the decoupling calculation formula provided by the invention can realize non-singular value calculation within the range of 0-360 degrees of the relative rotation angle in the system, and overcomes the defect that the prior art has an outer frame angle β_xkAngle of inner gimbal β ═ 90 °_ykSingular value problem at ± 90 °; compared with the existing decoupling method, the method is more accurate and has wider applicability;

(2) in the decoupling calculation, the sine and cosine calculation is carried out on the basis of the original relative angular velocity, the problem of gain amplification does not exist, and the problem that the gain tends to be infinite in the conventional decoupling method is avoided.

Drawings

FIG. 1 is a schematic diagram of a relationship between four body coordinate systems in a four-axis inertially stabilized platform system;

FIG. 2 is a schematic block diagram of a servo loop of a four-axis inertially stabilized platform of a dynamic tuning gyroscope in a decoupling scheme adopted in the prior art;

FIG. 3 illustrates a servo loop decoupling method for a four-axis inertial platform system according to the present invention;

FIG. 4 is a schematic block diagram of a servo loop of a four-axis inertially stabilized platform of a dynamic tuning gyroscope in a decoupling scheme adopted by the invention.

Detailed Description

The invention is described in further detail below with reference to the following figures and specific examples:

the invention provides a servo loop decoupling method of a four-axis inertially stabilized platform system, which is realized based on the four-axis inertially stabilized platform system. The four-axis stabilized platform system comprises a base, a follow-up frame, an outer frame, an inner frame and a platform body, wherein corresponding body coordinate systems are respectively a base body coordinate system X₁Y₁Z₁And a following frame coordinate system X_p3Y_p3Z_p3Outer frame body coordinate system X_p2Y_p2Z_p2Inner frame body coordinate system X_p1Y_p1Z_p1And table body coordinate system X_pY_pZ_p。

As shown in fig. 1, the relationship diagram of the five coordinate systems, the origins of the five coordinate systems are coincident, and the following relative constraint relationship exists: z of table body coordinate system_pZ of axis and inner frame body coordinate system_p1Y of body coordinate system of axis coincidence and outer frame_p2Y of axis and inner frame body coordinate system_p1X of the coordinate system of the axis coincidence, follow-up frame body_p3X of axis and outer frame body coordinate system_p2X of axis coincident, base body coordinate system₁The axis coincides with the Y-axis of the slave frame body coordinate system. Wherein, the base is fixedly connected with the carrier, when the stable platform system is driven by the carrier to rotate relatively internally: y of base around follow-up frame body coordinate system_p3The shaft rotates at β_yk′(ii) a X of following frame around outer frame body coordinate system_p2The shaft rotates at β_xk(ii) a Y of body coordinate system of outer frame around inner frame_p1The shaft rotates at β_ykZ of coordinate system of inner frame wound stage body_pThe shaft rotates at β_zk。

As shown in the processing flow chart of fig. 3, the decoupling method for the servo loop of the four-axis inertial platform system of the present invention comprises the following steps:

(1) obtaining the angular velocity of the table body X according to the output angular velocity of the gyroscope arranged on the table body_pAxis, Y_pAxis and Z_pAngle on axisComponent of velocity

(2) The relative rotation angle and the angular speed of the inner part of the four-axis inertially stabilized platform system are measured by the following method:

at X of the outer frame_p2An angle sensor is arranged on the shaft, and the X of the servo frame around the outer frame body coordinate system is obtained through measurement_p2Angle of rotation β of shaft_xk(ii) a Y of the inner frame_p1An angle sensor is arranged on the shaft, and the Y of the coordinate system of the outer frame around the inner frame body is obtained through measurement_p1Angle of rotation β of shaft_ykAnd angular velocityOn the table body Z_pThe on-axis sensor measures the rotation angle β of the inner frame around the Zp axis of the table body coordinate system_zkAnd angular velocityWherein the relative rotation angle β is obtained by the above measurement_xk、β_yk、β_zkThe value range of (1) is 0-360 degrees, namely the method is suitable for all-attitude calculation.

(3) And calculating the rotating angular speeds of the table body, the inner frame, the outer frame and the follow-up frame, wherein the specific calculation formula 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, ω is_zIs a table body Z_pThe resultant rotational angular velocity of the shaft; omega_yIs an inner frame Y_p1The resultant rotational angular velocity of the shaft; omega_xIs an outer frame X_p2The resultant rotational angular velocity of the shaft; omega_yk′Is a follow-up frame Y_p3The resultant rotational angular velocity of the shaft.

According to the servo loop decoupling method of the four-axis inertial platform system, a servo loop principle block diagram in engineering application is shown in fig. 4.

Example 1:

in this embodiment, the decoupling calculation is performed by using the calculation formula of the present invention, where the setting conditions are as follows: coordinate system X of base surrounding outer frame_p2Angle of rotation β of shaft_xk0; coordinate system Y of outer frame around inner frame_p1Angle of rotation β of shaft_yk0; coordinate system Z of internal frame winding table body_pAngle of rotation β of shaft_zk0; i.e. the three axes of rotation are perpendicular to each other.

The calculation formula provided according to the invention can be obtained:

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

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

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

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

from the above calculation results, the input values of the three-axis table controller are respectively consistent with the measured values of the respective gyroscopes, and the input of the control value of the follow-up frame is related to the angular velocity of the inner frame.

Example 2:

in this embodiment, the decoupling calculation is performed by using the calculation formula of the present invention, where the setting conditions are as follows: coordinate system X of base surrounding outer frame_p2Angle of rotation β of shaft_xk90 °; coordinate system Y of outer frame around inner frame_p1Angle of rotation β of shaft_yk0; coordinate system Z of internal frame winding table body_pAngle of rotation β of shaft_zk＝0。

The calculation formula provided according to the invention can be obtained:

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

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

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

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

from the above calculation results, the input values of the three-axis table controller are respectively consistent with the measured values of the respective gyroscopes, and the input values of the follow-up frame control values are related to the angular velocity of the table.

Example 3:

in this embodiment, the decoupling calculation is performed by using the calculation formula of the present invention, where the setting conditions are as follows: coordinate system X of base surrounding outer frame_p2Angle of rotation β of shaft_xk90 °; coordinate system Y of outer frame around inner frame_p1Angle of rotation β of shaft_yk90 °; coordinate system Z of internal frame winding table body_pAngle of rotation β of shaft_zk＝0。

The calculation formula provided according to the invention can be obtained:

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

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

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

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

from the above calculation results, it can be seen that, in addition to the table body Y and Z axis control amounts respectively corresponding to the respective gyro measurement values, the following frame controller input amount is related to the X gyro, and the outer frame input amount is related to the table body axis angular rate.

The three embodiments described above can verify that the decoupling method of the present invention is correct.

The above description is only one embodiment of the present invention, but the scope of the present invention is not limited thereto, and any changes or substitutions that can be easily conceived by those skilled in the art within the technical scope of the present invention are included in the scope of the present invention.

Those skilled in the art will appreciate that the invention may be practiced without these specific details.

Claims (3)

1. A servo loop decoupling method of a four-axis inertially stabilized platform system is characterized by comprising the following steps: based on the realization of four-axis inertially stabilized platform system, the stabilized platform system comprises a base, a follow-up frame, an outer frame, an inner frame and a platform body, and the corresponding body coordinate systems are respectively a base body coordinate system X₁Y₁Z₁And a following frame coordinate system X_p3Y_p3Z_p3Outer frame body coordinate system X_p2Y_p2Z_p2Inner frame body coordinate system X_p1Y_p1Z_p1And table body coordinate systemX_pY_pZ_p(ii) a The origins of the five coordinate systems coincide, and: z of table body coordinate system_pZ of axis and inner frame body coordinate system_p1Y of body coordinate system of axis coincidence and outer frame_p2Y of axis and inner frame body coordinate system_p1X of the coordinate system of the axis coincidence, follow-up frame body_p3X of axis and outer frame body coordinate system_p2X of axis coincident, base body coordinate system₁The axis is superposed with the Y axis of the servo frame body coordinate system; wherein the base is fixedly connected with the carrier, and when the stable platform system rotates relatively internally under the driving of the carrier, the base rotates around the Y of the coordinate system of the follow-up frame body_p3The axis rotates, and the follow-up frame rotates around the X of the coordinate system of the outer frame body_p2The shaft rotates, the outer frame rotates around the Y of the coordinate system of the inner frame body_p1Z of coordinate system of axis rotation and internal frame around table body_pRotating the shaft;

the servo loop decoupling method of the four-axis inertial platform system comprises the following steps:

(1) obtaining the angular velocity of the table body X according to the output angular velocity of the gyroscope arranged on the table body_pAxis, Y_pAxis and Z_pComponent of angular velocity on the shaft

(2) The inside relative pivoted angle and the angular velocity of four-axis inertially stabilized platform system are obtained in the measurement, include: x of following frame around outer frame body coordinate system_p2Angle of rotation β of shaft_xkY of coordinate system of outer frame around inner frame body_p1Angle of rotation β of shaft_ykAnd angular velocityZ of internal frame winding table body coordinate system_pAngle of rotation β of shaft_zkAnd angular velocity

(3) And calculating the rotating angular speeds of the table body, the inner frame, the outer frame and the follow-up frame, wherein the specific calculation formula 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, ω is_zIs a table body Z_pThe resultant rotational angular velocity of the shaft; omega_yIs an inner frame Y_p1The resultant rotational angular velocity of the shaft; omega_xIs an outer frame X_p2The resultant rotational angular velocity of the shaft; omega_yk′Is a follow-up frame Y_p3The resultant rotational angular velocity of the shaft.

2. The servo loop decoupling method of the four-axis inertially stabilized platform system according to claim 1, wherein: in the step (2), the relative rotation angle and the angular speed of the inner part of the four-axis inertially stabilized platform system are measured by the following method:

at X of the outer frame_p2An angle sensor is arranged on the shaft, and the X of the servo frame around the outer frame body coordinate system is obtained through measurement_p2Angle of rotation β of shaft_xk(ii) a Y of the inner frame_p1An angle sensor is arranged on the shaft, and the Y of the coordinate system of the outer frame around the inner frame body is obtained through measurement_p1Angle of rotation β of shaft_ykAnd angular velocityOn the table body Z_pThe on-axis sensor measures the rotation angle β of the inner frame around the Zp axis of the table body coordinate system_zkAnd angular velocity

3. The servo loop decoupling method for the four-axis inertially stabilized platform system according to claim 1 or 2, wherein in step (2), the rotation angle β is determined_xk、β_yk、β_zkThe value range of (a) is 0-360 degrees.