CN103425146B - A kind of inertially stabilized platform interference observer method for designing based on angular acceleration - Google Patents

Abstract

A kind of inertially stabilized platform interference observer method for designing based on angular acceleration, based on traditional three ring PID multiplex control system structures, according to current sensor, the current information that each framework rate gyro is measured in real time and each frame corners velocity information, estimate the disturbance torque suffered by each framework of stable platform, then the torque motor current value of offsetting needed for this disturbance torque is calculated, and this current value is compensated in the given value of current input value of electric current loop, motor is exported and disturbance torque equal and opposite in direction, the moment that direction is contrary, compensating platform disturbance torque disturbs, improve platform stable precision.Present invention achieves real-time estimation and the compensation of disturbance torque, improve lasting accuracy, be applicable to the aerial remote sensing inertial-stabilized platform with disturbance torque.

Description

A kind of inertially stabilized platform interference observer method for designing based on angular acceleration

Technical field

The present invention relates to a kind of inertially stabilized platform interference observer method for designing based on angular acceleration, can be used for the compensation of the airborne remote sensing inertially stabilized platform disturbance torque of various middle and high precision, be specially adapted to the aerial remote sensing inertial-stabilized platform with larger mass eccentricity.

Background technology

Airborne remote sensing system is the effective means obtaining At High Resolution remote sensing images, has vital role in fields such as base surveying, disaster monitoring, resources and environment investigation and meteorological model detections.Aerial remote sensing inertial-stabilized platform is arranged between flight carrier and remote sensing load, carries and stablizes remote sensing load, is one of important component part of airborne remote sensing system.Use stable platform effectively can isolate the imperfect attitude motion of flight carrier and inner various disturbance thereof to the impact of the remote sensing load optical axis, thus make remote sensing load attitude relative inertness space keep stable.After using stable platform, the degree of overlapping before and after remote sensing load and between adjacent two two field pictures significantly improves, and meets into figure requirement, can significantly improve the work efficiency of airborne remote sensing.Lasting accuracy is one of the key technical indexes of inertially stabilized platform, reflects the rejection ability of stable platform to disturbance torque.

Three axle inertially stabilized platforms can carry multiple load, the centroid position of different loads is different, and barycenter can offset in the course of the work, add because machining causes the uncertain of the centroid position of platform own, cause the mode be difficult to by counterweight to overcome mass eccentricity completely.Due to the existence of mass eccentricity, and the imaging quality of loads of platform bearer is comparatively large, under the effect of acceleration of gravity and aircraft disturbing acceleration, can produce larger unbalanced moments.Except mass unbalance torque, platform is also subject to the impact of other disturbance torques such as moment of friction, framework coupling torque, causes platform stable precise decreasing, causes imaging load image quality to decline, and reduces airborne remote sensing operating efficiency.Therefore, when carrying out Control System of Stable Platform design, must compensate stable platform disturbance torque, improving control accuracy and the dynamic property of stable platform.In prior art, usually FEEDBACK CONTROL in three ring PID multiplex control systems is utilized to compensate the interference in system, but this compensation method increases rejection ability to disturbance by increasing pid parameter, and increase pid parameter and system stability can be caused to decline even cause the instability of system, if interference be uncertain or time become, the compensation effect of regulatory PID control can be very undesirable, interference observer can be real-time to the disturbance torque suffered by system observation out, can by disturbance torque full remuneration by feedforward control, but when utilizing interference observer to carry out disturbance torque observation, usually the differential of information of asking for is needed, adopting traditional differential mode to ask for differential can cause noise signal to be amplified, the delayed phase utilizing Kalman filtering to ask for differential signal to cause differential signal, and utilize Tracking differentiator to ask for differential in the starting stage of differential signal to have larger evaluated error, the inaccurate meeting asked for due to differential signal causes the bad system that even causes of disturbance torque compensation effect unstable.

Summary of the invention

The technical issues that need to address of the present invention are: overcome conventional three ring PID complex controll and common interference observer to the defect of platform disturbance torque rejection ability deficiency, there is provided a kind of inertially stabilized platform interference observer method for designing based on angular acceleration, in order to improve system stability precision.

Technical solution of the present invention is: a kind of inertially stabilized platform interference observer method for designing based on angular acceleration, based on traditional three ring PID multiplex control system structures, according to current sensor, each framework rate gyro measured value estimation disturbance torque value, further calculation compensation current value, specifically comprises the following steps:

(1) set up airborne remote sensing three axle inertially stabilized platform three ring PID multiplex control system, be divided into following three steps:

1. electric current loop PID backfeed loop is set up;

The output in stabilizing ring loop is as the given value of current value I of current loop _set, the current sensor output framework motor current value in power of motor driving circuit is as the current feedback values I of current loop _fb, the two error amount is as the input of electric current loop PID controller, and electric current loop PID controller output valve makes frame motor produce corresponding current value as input framework electric moter voltage value.

2. stabilizing ring PID backfeed loop is set up;

The output in tracking loop loop is as the given ω of angular velocity in stabilizing ring loop _set, each frame corners speed feedback value ω measured by the rate gyro be installed on each framework _fb, the two error amount is as the input of stabilizing ring PID controller, and stabilizing ring PID controller output valve is as electric current loop given value of current value I _set, driver framework rotates with given angular velocity.

3. tracking loop PID backfeed loop is set up;

Two kinds of patterns are divided in tracking loop loop, utonomous working pattern and with POS work in combination pattern.Utonomous working pattern is using accelerometer as measuring sensor measuring position value of feedback θ _fb, the attitude information exported using POS with POS work in combination pattern is as location feedback value θ _fb, utonomous working mode position setting value is 0, even if platform keeps local level, with POS work in combination pattern by host computer desired location setting value θ _set, setting value and the error amount both value of feedback are as the input of tracking loop PID controller, and tracking loop PID controller output valve is as stabilizing ring angular velocity set-point, and driver framework turns to the angle of specifying.

(2) the current sensor measurement roll frame motor current value I on each frame motor drive circuit board is utilized _x, pitching frame motor current value I _ywith orientation frame motor current value I _z; The rate gyro be installed on each framework is utilized to measure roll frame corners speed omega _x, pitching frame angular velocity omega _ywith orientation frame corners speed omega _z;

(3) according to step (2) medium-rate gyro to measure value-roll frame corners speed omega _x, pitching frame angular velocity omega _ywith orientation frame corners speed omega _z, utilize Kalman filtering and Tracking differentiator to estimate the angular acceleration information-roll frame corners acceleration of each framework pitching frame angular acceleration with orientation frame corners acceleration

(4) according to measured value I direct in step (2), step (3) _x, I _y, I _zwith indirect calculation value calculate roll frame disturbance torque value M _{d_x}, pitching frame disturbance torque value M _{d_y}and orientation frame disturbance torque value M _{d_z};

(5) according to each framework disturbance torque value M calculated in step (4) _{d_x}, M _{d_y}and M _{d_z}, calculate the torque motor current value I offset needed for each framework disturbance torque further _{com_x}, I _{com_y}and I _{com_z}, and compensate in the given value of current input value of electric current loop by the current value calculated, motor exports the moment contrary with disturbance torque equal and opposite in direction, direction, compensation inertially stabilized platform disturbance torque.

Kalman filtering and Tracking differentiator is utilized to estimate the angular acceleration information of each framework according to framework rate gyro measured value each in step (2) in described step (3), specific as follows:

(31) the angular acceleration information of each framework is estimated according to Kalman filtering, as follows:

1. the Kalman filtering equation of motion and observation equation is set up:

$\left.\begin{array}{c}{x}_{k+1}={\mathrm{Ax}}_k+w_k\\ y_k={\mathrm{Hx}}_k+v_k\end{array}\right\}$

2. according to the equation of motion set up and observation equation, Kalman filter equation is set up:

$\left.\begin{array}{c}{\hat{x}}_{k\backslash k-1}=A{\hat{x}}_{k-1}\\ {\hat{x}}_k={\hat{x}}_{k/k-1}+K_k(y_k-H{\hat{x}}_{k/k-1})\\ K_k=P_{k/k-1}{H}^T{({\mathrm{HP}}_{k/k-1}{H}^T+R)}^{-1}\\ P_{k/k-1}={\mathrm{AP}}_{k-1}{A}^T+Q\\ P_k=(I-K_{k}H)P_{k/k-1}\end{array}\right\}$

Wherein define: x _kfor the n in a kth sampling period ties up state vector; y _kfor the one dimension measuring value in a kth sampling period; A, H are that n × n, 1 × n maintain matrix number; w _k, v _kbe respectively system noise and measurement noise, separate between them, and the w of different value of K _kand v _kalso separate; for the one-step prediction of system state is estimated; for laststate optimal estimation; P _k/k-1for corresponding prior uncertainty; P _kfor corresponding posteriori error; Q is system noise variance matrix; R is measuring noise square difference battle array; K _kfor Kalman filtering gain.

3. according to the angular acceleration information that angular velocity information estimation needs, Kalman filter equation parameter is set, as follows:

$\begin{array}{ccc}{x}_k=\left[\begin{array}{c}{\mathrm{\ω}}_k\\ {\mathrm{\β}}_k\end{array}\right]& A=\left[\begin{array}{cc}1& T\\ 0& 1\end{array}\right]& H=\left[\begin{array}{cc}1& 0\end{array}\right]\end{array}$

Wherein, ω _kfor the angular velocity that gyro records; β _kfor the angular acceleration that will estimate; T is the sampling period.

(32) the angular acceleration information of each framework is estimated according to Tracking differentiator, as follows:

1. Second-Order Discrete Tracking differentiator equation is set up, as follows:

$\left.\begin{array}{c}{\hat{x}}_1(k+1)={\hat{x}}_1\left(k\right)+T{\hat{x}}_2\left(k\right)\\ {\hat{x}}_2(k+1)={\hat{x}}_2\left(k\right)+Tfst\left(\mathrm{\ϵ}\right(k),{\hat{x}}_2(k),M,h)\end{array}\right\}$

Wherein, T is the sampling time; for the evaluated error of input variable r (k), M is velocity factor, major effect tracking velocity; H is filtering factor, major effect filter effect, x ₁(k), x ₂k () is output, wherein x ₁k () follows the tracks of input r (k), x ₂k () is " approximate differential " of input r (k), nonlinear function fst (v1, v2, M, h) is defined as follows:

$fst(v_1,v_2,M,h)=-\left\{\begin{array}{cc}Ma/d,& \left|a\right|\≤d\\ Msign\left(a\right),& \left|a\right|>d\end{array}\right.$

In formula, sign () is sign function; A and d is defined as follows:

$a=\left\{\begin{array}{cc}{v}_2+y/h,& \left|y\right|\≤d_0\\ v_2+\frac{a_0-d}{2}sign\left(y\right),& \left|y\right|>d_0\end{array}\right.$

Wherein,

$\left\{\begin{array}{c}d=Mh\\ d_0=dh\\ y=v_1+h{v}_2\\ a_0=\sqrt{d^2+8M\left|y\right|}\end{array}\right.$

2. input variable r (k) is set to angular velocity omega _k, then output variable x ₁(k), x ₂k () is respectively input angular velocity ω _ktracking signal and differential signal, wherein x ₂(k) angular acceleration information for estimating;

(33) the angular acceleration information of each framework that each frame corners acceleration information estimated according to step (31) and step (32) estimate is through merging the angular acceleration information obtaining more desirable each framework, as follows:

The judging quota function that error between the angular acceleration values selecting the angular acceleration values of Kalman Filter Estimation and Tracking differentiator to estimate is constructed as follows:

$J={\mathrm{\α\ϵ}}^2\left(t\right)+\mathrm{\β}{\∫}_0^t{e}^{-\mathrm{\λ}(t-\mathrm{\τ})}{\mathrm{\ϵ}}^2\left(\mathrm{\τ}\right)d\mathrm{\τ}$

In formula, α >=0, β >0, λ >0, alpha+beta=1, error between the angular acceleration that ε (t) is Kalman Filter Estimation and the angular acceleration that Tracking differentiator is estimated, α represents the proportion that transient error is shared in judging quota function, β represents the proportion that mistake error is shared in judging quota function, λ is forgetting factor, determine the length of memory, according to the angular acceleration values that angular acceleration and the Tracking differentiator of Kalman Filter Estimation are estimated, choose reasonable parameter alpha and β, trade off between transient error and history error, and then obtain the estimation of better angular acceleration.

According to measured value I direct in step (2), step (3) in described step (4) _x, I _y, I _zwith indirect calculation value calculate roll frame disturbance torque value M _{d_x}, pitching frame disturbance torque value M _{d_y}and orientation frame disturbance torque value M _{d_z}, specific as follows:

$M_{d\_x}=J_{n\_x}{\stackrel{\·}{\mathrm{\ω}}}_x-C_{mn\_x}{K}_{gr\_x}{I}_x;$

$M_{d\_y}=J_{n\_y}{\stackrel{\·}{\mathrm{\ω}}}_y-C_{mn\_y}{K}_{gr\_y}{I}_y;$

$M_{d\_z}=J_{n\_z}{\stackrel{\·}{\mathrm{\ω}}}_z-C_{mn\_z}{K}_{gr\_z}{I}_z;$

Wherein define: J _{n_x}, J _{n_y}and J _{n_z}be respectively roll frame, pitching frame and orientation frame moment of inertia nominal value; C _{mn_x}, C _{mn_y}and C _{mn_z}be respectively roll frame, pitching frame and orientation frame motor torque system nominal value; K _{gr_x}, K _{gr_y}and K _{gr_z}be respectively roll frame, pitching frame and orientation frame motor-driven coefficient.

Offset the torque motor current value needed for disturbance torque in described step (5), step is as follows:

(51) the torque motor current value of offsetting needed for this disturbance torque is calculated according to the disturbance torque value of estimation, as follows:

1. roll frame disturbance torque M is offset _{d_x}required torque motor current value

2. pitching frame disturbance torque M is offset _{d_y}required torque motor current value

3. orientation frame disturbance torque M is offset _{d_z}required torque motor current value

Wherein, C _{mn_x}, C _{mn_y}and C _{mn_z}be respectively roll frame, pitching frame and orientation frame motor torque system nominal value; K _{gr_x}, K _{gr_y}and K _{gr_z}be respectively roll frame, pitching frame and orientation frame motor-driven coefficient.

(52) by current value I in step (51) _{com_x}, I _{com_y}, I _{com_z}respectively feedforward compensation is in the given value of current input value of roll frame, pitching frame and orientation frame control system electric current loop, and motor exports the moment contrary with disturbance torque equal and opposite in direction, direction, compensation stable platform disturbance torque.

Principle of the present invention is: set up traditional three ring PID multiplex control systems for airborne remote sensing three axle inertially stabilized platform;

The relation of disturbance torque suffered by each framework and stable platform current of electric and frame corners acceleration is drawn based on interference observer structure.The current sensor on drive circuit board is utilized to record each frame motor electric current I _x, I _yand I _z; The rate gyro be arranged on each framework is utilized to record the angular velocity information ω of each framework _x, ω _yand ω _z; According to the frame corners velocity information that rate gyro records, comprehensive utilization Kalman filtering and the angular acceleration information of Tracking differentiator to each framework are estimated.Roll frame disturbance torque value M is calculated according to above directly measured value or indirect calculation value _{d_x}, pitching frame disturbance torque value M _{d_y}and orientation frame disturbance torque value M _{d_z}; Calculate further and offset roll frame disturbance torque M _{d_x}required torque motor current value I _{com_x}, offset pitching frame disturbance torque M _{d_y}required torque motor current value I _{com_y}, offset orientation frame disturbance torque M _{d_z}required torque motor current value I _{com_z}, and compensate in the given value of current input value of electric current loop by the current value calculated, motor exports the moment contrary with disturbance torque equal and opposite in direction, direction, compensating platform disturbance torque;

The present invention's advantage is compared with prior art:

(1) the present invention is owing to estimating in real time the disturbance torque of stable platform and compensating, and overcomes the deficiency that conventional three ring PID control, platform stable precision is improved;

(2) compensation principle of the present invention is distinct, and backoff algorithm is succinct, is easy to programming realization in dsp;

(3) the present invention does not need extra sensor, is realized the compensation of disturbance torque by the improvement of control algolithm, has structure simple, is convenient to the feature of Project Realization.

Accompanying drawing explanation

Fig. 1 is the disturbance torque compensating control method process flow diagram in the present invention;

Fig. 2 is that utilize Kalman filtering and the Tracking differentiator in the present invention estimates angular acceleration schematic diagram;

Fig. 3 is the interference observer structural drawing estimated based on angular acceleration in the present invention.

Embodiment

As shown in Figure 1, Figure 2 and Figure 3, specific embodiment of the invention method is as follows:

(1) set up airborne remote sensing three axle inertially stabilized platform three ring PID multiplex control system, step is as follows:

(11) electric current loop PID backfeed loop is set up;

The output in stabilizing ring loop is as the given value of current value I of current loop _set, the current sensor output framework motor current value in power of motor driving circuit is as the current feedback values I of current loop _fb, the two error amount is as the input of electric current loop PID controller, and electric current loop PID controller output valve makes frame motor produce corresponding current value as input framework electric moter voltage value.

(12) stabilizing ring PID backfeed loop is set up;

The output in tracking loop loop is as the given ω of angular velocity in stabilizing ring loop _set, each frame corners speed feedback value ω measured by the rate gyro be installed on each framework _fb, the two error amount is as the input of stabilizing ring PID controller, and stabilizing ring PID controller output valve is as electric current loop given value of current value I _set, driver framework rotates with given angular velocity.

(13) tracking loop PID backfeed loop is set up;

Two kinds of patterns are divided in tracking loop loop, utonomous working pattern and with POS work in combination pattern.Utonomous working pattern is using accelerometer as measuring sensor measuring position value of feedback θ _fb, the attitude information exported using POS with POS work in combination pattern is as location feedback value θ _fb, utonomous working mode position setting value is 0, even if platform keeps local level, with POS work in combination pattern by host computer desired location setting value θ _set, setting value and the error amount both value of feedback are as the input of tracking loop PID controller, and tracking loop PID controller output valve is as stabilizing ring angular velocity set-point, and driver framework turns to the angle of specifying.

(2) current sensor, each framework rate gyro sensor information obtaining step are as follows:

(21) each frame motor electric current of the current sensor measurement on drive circuit board roll frame current of electric I _x, pitching frame current of electric I _ywith orientation frame current of electric I _z;

(22) the angular velocity information roll frame corners speed omega of each framework measured by the rate gyro being arranged on each framework _x, pitching frame angular velocity omega _ywith orientation frame corners speed omega _z;

(3) as shown in Figure 2, according to the angular velocity information ω of each framework that rate gyro is measured, Kalman filtering is utilized to estimate the angular acceleration information of framework and utilize Tracking differentiator to estimate the angular acceleration information of framework then merge through judgment criteria, obtain angular acceleration information more accurately, concrete steps are as follows:

(31) the angular acceleration information of each framework is estimated according to Kalman filtering, as follows:

1. the Kalman filtering equation of motion and observation equation is set up:

$\left.\begin{array}{c}{x}_{k+1}={\mathrm{Ax}}_k+w_k\\ y_k={\mathrm{Hx}}_k+v_k\end{array}\right\}$

2. according to the equation of motion set up and observation equation, Kalman filter equation is set up:

$\left.\begin{array}{c}{\hat{x}}_{k\backslash k-1}=A{\hat{x}}_{k-1}\\ {\hat{x}}_k={\hat{x}}_{k/k-1}+K_k(y_k-H{\hat{x}}_{k/k-1})\\ K_k=P_{k/k-1}{H}^T{({\mathrm{HP}}_{k/k-1}{H}^T+R)}^{-1}\\ P_{k/k-1}={\mathrm{AP}}_{k-1}{A}^T+Q\\ P_k=(I-K_{k}H)P_{k/k-1}\end{array}\right\}$

Wherein define: x _kfor the n in a kth sampling period ties up state vector; y _kfor the one dimension measuring value in a kth sampling period; A, H are that n × n, 1 × n maintain matrix number; w _k, v _kbe respectively system noise and measurement noise, separate between them, and the w of different value of K _kand v _kalso separate; for the one-step prediction of system state is estimated; for laststate optimal estimation; P _k/k-1for corresponding prior uncertainty; P _kfor corresponding posteriori error; Q is system noise variance matrix; R is measuring noise square difference battle array; K _kfor Kalman filtering gain.

3. according to the angular acceleration information that angular velocity information estimation needs, Kalman filter equation parameter is set, as follows:

$\begin{array}{ccc}{x}_k=\left[\begin{array}{c}{\mathrm{\ω}}_k\\ {\mathrm{\β}}_k\end{array}\right]& A=\left[\begin{array}{cc}1& T\\ 0& 1\end{array}\right]& H=\left[\begin{array}{cc}1& 0\end{array}\right]\end{array}$

Wherein, ω _kfor the angular velocity that gyro records; β _kfor the angular acceleration that will estimate; T is the sampling period.

(32) the angular acceleration information of each framework is estimated according to Tracking differentiator, as follows:

1. Second-Order Discrete Tracking differentiator equation is set up, as follows:

$\left.\begin{array}{c}{\hat{x}}_1(k+1)={\hat{x}}_1\left(k\right)+T{\hat{x}}_2\left(k\right)\\ {\hat{x}}_2(k+1)={\hat{x}}_2\left(k\right)+Tfst\left(\mathrm{\ϵ}\right(k),{\hat{x}}_2(k),M,h)\end{array}\right\}$

Wherein, T is the sampling time; for the evaluated error of input variable r (k), M is velocity factor, major effect tracking velocity; H is filtering factor, major effect filter effect, x ₁(k), x ₂k () is output, wherein x ₁k () follows the tracks of input r (k), x ₂k () is " approximate differential " of input r (k), nonlinear function fst (v1, v2, M, h) is defined as follows:

$fst(v_1,v_2,M,h)=-\left\{\begin{array}{cc}Ma/d,& \left|a\right|\≤d\\ Msign\left(a\right),& \left|a\right|>d\end{array}\right.$

In formula, sign () is sign function; A and d is defined as follows:

$a=\left\{\begin{array}{cc}{v}_2+y/h,& \left|y\right|\≤d_0\\ v_2+\frac{a_0-d}{2}sign\left(y\right),& \left|y\right|>d_0\end{array}\right.$

Wherein,

$\left\{\begin{array}{c}d=Mh\\ d_0=dh\\ y=v_1+h{v}_2\\ a_0=\sqrt{d^2+8M\left|y\right|}\end{array}\right.$

2. input variable r (k) is set to angular velocity omega _k, then output variable x ₁(k), x ₂k () is respectively input angular velocity ω _ktracking signal and differential signal, wherein x ₂(k) angular acceleration information for estimating.

(33) the angular acceleration information of each framework obtained according to step (31), step (32) is through merging the angular acceleration information obtaining more desirable each framework, as follows:

The judging quota function that error between the angular acceleration values selecting the angular acceleration values of Kalman Filter Estimation and Tracking differentiator to estimate is constructed as follows:

$J={\mathrm{\α\ϵ}}^2\left(t\right)+\mathrm{\β}{\∫}_0^t{e}^{-\mathrm{\λ}(t-\mathrm{\τ})}{\mathrm{\ϵ}}^2\left(\mathrm{\τ}\right)d\mathrm{\τ}$

In formula, α >=0, β >0, λ >0, alpha+beta=1, error between the angular acceleration that ε (t) is Kalman Filter Estimation and the angular acceleration that Tracking differentiator is estimated, α represents the proportion that transient error is shared in judging quota function, β represents the proportion that mistake error is shared in judging quota function, λ is forgetting factor, determine the length of memory, according to the angular acceleration values that angular acceleration and the Tracking differentiator of Kalman Filter Estimation are estimated, choose reasonable parameter alpha and β, trade off between transient error and history error, and then obtain the estimation of better angular acceleration.When transient error is larger to judging quota function influences, less α should be selected, when mistake error is larger to judging quota function influences, less β should be selected, by experiment, select α=0.324, β=0.676, the angular acceleration now obtained is estimated ideal.

(4) as shown in Figure 3, the interference observer structure estimated based on angular acceleration is on the basis of traditional three ring PID multiplex control systems, exports ω by angular velocity _outand electric current exports I _out, by certain frame motor current value I calculated needed for compensate for disturbances moment _com, this current value is compensated to electric current loop input end and frame motor can be made to produce the moment contrary with disturbance torque equal and opposite in direction, direction, compensate for disturbances moment.According to measured value I direct in step (2), step (3) _x, I _y, I _zwith indirect calculation value calculate roll frame disturbance torque value M _{d_x}, pitching frame disturbance torque value M _{d_y}and orientation frame disturbance torque value M _{d_z};

$M_{d\_x}=J_{n\_x}{\stackrel{\·}{\mathrm{\ω}}}_x-C_{mn\_x}{K}_{gr\_x}{I}_x;$

$M_{d\_y}=J_{n\_y}{\stackrel{\·}{\mathrm{\ω}}}_y-C_{mn\_y}{K}_{gr\_y}{I}_y;$

$M_{d\_z}=J_{n\_z}{\stackrel{\·}{\mathrm{\ω}}}_z-C_{mn\_z}{K}_{gr\_z}{I}_z;$

Wherein define: J _{n_x}, J _{n_y}and J _{n_z}be respectively roll frame, pitching frame and orientation frame moment of inertia nominal value; C _{mn_x}, C _{mn_y}and C _{mn_z}be respectively roll frame, pitching frame and orientation frame motor torque system nominal value; K _{gr_x}, K _{gr_y}and K _{gr_z}be respectively roll frame, pitching frame and orientation frame motor-driven coefficient.

(5) as shown in Figure 3, according to the disturbance torque value that each framework calculated is subject to, the torque motor current value step offset in (4) needed for each framework disturbance torque is calculated as follows:

(51) the torque motor current value of offsetting needed for this disturbance torque is calculated according to the disturbance torque value of estimation, as follows:

1. roll frame disturbance torque M is offset _{d_x}required torque motor current value

2. pitching frame disturbance torque M is offset _{d_y}required torque motor current value

3. orientation frame disturbance torque M is offset _{d_z}required torque motor current value

Wherein, C _{mn_x}, C _{mn_y}and C _{mn_z}be respectively roll frame, pitching frame and orientation frame motor torque system nominal value; K _{gr_x}, K _{gr_y}and K _{gr_z}be respectively roll frame, pitching frame and orientation frame motor-driven coefficient.

(52) by current value I in (51) _{com_x}, I _{com_y}, I _{com_z}respectively feedforward compensation is in the given value of current input value of roll frame, pitching frame and orientation frame control system electric current loop, and motor exports the moment contrary with disturbance torque equal and opposite in direction, direction, compensation stable platform disturbance torque.

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

Claims (4)

1. the inertially stabilized platform interference observer method for designing based on angular acceleration, it is characterized in that: based on traditional three ring PID multiplex control system structures, each framework disturbance torque value is estimated according to current sensor, each framework rate gyro measured value, further calculation compensation current value, specifically comprises the following steps:

(1) set up airborne remote sensing three axle inertially stabilized platform three ring PID multiplex control system, be divided into following three steps:

1. electric current loop PID backfeed loop is set up;

The output in stabilizing ring loop is as the given value of current value I of current loop _set, the current sensor output framework motor current value in power of motor driving circuit is as the current feedback values I of current loop _fb, the two error amount is as the input of electric current loop PID controller, and electric current loop PID controller output valve makes frame motor produce corresponding current value as input framework electric moter voltage value;

2. stabilizing ring PID backfeed loop is set up;

The output in tracking loop loop is as the given ω of angular velocity in stabilizing ring loop _set, each frame corners speed feedback value ω measured by the rate gyro be installed on each framework _fb, the two error amount is as the input of stabilizing ring PID controller, and stabilizing ring PID controller output valve is as electric current loop given value of current value I _set, driver framework rotates with given angular velocity;

3. tracking loop PID backfeed loop is set up;

Two kinds of patterns are divided in tracking loop loop, utonomous working pattern and with POS work in combination pattern, utonomous working pattern is using accelerometer as measuring sensor measuring position value of feedback θ _fb, the attitude information exported using POS with POS work in combination pattern is as location feedback value θ _fb, utonomous working mode position setting value is 0, even if platform keeps local level, with POS work in combination pattern by host computer desired location setting value θ _set, setting value and the error amount both value of feedback are as the input of tracking loop PID controller, and tracking loop PID controller output valve is as stabilizing ring angular velocity set-point, and driver framework turns to the angle of specifying;

(2) the current sensor measurement roll frame motor current value I on each frame motor drive circuit board is utilized _x, pitching frame motor current value I _ywith orientation frame current of electric I _z; The rate gyro be installed on each framework is utilized to measure roll frame corners speed omega _x, pitching frame angular velocity omega _ywith orientation frame corners speed omega _z;

(3) according to step (2) medium-rate gyro to measure value-roll frame corners speed omega _x, pitching frame angular velocity omega _ywith orientation frame corners speed omega _z, utilize Kalman filtering and Tracking differentiator to estimate the angular acceleration information-roll frame corners acceleration of each framework pitching frame angular acceleration with orientation frame corners acceleration

(4) according to measured value I direct in step (2), step (3) _x, I _y, I _zwith indirect calculation value calculate roll frame disturbance torque value M _{d_x}, pitching frame disturbance torque value M _{d_y}and orientation frame disturbance torque value M _{d_z};

(5) according to each framework disturbance torque value M calculated in step (4) _{d_x}, M _{d_y}and M _{d_z}, calculate the torque motor current value I offset needed for each framework disturbance torque further _{com_x}, I _{com_y}and I _{com_z}, and compensate in the given value of current input value of electric current loop by the current value calculated, motor exports the moment contrary with disturbance torque equal and opposite in direction, direction, compensation inertially stabilized platform disturbance torque.

2. the inertially stabilized platform interference observer method for designing based on angular acceleration according to claim 1, it is characterized in that: utilize Kalman filtering and Tracking differentiator to estimate the angular acceleration information of each framework according to framework rate gyro measured value each in step (2) in described step (3), concrete grammar is as follows:

(1) the angular acceleration information of each framework is estimated according to Kalman filtering, as follows:

1. the Kalman filtering equation of motion and observation equation is set up:

$\left.\begin{array}{c}{x}_{k+1}={\mathrm{Ax}}_k+w_k\\ y_k={\mathrm{Hx}}_k+v_k\end{array}\right\}$

2. according to the equation of motion set up and observation equation, Kalman filter equation is set up:

$\left.\begin{array}{c}{\hat{x}}_{k\backslash k-1}=A{\hat{x}}_{k-1}\\ {\hat{x}}_k={\hat{x}}_{k/k-1}+K_k(y_k-H{\hat{x}}_{k/k-1})\\ K_k=P_{k/k-1}{H}^T{({\mathrm{HP}}_{k/k-1}{H}^T+R)}^{-1}\\ P_{k/k-1}={\mathrm{AP}}_{k-1}{A}^T+Q\\ P_k=(I-K_{k}H)P_{k/k-1}\end{array}\right\}$

Wherein define: x _kfor the n in a kth sampling period ties up state vector; y _kfor the 1 dimension measuring value in a kth sampling period; A, H are that n × n, 1 × n maintain matrix number; w _k, v _kbe respectively system noise and measurement noise, separate between them, and the w of different value of K _kand v _kalso separate; for the one-step prediction of system state is estimated; for laststate optimal estimation; P _k/k-1for corresponding prior uncertainty; P _kfor corresponding posteriori error; Q is system noise variance matrix; R is measuring noise square difference battle array; K _kfor Kalman filtering gain;

3. according to the angular acceleration information that angular velocity information estimation needs, Kalman filter equation parameter is set, as follows:

$\begin{array}{ccc}{x}_k=\left[\begin{array}{c}{\mathrm{\ω}}_k\\ {\mathrm{\β}}_k\end{array}\right]& A=\left[\begin{array}{cc}1& T\\ 0& 1\end{array}\right]& H=\left[\begin{array}{cc}1& 0\end{array}\right]\end{array}$

Wherein, ω _kfor the angular velocity that gyro records; β _kfor the angular acceleration that will estimate; T is the sampling period;

(2) the angular acceleration information of each framework is estimated according to Tracking differentiator, as follows:

1. Second-Order Discrete Tracking differentiator equation is set up, as follows:

$\left.\begin{array}{c}{\hat{x}}_1(k+1)={\hat{x}}_1\left(k\right)+T{\hat{x}}_2\left(k\right)\\ {\hat{x}}_2(k+1)={\hat{x}}_2\left(k\right)+Tfst\left(\mathrm{\ϵ}\right(k),{\hat{x}}_2(k),M,h)\end{array}\right\}$

Wherein, T is the sampling time; for the evaluated error of input variable r (k), M is velocity factor, major effect tracking velocity; H is filtering factor, major effect filter effect, x ₁(k), x ₂k () is output, wherein x ₁k () follows the tracks of input r (k), x ₂k () is " approximate differential " of input r (k), nonlinear function fst (v1, v2, M, h) is defined as follows:

$fst(v_1,v_2,M,h)=-\left\{\begin{array}{cc}Ma/d,& \left|a\right|\≤d\\ Msign\left(a\right),& \left|a\right|>d\end{array}\right.$

In formula, sign () is sign function; A and d is defined as follows:

$a=\left\{\begin{array}{cc}{v}_2+y/h,& \left|y\right|\≤d_0\\ v_2+\frac{a_0-d}{2}sign\left(y\right),& \left|y\right|>d_0\end{array}\right.$

Wherein,

$\left\{\begin{array}{c}d=Mh\\ d_0=dh\\ y=v_1+{\mathrm{hv}}_2\\ a_0=\sqrt{d^2+8M\left|y\right|}\end{array}\right.$

2. input variable r (k) is set to angular velocity omega _k, then output variable x ₁(k), x ₂k () is respectively input angular velocity ω _ktracking signal and differential signal, wherein x ₂(k) angular acceleration information for estimating;

(3) the angular acceleration information of each framework estimated according to each frame corners acceleration information of being estimated by step (1) and step (2), through merging the angular acceleration information obtaining more desirable each framework, method is as follows:

The judging quota function that error between the angular acceleration values selecting the angular acceleration values of Kalman Filter Estimation and Tracking differentiator to estimate is constructed as follows:

$J={\mathrm{\α\ϵ}}^2\left(t\right)+\mathrm{\β}{\∫}_0^t{e}^{-\mathrm{\λ}(t-\mathrm{\τ})}{\mathrm{\ϵ}}^2\left(\mathrm{\τ}\right)d\mathrm{\τ}$

In formula, α >=0, β > 0, λ > 0, alpha+beta=1, error between the angular acceleration that ε (t) is Kalman Filter Estimation and the angular acceleration that Tracking differentiator is estimated, α represents the proportion that transient error is shared in judging quota function, β represents the proportion that history error is shared in judging quota function, λ is forgetting factor, determine the length of memory, according to the angular acceleration values that angular acceleration and the Tracking differentiator of Kalman Filter Estimation are estimated, choose reasonable parameter alpha and β, carrying out between transient error and history error trades off and then obtain better angular acceleration estimates.

3. the inertially stabilized platform interference observer method for designing based on angular acceleration according to claim 1, is characterized in that: according to measured value I direct in step (2), step (3) in described step (4) _x, I _y, I _zwith indirect calculation value calculate roll frame disturbance torque value M _{d_x}, pitching frame disturbance torque value M _{d_y}and orientation frame disturbance torque value M _{d_z}, specific as follows:

$M_{d\_x}=J_{n\_x}{\stackrel{\·}{\mathrm{\ω}}}_x-C_{mn\_x}{K}_{gr\_x}{I}_x;$

$M_{d\_y}=J_{n\_y}{\stackrel{\·}{\mathrm{\ω}}}_y-C_{mn\_y}{K}_{gr\_y}{I}_y;$

$M_{d\_z}=J_{n\_z}{\stackrel{\·}{\mathrm{\ω}}}_z-C_{mn\_z}{K}_{gr\_z}{I}_z;$

Wherein define: J _{n_x}, J _{n_y}and J _{n_z}be respectively roll frame, pitching frame and orientation frame moment of inertia nominal value; C _{mn_x}, C _{mn_y}and C _{mn_z}be respectively roll frame, pitching frame and orientation frame motor torque system nominal value; K _{gr_x}, K _{gr_y}and K _{gr_z}be respectively roll frame, pitching frame and orientation frame motor-driven coefficient.

4. the inertially stabilized platform interference observer method for designing based on angular acceleration according to claim 1, is characterized in that: calculate the torque motor current value step offset needed for disturbance torque in described step (5) as follows:

(1) the torque motor current value of offsetting needed for this disturbance torque is calculated according to the disturbance torque value of estimation, as follows:

1. roll frame disturbance torque M is offset _{d_x}required torque motor current value

2. pitching frame disturbance torque M is offset _{d_y}required torque motor current value

3. orientation frame disturbance torque M is offset _{d_z}required torque motor current value

Wherein, C _{mn_x}, C _{mn_y}and C _{mn_z}be respectively roll frame, pitching frame and orientation frame motor torque system nominal value; K _{gr_x}, K _{gr_y}and K _{gr_z}be respectively roll frame, pitching frame and orientation frame motor-driven coefficient;

(2) by current value I in step (1) _{com_x}, I _{com_y}, I _{com_z}respectively feedforward compensation is in the given value of current input value of roll frame, pitching frame and orientation frame control system electric current loop, and motor exports the moment contrary with disturbance torque equal and opposite in direction, direction, compensation stable platform disturbance torque.