CN103488081B - Inertially-stabilizeplatform platform control method - Google Patents

Abstract

A kind of Inertially-stabilizeplatform platform control method, comprises step: under quiet pedestal, and the roll frame obtaining inertially stabilized platform is relative to the first rotational angular velocity of pedestal and pitching frame the second rotational angle relative to roll frame; When the sine value of the second rotational angle and the absolute value of the first rotational angular velocity product are less than predetermined threshold value, the pitching frame obtaining inertially stabilized platform relative to the second rotational angular velocity of roll frame, orientation frame relative to the 3rd rotational angular velocity of pitching frame; Euler dynamical equations is utilized to set up the kinetic model of inertially stabilized platform under quiet pedestal; According to the input voltage of described first rotational angular velocity, the second rotational angular velocity, the 3rd rotational angular velocity, the linear switching function prestored, the exponentially approaching rule prestored and described kinetic model difference computer azimuth frame, pitching frame and roll frame; Thus orientation frame, pitching frame, roll frame are controlled.The control accuracy of inertially stabilized platform is improve by this programme.

Description

Inertially-stabilizeplatform platform control method

Technical field

The present invention relates to aerial remote sens ing technique field, particularly relate to Inertially-stabilizeplatform platform control method.

Background technology

In recent years, along with deepening continuously of remote sensing application, each field is more and more urgent to the needs setting up stable remote sensing system.Airborne remote sensing system all has unique advantage in maneuverability, real-time, repeated measures, remote sensing equipment convertibility, acquisition high-definition remote sensing data capability, financial cost and stereopsis etc.High resolving power, high precision, real time kinematics imaging are the cores of aerial remote sens ing technique, are the guarantees obtaining high-quality remotely-sensed data.

Inertially stabilized platform technology improves the stability of the imaging load optical axis, makes imaging load can the high-precision image of Real-time Obtaining.Inertially stabilized platform adopts Three shaft frame usually, be followed successively by roll frame, pitching frame and orientation frame from outside to inside, three frameworks isolate the disturbance on three degree of freedom respectively, finally realize being arranged on the vertical over the ground stable of the most upper camera optical axis of inner frame (orientation frame) and stablize the tracking of flight path.

Inertially stabilized platform technology adopts PID (proportional-integral-differential) control method usually.PID is by measuring position, is compared position obtain site error value with desired location, then carries out control according to site error value to inertially stabilized platform and eliminates error amount.But, when inertially stabilized platform frame movement angular velocity and angular acceleration larger time, the Dynamics Coupling between each framework is just relatively more serious, can have influence on the stability of system time serious.And the PID control method usually adopted, be directly calculate site error value, cause the factor of site error value to comprise various disturbing factor, such as eccentric disturbing factor, friction disturbing factor etc.Owing to not estimating coupling torque, the impact that coupling factor brings cannot be solved, thus reduce the control accuracy of inertially stabilized platform.

Summary of the invention

Based on this, be necessary, for the low problem of the control accuracy of inertially stabilized platform, to provide a kind of inertially stabilized platform decoupling control method.

A kind of Inertially-stabilizeplatform platform control method, comprises step:

Under quiet pedestal, the roll frame obtaining inertially stabilized platform is relative to the first rotational angular velocity of pedestal and pitching frame the second rotational angle relative to described roll frame;

When the sine value of described second rotational angle and the absolute value of described first rotational angular velocity product are less than predetermined threshold value, obtain second rotational angular velocity of pitching frame relative to described roll frame of described inertially stabilized platform, orientation frame relative to the 3rd rotational angular velocity of described pitching frame;

Euler dynamical equations is utilized to set up the Platform dynamics equation of inertially stabilized platform under quiet pedestal;

According to the input voltage of described first rotational angular velocity, the second rotational angular velocity, the 3rd rotational angular velocity, the linear switching function prestored, the exponentially approaching rule prestored and described Platform dynamics equation computer azimuth frame, the input voltage of pitching frame and the input voltage of roll frame;

Input voltage according to the input voltage of described orientation frame, the input voltage of pitching frame and roll frame controls orientation frame, pitching frame, roll frame.

Above-mentioned Inertially-stabilizeplatform platform control method, whether predetermined threshold value is less than by the absolute value of the sine value and the first rotational angular velocity product that judge the second rotational angle, when the condition is satisfied, the time exsiccation being coupled as system is disturbed, introduce the linear switching function and exponentially approaching rule that prestore, in conjunction with Platform dynamics equation, calculate the input voltage of orientation frame, the input voltage of pitching frame and the input voltage of roll frame, inertial platform is controlled, realize each framework coupling torque real-Time Compensation, eliminate the Dynamics Coupling impact between each framework, thus improve the control accuracy of inertially stabilized platform, the stability of the Enhanced Imaging load optical axis.

Accompanying drawing explanation

Fig. 1 is the schematic flow sheet of Inertially-stabilizeplatform platform control method embodiment of the present invention;

Fig. 2 is inertially stabilized platform structural representation in the embodiment of the present invention.

Embodiment

Each embodiment below for Inertially-stabilizeplatform platform control method of the present invention is described in detail.

See Fig. 1, be the schematic flow sheet of Inertially-stabilizeplatform platform control method embodiment of the present invention, comprise step:

Step S101: under quiet pedestal, the roll frame obtaining inertially stabilized platform is relative to the first rotational angular velocity of pedestal and pitching frame the second rotational angle relative to roll frame.The prerequisite that this programme is implemented is that the situation of pedestal geo-stationary, is quiet pedestal.The method obtaining the second rotational angle obtains including but not limited to the direct code-disc from being arranged on pitching frame.Obtain the method for the first rotational angular velocity including but not limited to when roll gyro is directly installed on roll frame, the roll frame obtained from roll gyro is the first rotational angular velocity relative to the rotational angular velocity of inertial system.

Step S102: judge whether the sine value of the second rotational angle and the absolute value of the first rotational angular velocity product are less than predetermined threshold value, if so, enter step S103.Wherein, predetermined threshold value sets as required, and object is to judge that this absolute value can be ignored.

Step S103: the pitching frame obtaining inertially stabilized platform is relative to the second rotational angular velocity of roll frame and orientation frame the 3rd rotational angular velocity relative to pitching frame, the method obtaining the second rotational angular velocity measures the second angle of rotation including but not limited to by the code-disc be arranged on pitching frame, and the second angle of rotation differential is obtained the second rotational angular velocity.3rd angle of rotation differential, including but not limited to being measured the 3rd angle of rotation by the code-disc be arranged on orientation frame, is obtained the 3rd rotational angular velocity by the method obtaining the 3rd rotational angular velocity.

Step S104: after calculating (roll frame, orientation frame, pitching frame) momentum moment of each framework, moment, utilizes euler dynamical equations to set up the kinetic model of inertially stabilized platform under quiet pedestal.The momentum moment calculated, moment are substituted into euler dynamical equations, then launches, the kinetic model of inertially stabilized platform under quiet pedestal can be obtained.

Step S105: according to the input voltage of the first rotational angular velocity, the second rotational angular velocity, the 3rd rotational angular velocity, the linear switching function prestored, the exponentially approaching rule prestored and platform kinetics equation computer azimuth frame, the input voltage of pitching frame and the input voltage of roll frame.Wherein, the linear switching function prestored is obtained by rotational angle and rotational angular velocity.Exponentially approaching rule is used for improving the character of non-slip mode.Moment of inertia, motor torque coefficient, armature resistance, ratio of gear are all build-in attributes, can realize prestoring.Input voltage can be obtained in conjunction with switching function, exponentially approaching rule and kinetics equation.

Step S106: the input voltage according to the input voltage of orientation frame, the input voltage of pitching frame and roll frame controls orientation frame, pitching frame, roll frame.Control to refer to by the input voltage of orientation frame being input in the motor of orientation frame to orientation frame according to the input voltage of orientation frame in the present embodiment, the coupling torque of motor driving moment to orientation frame compensates, thus the Dynamics Coupling eliminated between orientation frame and roll frame affects.Roll frame and pitching frame are in like manner.

Whether predetermined threshold value is less than by the absolute value of the sine value and the first rotational angular velocity product that judge the second rotational angle, when the condition is satisfied, the time exsiccation being coupled as system is disturbed, introduce the linear switching function and exponentially approaching rule that prestore, in conjunction with Platform dynamics equation, calculate the input voltage of orientation frame, the input voltage of pitching frame and the input voltage of roll frame, inertial platform is controlled, realize each framework coupling torque real-Time Compensation, eliminate the Dynamics Coupling impact between each framework, thus improve the control accuracy of inertially stabilized platform, the stability of the Enhanced Imaging load optical axis.

In order to more clear this programme, lift several specific embodiment and be described, specific as follows:

Based on rigid dynamics characteristic and space vector superposition principle, set up three axle inertially stabilized platform coordinate systems: base coordinate system, roll frame coordinate system, pitching frame coordinate system, orientation frame coordinate system.Wherein base coordinate system Ox _by _bz _b: x _b, y _b, z _bpoint to respectively the right side of flight carrier, front, on; Roll frame coordinate system Ox _ry _rz _r: roll axle y _rwith y _bin the same way, Ox _ry _rz _rrelative Ox _by _bz _bsystem is around y _baxle rotates, and produces roll angle i.e. the first rotational angle θ _r; Pitching frame coordinate system Ox _fy _fz _f: pitch axis x _fwith x _rin the same way, Ox _fy _fz _frelative Ox _ry _rz _rsystem is around x _raxle rotates, and produces the angle of pitch i.e. the second rotational angle θ _f; Orientation frame coordinate system Ox _ay _az _a: azimuth axis z _awith z _fin the same way, Ox _ay _az _arelative Ox _fy _fz _fsystem is around z _faxle rotates, and produces position angle i.e. the 3rd rotational angle θ _a.And define be respectively pedestal to roll frame, roll frame to pitching frame, pitching frame to the direction cosine matrix of orientation frame, according to coordinate system relation, make rotational angle θ _r, θ _f, θ _ajust be counterclockwise.

for the angular velocity in roll framework relative inertness space is at Ox _ry _rz _rthe expression of system, its projection on x, y, z axle is designated as respectively for the angular velocity in pitching frame relative inertness space is at Ox _fy _fz _fthe expression of system, its projection on x, y, z axle is designated as respectively for the angular velocity in framework relative inertness space, orientation is at Ox _ay _az _athe expression of system, its projection on x, y, z axle is designated as respectively for the relative roll framework of pitching frame the second corner angular velocity, for roll framework opposite base first corner angular velocity, for the method for three turning angles angular velocity of the relative pitching frame of orientation framework.Under pedestal is geo-stationary situation, set up three axis angular rate relation equations, as follows:

1. angular velocity being projected as at roll frame coordinate system in framework relative inertness space is rolled:

${\mathrm{\ω}}_{ir}^r=\left(\begin{array}{c}0\\ {\stackrel{\·}{\mathrm{\θ}}}_r\\ 0\end{array}\right)---\left(1\right)$

2. angular velocity being projected as at pitching frame coordinate system in pitching frame relative inertness space:

${\mathrm{\ω}}_{if}^f=C_r^f{\mathrm{\ω}}_{ir}^r+{\mathrm{\ω}}_{rf}^f=\left(\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_f\\ {\stackrel{\·}{\mathrm{\θ}}}_r{\mathrm{cos\θ}}_f\\ -{\stackrel{\·}{\mathrm{\θ}}}_r{\mathrm{sin\θ}}_f\end{array}\right)---\left(2\right)$

3. angular velocity being projected as at orientation frame coordinate system in orientation frame relative inertness space:

${\mathrm{\ω}}_{ia}^a=C_f^a{\mathrm{\ω}}_{if}^f+{\mathrm{\ω}}_{fa}^a=\left(\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_{f}cos{\mathrm{\θ}}_a+{\stackrel{\·}{\mathrm{\θ}}}_{r}cos{\mathrm{\θ}}_{f}sin{\mathrm{\θ}}_a\\ -{\stackrel{\·}{\mathrm{\θ}}}_f{\mathrm{sin\θ}}_a+{\stackrel{\·}{\mathrm{\θ}}}_r{\mathrm{cos\θ}}_f{\mathrm{cos\θ}}_a\\ {\stackrel{\·}{\mathrm{\θ}}}_a-{\stackrel{\·}{\mathrm{\θ}}}_r{\mathrm{sin\θ}}_f\end{array}\right)---\left(3\right)$

Pitching frame is relative to the second rotational angle θ of roll frame _fcan directly obtain from the code-disc being arranged on pitching frame.Obtain first rotational angular velocity of roll frame relative to pedestal of inertially stabilized platform under quiet pedestal there are two kinds of methods: when roll gyro is directly installed on roll frame, from (1), directly can obtain the first rotational angular velocity from roll gyro when roll gyro installation is on pitching frame, the roll frame obtained from roll gyro relative to the rotational angular velocity of inertial system is from (2), ${\mathrm{\ω}}_{ify}^f={\stackrel{\·}{\mathrm{\θ}}}_r{\mathrm{cos\θ}}_f,$ Then ${\stackrel{\·}{\mathrm{\θ}}}_r=\frac{{\mathrm{\ω}}_{ify}^f}{{\mathrm{cos\θ}}_f}.$

From (3), when when can ignore, now can adopt the decoupling control method based on sliding moding structure, thus need right judge.When the sine value of the second rotational angle and the absolute value of the first rotational angular velocity product are less than predetermined threshold value, namely now adopt the decoupling control method of this programme, in a specific embodiment, when when being false, then adopt traditional pid control algorithm.Predetermined threshold value can set as required, differs and is decided to be 10 ^-7.Roll frame and pitching frame, without the need to judging, directly can adopt decoupling control method.

When when meeting, roll frame, pitching frame and orientation frame all adopt the decoupling control method based on sliding moding structure, specific as follows:

The source of the present embodiment each parameter for convenience of description, lifting one of them concrete inertially stabilized platform and illustrate, as shown in Figure 2, is inertially stabilized platform structural representation in the embodiment of the present invention.Three axle inertially stabilized platforms comprise: roll frame 10, pitching frame 20, orientation frame 30.Roll frame is provided with roll frame torque motor 11 and roll frame code-disc 12, pitching frame 20 is provided with pitching frame torque motor 21, pitching frame code-disc 22, pitching frame rate gyro 23, roll frame rate gyro 24, orientation frame 30 is provided with orientation frame torque motor 31, POS32 and orientation frame rate gyro 33.The present embodiment only enumerates wherein a kind of inertially stabilized platform, but is not limited to the said installation method of the present embodiment.

The POS being installed on orientation frame is utilized to measure orientation frame relative to the rotational angle on three directions of inertial system, as the value of feedback of position ring closed loop; Utilize the roll gyro to measure roll frame being installed on pitching frame relative to the rotational angular velocity of inertial system according to can be calculated the pitch gyro being installed on pitching frame is utilized to measure the rotational angular velocity of pitching frame relative to inertial system the traverse gyro being installed on orientation frame is utilized to measure the rotational angular velocity of orientation frame relative to inertial system because can ignore, so the code-disc being installed on pitching frame is utilized to measure θ _f, utilize the code-disc being installed on roll framework to measure θ _r.

The angular velocity that the rotational angle utilizing POS measurement to obtain and gyro to measure obtain, the linear switching function prestored is:

s＝cx ₁+x ₂

X ₁=-θ is the rotational angle utilizing POS measurement to obtain.

the rotational angular velocity of three the framework relative inertness systems obtained for utilizing gyro to measure.

That is: $s_j=-c_j{\mathrm{\θ}}_j-{\stackrel{\·}{\mathrm{\θ}}}_j,j=a,f,r---\left(4\right)$

For ensureing that the motion of system when sliding mode can non-overshoot ground asymptotically stability be in zero fast, according to statistics, getting c is the negative real root that absolute value is larger, and stores.

The character of non-slip mode is improved with exponentially approaching rule.The exponentially approaching rule prestored is

${\stackrel{\·}{s}}_j=-{\mathrm{\ϵ}}_{j}sign\left(s_j\right)-k_j{s}_j,\mathrm{\ϵ}>0,k>0,j=a,f,r---\left(5\right)$

Speed when ε is phase path arrival diverter surface, ε is less, and the buffeting caused due to inertia is less.K is larger simultaneously, and velocity of approach is faster.So, determine the value of ε and k in advance through statistics, and store.

In conjunction with the exponential approach rate in the linear switching function in (4) and (5), can obtain:

${\stackrel{\·}{s}}_j=c_j{\stackrel{\·}{x}}_1+{\stackrel{\·\·}{x}}_2=-{\mathrm{\ϵ}}_{j}sign\left(s_j\right)-k_j{s}_j---\left(6\right)$

To x ₁, x ₂differentiate can obtain: ${\stackrel{\·}{x}}_1=-\stackrel{\·}{\mathrm{\θ}}=x_2,{\stackrel{\·}{x}}_2=-\stackrel{\·\·}{\mathrm{\θ}}$

Utilize euler dynamical equations to set up the kinetic model under quiet pedestal to inertially stabilized platform, kinetic model comprises:

Orientation frame around the kinetics equation of azimuth axis is:

$J_{az}{\stackrel{\·\·}{\mathrm{\θ}}}_a-J_{az}{\stackrel{\·}{\mathrm{\θ}}}_f{\stackrel{\·}{\mathrm{\θ}}}_r-J_{az}{\mathrm{\θ}}_f{\stackrel{\·\·}{\mathrm{\θ}}}_r=M_z---\left(7\right)$

Wherein, $M_z=-\frac{{\left(K_{Ma}{i}_a\right)}^2}{R_a}{\stackrel{\·}{\mathrm{\θ}}}_a+\frac{K_{Ma}{i}_a}{R_a}{u}_a$

Pitching frame assembly around the kinetics equation of pitch axis is:

$(J_{fx}+J_{ax}){\stackrel{\·\·}{\mathrm{\θ}}}_f+J_{az}{\stackrel{\·}{\mathrm{\θ}}}_a{\stackrel{\·}{\mathrm{\θ}}}_r-(J_{fz}+J_{az}){\mathrm{\θ}}_f{{\stackrel{\·}{\mathrm{\θ}}}_r}^2=M_x---\left(8\right)$

Wherein, $M_x=-\frac{{\left(K_{Mf}{i}_f\right)}^2}{R_f}{\stackrel{\·}{\mathrm{\θ}}}_f+\frac{K_{Mf}{i}_f}{R_f}{u}_f$

Roll frame assembly around roll direction of principal axis kinetics equation is:

$(J_{ry}+J_{fy}+J_{ay}){\stackrel{\·\·}{\mathrm{\θ}}}_r-J_{az}{\mathrm{\θ}}_f{\stackrel{\·\·}{\mathrm{\θ}}}_a-J_{az}{\stackrel{\·}{\mathrm{\θ}}}_f{\stackrel{\·}{\mathrm{\θ}}}_a=M_y,---\left(9\right)$

Wherein, $M_y=-\frac{{\left(K_{Mr}{i}_r\right)}^2}{R_r}{\stackrel{\·}{\mathrm{\θ}}}_r+\frac{K_{Mr}{i}_r}{R_r}{u}_r$

Wherein, J _ax, J _ay, J _azrepresent the orientation framework that the prestores moment of inertia at x, y, z axle respectively; J _fx, J _fy, J _fzrepresent the pitching frame that the prestores moment of inertia at x, y, z axle respectively; J _rythe roll framework that expression prestores is at the moment of inertia of y-axis;

represent three rotational angular velocity of orientation frame relative to pitching frame; represent the three angle of rotation acceleration of orientation frame relative to pitching frame;

θ _frepresent second rotational angle of pitching frame relative to roll frame, represent second rotational angular velocity of pitching frame relative to roll frame; represent the second angle of rotation acceleration of pitching frame relative to roll frame;

represent first rotational angular velocity of roll frame relative to pedestal; represent the first angle of rotation acceleration of roll frame relative to pedestal;

K _ma, K _mfand K _mrrepresent the motor torque coefficient of the motor torque coefficient of the orientation frame prestored, the motor torque coefficient of pitching frame and roll frame respectively.Certainly except by prestoring, also can obtaining by gathering.

R _a, R _fand R _rrepresent the armature resistance of the armature resistance of the orientation frame prestored, the armature resistance of pitching frame and roll frame respectively.Certainly except by prestoring, also can obtaining by gathering.

I _a, i _fand i _rrepresent the ratio of gear of the ratio of gear of the orientation frame prestored, the ratio of gear of pitching frame and roll frame respectively;

U _a, u _fand u _rrepresent the input voltage of orientation frame, the input voltage of pitching frame and the input voltage of roll frame motor respectively;

M _xrepresent the comprehensive moment that pitching frame is subject to along x-axis, M _yrepresent the comprehensive moment that roll frame is subject to along y-axis, M _zrepresent the comprehensive moment that orientation frame is subject to along z-axis.

Can obtain according to formula (7), (8), (9):

${\stackrel{\·\·}{\mathrm{\θ}}}_a=-\frac{{\left(K_{Ma}{i}_a\right)}^2}{J_{az}{R}_a}{\stackrel{\·}{\mathrm{\θ}}}_a+\frac{K_{Ma}{i}_a}{J_{az}{R}_a}{u}_a+{\stackrel{\·}{\mathrm{\θ}}}_f{\stackrel{\·}{\mathrm{\θ}}}_r+{\mathrm{\θ}}_f{\stackrel{\·\·}{\mathrm{\θ}}}_r---\left(10\right)$

${\stackrel{\·\·}{\mathrm{\θ}}}_f=-\frac{{\left(K_{Mf}{i}_f\right)}^2}{(J_{fx}+J_{ax})R_f}{\stackrel{\·}{\mathrm{\θ}}}_f+\frac{K_{Mf}{i}_f}{(J_{fx}+J_{ax})R_f}{u}_f-\frac{J_{az}}{(J_{fx}+J_{ax})}{\stackrel{\·}{\mathrm{\θ}}}_a{\stackrel{\·}{\mathrm{\θ}}}_r+\frac{(J_{fz}+J_{az})}{(J_{fx}+J_{ax})}{\mathrm{\θ}}_f{{\stackrel{\·}{\mathrm{\θ}}}_r}^2---\left(11\right)$

$\begin{array}{c}{\stackrel{\·\·}{\mathrm{\θ}}}_r=-\frac{{\left(K_{Mr}{i}_r\right)}^2}{(J_{ry}+J_{fy}+J_{ay})R_r}{\stackrel{\·}{\mathrm{\θ}}}_r+\frac{K_{Mr}{i}_r}{(J_{ry}+J_{fy}+J_{ay})R_r}{u}_r\\ +\frac{J_{az}}{(J_{ry}+J_{fy}+J_{ay})}{\mathrm{\θ}}_f{\stackrel{\·\·}{\mathrm{\θ}}}_a+\frac{J_{az}}{(J_{ry}+J_{fy}+J_{ay})}{\stackrel{\·}{\mathrm{\θ}}}_f{\stackrel{\·}{\mathrm{\θ}}}_a\end{array}---\left(12\right)$

Therefore, can calculate by formula (6), (10), (11), (12) input voltage that three frameworks click is:

$u_a=\frac{J_{az}{R}_a}{K_{Ma}{i}_a}\left(\right(c_a-\frac{{\left(K_{Ma}{i}_a\right)}^2}{J_{az}{R}_a}\left)\right(-{\stackrel{\·}{\mathrm{\θ}}}_a)+{\mathrm{\ϵ}}_{a}sign(s_a)+k_a{s}_a-\frac{M_{admax}}{J_{az}})---\left(13\right)$

$u_f=\frac{(J_{fx}+J_{ax})R_f}{K_{Mf}{i}_f}\left(\right(c_f-\frac{{\left(K_{Mf}{i}_f\right)}^2}{(J_{fx}+J_{ax})R_f}\left)\right(-{\stackrel{\·}{\mathrm{\θ}}}_f)+{\mathrm{\ϵ}}_{f}sign(s_f)+k_f{s}_f-\frac{M_{fd\max }}{(J_{fx}+J_{ax})})---\left(14\right)$

$\begin{array}{c}{u}_r=\frac{(J_{ry}+J_{fy}+J_{ay})R_r}{K_{Mr}{i}_r}(\left(c_r-\frac{{\left(K_{Mr}{i}_r\right)}^2}{(J_{ry}+J_{fy}+J_{ay})R_r}\right)\left(-{\stackrel{\·}{\mathrm{\θ}}}_r\right)+{\mathrm{\ϵ}}_{r}sign\left(s_r\right)\\ +k_r{s}_r-\frac{M_{rd\max }}{(J_{ry}+J_{fy}+J_{ay})})\end{array}---\left(15\right)$

Wherein, J _ax, J _ay, J _azrepresent the orientation framework that the prestores moment of inertia at x, y, z axle respectively; J _fx, J _fy, J _fzrepresent the pitching frame that the prestores moment of inertia at x, y, z axle respectively; J _rythe roll framework that expression prestores is at the moment of inertia of y-axis;

represent three rotational angular velocity of orientation frame relative to pitching frame, represent second rotational angular velocity of pitching frame relative to roll frame, represent first rotational angular velocity of roll frame relative to pedestal;

K _ma, K _mfand K _mrrepresent the motor torque coefficient of the motor torque coefficient of the orientation frame prestored, the motor torque coefficient of pitching frame and roll frame respectively;

R _a, R _fand R _rrepresent the armature resistance of the armature resistance of the orientation frame prestored, the armature resistance of pitching frame and roll frame respectively;

I _a, i _fand i _rrepresent the ratio of gear of the ratio of gear of the orientation frame prestored, the ratio of gear of pitching frame and roll frame respectively;

U _a, u _fand u _rrepresent the input voltage of orientation frame, the input voltage of pitching frame and the input voltage of roll frame motor respectively;

M _admaxrepresent the maximum coupling torque of the orientation frame prestored, M _fdmaxrepresent the maximum coupling torque of the pitching frame prestored, M _rdmaxrepresent the maximum coupling torque of the roll frame prestored.

In a specific embodiment, before step S105, also comprise the estimation to each framework coupling torque, that is:

Adopt formula $M_{ad\max }=J_{az}{\stackrel{\·}{\mathrm{\θ}}}_{f\max }{\stackrel{\·}{\mathrm{\θ}}}_{r\max }+J_{az}{\mathrm{\θ}}_{fmax}{\stackrel{\·\·}{\mathrm{\θ}}}_{rmax}$ The maximum coupling torque of computer azimuth frame,

Adopt formula calculate the maximum coupling torque of pitching frame,

Adopt formula $M_{rdmax}=J_{az}{\mathrm{\θ}}_{fmax}{\stackrel{\·\·}{\mathrm{\θ}}}_{amax}+J_{az}{\stackrel{\·}{\mathrm{\θ}}}_{fmax}{\stackrel{\·}{\mathrm{\θ}}}_{amax}$ Calculate the maximum coupling torque of roll frame,

represent the 3rd rotational angular velocity maximum in the 3rd rotational angular velocity obtained, represent the 3rd angle of rotation acceleration maximum in the 3rd angle of rotation acceleration obtained, θ _fmaxrepresent the second rotational angle maximum in the second rotational angle obtained, represent the second rotational angular velocity maximum in the second rotational angular velocity obtained, represent obtain the first angle of rotation speed in maximum the first rotational angular velocity, represent the first angle of rotation acceleration maximum in the first angle of rotation acceleration obtained.Thus the estimation achieved coupling torque.

Finally, compensate according to the coupling torque of input voltage to orientation frame, pitching frame, roll frame of the input voltage of orientation frame, the input voltage of pitching frame and roll frame.Each Preset Time, can carry out single compensation, realize the real-Time Compensation of coupling torque.

The present embodiment, utilizes the POS being installed on orientation frame to measure orientation frame relative to the rotational angle on three directions of inertial system; Utilize and be installed on the roll of pitching frame, roll frame measured by pitch gyro and pitching frame relative to the rotational angular velocity of inertial system the traverse gyro being installed on orientation frame is utilized to measure the rotational angular velocity of orientation frame relative to inertial system the code-disc being installed on pitching frame is utilized to measure θ _f, utilize the code-disc being installed on roll framework to measure θ _r, for roll frame and pitching frame, time exsiccation coupling being regarded as system is disturbed, and introduces exponentially approaching rule simultaneously, designs the Nonlinear Decoupling control method based on sliding moding structure.For orientation frame, in the process controlled, introduce after judging that the factor judges, re-use based on the Nonlinear Decoupling control of sliding moding structure or the control method based on PID.

The present invention's advantage is compared with prior art: the present invention, owing to estimating in real time coupling torque between framework and compensating, overcomes the deficiency that PID controls, platform stable precision is improved; Overcome the problem of parameter uncertainty in feedforward compensation method simultaneously.Compensation principle of the present invention is distinct, and backoff algorithm is succinct, is easy to programming realization in dsp;

The above embodiment only have expressed several embodiment of the present invention, and it describes comparatively concrete and detailed, but therefore can not be interpreted as the restriction to the scope of the claims of the present invention.It should be pointed out that for the person of ordinary skill of the art, without departing from the inventive concept of the premise, can also make some distortion and improvement, these all belong to protection scope of the present invention.Therefore, the protection domain of patent of the present invention should be as the criterion with claims.

Claims (7)

1. an Inertially-stabilizeplatform platform control method, is characterized in that, comprises step:

Under quiet pedestal, the roll frame obtaining inertially stabilized platform is relative to the first rotational angular velocity of pedestal and pitching frame the second rotational angle relative to described roll frame;

When the sine value of described second rotational angle and the absolute value of described first rotational angular velocity product are less than predetermined threshold value, obtain second rotational angular velocity of pitching frame relative to described roll frame of described inertially stabilized platform, orientation frame relative to the 3rd rotational angular velocity of described pitching frame;

Euler dynamical equations is utilized to set up the Platform dynamics equation of stable inertia under quiet pedestal;

According to the input voltage of described first rotational angular velocity, the second rotational angular velocity, the 3rd rotational angular velocity, the linear switching function prestored, the exponentially approaching rule prestored and described Platform dynamics equation computer azimuth frame, the input voltage of pitching frame and the input voltage of roll frame;

Input voltage according to the input voltage of described orientation frame, the input voltage of pitching frame and roll frame controls orientation frame, pitching frame, roll frame.

2. Inertially-stabilizeplatform platform control method according to claim 1, is characterized in that, described Platform dynamics equation comprises:

Orientation frame around the kinetics equation of azimuth axis is:

$J_{az}{\stackrel{\·\·}{\mathrm{\θ}}}_a-J_{az}{\stackrel{\·}{\mathrm{\θ}}}_f{\stackrel{\·}{\mathrm{\θ}}}_r-J_{az}{\mathrm{\θ}}_f{\stackrel{\·\·}{\mathrm{\θ}}}_r=M_z,$ Wherein, $M_z=-\frac{{\left(K_{Ma}{i}_a\right)}^2}{R_a}{\stackrel{\·}{\mathrm{\θ}}}_a+\frac{K_{Ma}{i}_a}{R_a}{u}_a$

Pitching frame assembly around the kinetics equation of pitch axis is:

$(J_{fx}+J_{ax}){\stackrel{\·\·}{\mathrm{\θ}}}_f+J_{az}{\stackrel{\·}{\mathrm{\θ}}}_a{\stackrel{\·}{\mathrm{\θ}}}_r-(J_{fz}+J_{az}){\mathrm{\θ}}_f{{\stackrel{\·}{\mathrm{\θ}}}_r}^2=M_x,$

Wherein, $M_x=-\frac{{\left(K_{Mf}{i}_f\right)}^2}{R_f}{\stackrel{\·}{\mathrm{\θ}}}_f+\frac{K_{Mf}{i}_f}{R_f}{u}_f$

Roll frame assembly around roll direction of principal axis kinetics equation is:

$(J_{ry}+J_{fy}+J_{ay}){\stackrel{\·\·}{\mathrm{\θ}}}_r-J_{az}{\mathrm{\θ}}_f{\stackrel{\·\·}{\mathrm{\θ}}}_a-J_{az}{\stackrel{\·}{\mathrm{\θ}}}_f{\stackrel{\·}{\mathrm{\θ}}}_a=M_y,$ Wherein, $M_y=-\frac{{\left(K_{Mr}{i}_r\right)}^2}{R_r}{\stackrel{\·}{\mathrm{\θ}}}_r+\frac{K_{Mr}{i}_r}{R_r}{u}_r$

Wherein, J _ax, J _ay, J _azrepresent the orientation framework that the prestores moment of inertia at x, y, z axle respectively; J _fx, J _fy, J _fzrepresent the pitching frame that the prestores moment of inertia at x, y, z axle respectively; J _rythe roll framework that expression prestores is at the moment of inertia of y-axis;

represent three rotational angular velocity of orientation frame relative to pitching frame; represent the three angle of rotation acceleration of orientation frame relative to described pitching frame;

θ _frepresent second rotational angle of pitching frame relative to roll frame, represent second rotational angular velocity of pitching frame relative to roll frame; represent the second angle of rotation acceleration of pitching frame relative to roll frame;

represent first rotational angular velocity of roll frame relative to pedestal; represent the first angle of rotation acceleration of roll frame relative to pedestal;

K _ma, K _mfand K _mrrepresent the motor torque coefficient of the motor torque coefficient of the orientation frame prestored, the motor torque coefficient of pitching frame and roll frame respectively;

R _a, R _fand R _rrepresent the armature resistance of the armature resistance of the orientation frame prestored, the armature resistance of pitching frame and roll frame respectively;

I _a, i _fand i _rrepresent the ratio of gear of the ratio of gear of the orientation frame prestored, the ratio of gear of pitching frame and roll frame respectively;

U _a, u _fand u _rrepresent the input voltage of orientation frame, the input voltage of pitching frame and the input voltage of roll frame motor respectively;

M _xrepresent the comprehensive moment that pitching frame is subject to along x-axis, M _yrepresent the comprehensive moment that roll frame is subject to along y-axis, M _zrepresent the comprehensive moment that orientation frame is subject to along z-axis.

3. Inertially-stabilizeplatform platform control method according to claim 1 and 2, is characterized in that, adopts formula:

$u_a=\frac{J_{az}{R}_z}{K_{Ma}{i}_a}\left(\left(c_a-\frac{{\left(K_{Ma}{i}_a\right)}^2}{J_{az}{R}_a}\right)\left(-{\stackrel{\·}{\mathrm{\θ}}}_a\right)+{\mathrm{\ϵ}}_{a}sign\left(s_a\right)+k_a{s}_a-\frac{M_{ad\max }}{J_{az}}\right),$

Calculate the input voltage of described orientation frame;

Adopt formula:

$u_f=\frac{\left(J_{fx}+J_{ax}\right)R_f}{K_{Mf}{i}_f}\left(\left(c_f-\frac{{\left(K_{Mf}{i}_f\right)}^2}{\left(J_{fx}+J_{ax}\right)R_f}\right)\left(-{\stackrel{\·}{\mathrm{\θ}}}_f\right)+{\mathrm{\ϵ}}_{f}sign\left(s_f\right)+k_f{s}_f-\frac{M_{fd\max }}{\left(J_{fx}+J_{ax}\right)}\right),$

Calculate the input voltage of described pitching frame;

Adopt formula:

$\begin{array}{c}{u}_r=\frac{\left(J_{ry}+J_{fy}+J_{ay}\right)R_r}{K_{Mr}{i}_r}(\left(c_r-\frac{{\left(K_{Mr}{i}_r\right)}^2}{\left(J_{ry}+J_{fy}+J_{ay}\right)R_r}\right)\left(-{\stackrel{\·}{\mathrm{\θ}}}_r\right)+{\mathrm{\ϵ}}_{r}sign\left(s_r\right)\\ +k_r{s}_r-\frac{M_{rd\max }}{\left(J_{ry}+J_{fy}+J_{ay}\right)})\end{array},$

Calculate the input voltage of described roll frame;

Wherein, the linear switching function prestored described in is: j=a, f, r

The described exponentially approaching rule prestored is: ε > 0, k > 0, j=a, f, r, c _j, ε _j, k _jrepresent the coefficient prestored respectively,

Wherein, J _ax, J _ay, J _azrepresent the orientation framework that the prestores moment of inertia at x, y, z axle respectively; J _fx, J _fy, J _fzrepresent the pitching frame that the prestores moment of inertia at x, y, z axle respectively; J _rythe roll framework that expression prestores is at the moment of inertia of y-axis;

represent three rotational angular velocity of orientation frame relative to pitching frame, represent second rotational angular velocity of pitching frame relative to roll frame, represent first rotational angular velocity of roll frame relative to pedestal;

K _ma, K _mfand K _mrrepresent the motor torque coefficient of the motor torque coefficient of the orientation frame prestored, the motor torque coefficient of pitching frame and roll frame respectively;

R _a, R _fand R _rrepresent the armature resistance of the armature resistance of the orientation frame prestored, the armature resistance of pitching frame and roll frame respectively;

I _a, i _fand i _rrepresent the ratio of gear of the ratio of gear of the orientation frame prestored, the ratio of gear of pitching frame and roll frame respectively;

U _a, u _fand u _rrepresent the input voltage of orientation frame, the input voltage of pitching frame and the input voltage of roll frame motor respectively;

M _admaxrepresent the maximum coupling torque of the orientation frame prestored, M _fdmaxrepresent the maximum coupling torque of the pitching frame prestored, M _rdmaxrepresent the maximum coupling torque of the roll frame prestored.

4. Inertially-stabilizeplatform platform control method according to claim 3, it is characterized in that, before the described input voltage according to described first rotational angular velocity, the second rotational angular velocity, the 3rd rotational angular velocity, the linear switching function prestored, the exponentially approaching rule prestored and described Platform dynamics equation computer azimuth frame, the input voltage of pitching frame and the input voltage step of roll frame, also comprise step:

Adopt formula $M_{ad\max }=J_{az}{\stackrel{\·}{\mathrm{\θ}}}_{f\max }{\stackrel{\·}{\mathrm{\θ}}}_{r\max }+J_{az}{\mathrm{\θ}}_{fmax}{\stackrel{\·\·}{\mathrm{\θ}}}_{rmax}$ Determine the maximum coupling torque of orientation frame,

Adopt formula determine the maximum coupling torque of pitching frame,

Adopt formula $M_{rdmax}=J_{az}{\mathrm{\θ}}_{fmax}{\stackrel{\·\·}{\mathrm{\θ}}}_{amax}+J_{az}{\stackrel{\·}{\mathrm{\θ}}}_{fmax}{\stackrel{\·}{\mathrm{\θ}}}_{amax}$ Determine the maximum coupling torque of roll frame,

represent the 3rd maximum rotational angular velocity, represent the 3rd maximum angle of rotation acceleration, θ _fmaxrepresent the second maximum rotational angle, represent the second maximum rotational angular velocity, represent the first maximum rotational angular velocity, represent the first maximum angle of rotation acceleration.

5. Inertially-stabilizeplatform platform control method according to claim 1 and 2, it is characterized in that, when the sine value of described second rotational angle and the product of described first rotational angular velocity are more than or equal to predetermined threshold value, pid control algorithm is adopted to control orientation frame.

6. Inertially-stabilizeplatform platform control method according to claim 3, is characterized in that, when the sine value of described second rotational angle and the product of described first rotational angular velocity are more than or equal to predetermined threshold value, adopts pid control algorithm to control orientation frame.

7. Inertially-stabilizeplatform platform control method according to claim 1 and 2, it is characterized in that, described under quiet pedestal, the roll frame obtaining inertially stabilized platform relative in the second rotational angle step of described roll frame relative to the first rotational angular velocity of pedestal and pitching frame, obtains described first rotational angular velocity and comprises step:

Under quiet pedestal, obtain the rotational angular velocity of roll frame relative to inertial system from the roll gyro the pitching frame of inertially stabilized platform;

Ratio according to the cosine value of described rotational angular velocity and described second rotational angle obtains described first rotational angular velocity.