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

Design method of inertial stabilization platform disturbance observer based on angular acceleration

Technical Field

The invention relates to a design method of an inertial stabilization platform disturbance observer based on angular acceleration, which can be used for compensating various medium and high precision inertial stabilization platform disturbance moments for aerial remote sensing, and is particularly suitable for an aerial remote sensing inertial stabilization platform with larger mass eccentricity.

Background

The aerial remote sensing system is an effective means for acquiring high-resolution and high-precision remote sensing images, and plays an important role in the fields of basic mapping, disaster monitoring, resource and environment investigation, meteorological hydrological detection and the like. The aerial remote sensing inertial stabilization platform is arranged between a flight carrier and a remote sensing load, bears and stabilizes the remote sensing load, and is one of important components of an aerial remote sensing system. The stable platform can effectively isolate the influence of the non-ideal attitude motion of the flight carrier and various internal disturbances on the visual axis of the remote sensing load, so that the attitude of the remote sensing load is kept stable relative to the inertial space. After the stable platform is used, the overlapping degree between the front and back remote sensing load images and between the two adjacent frames of images is greatly improved, the imaging requirement is met, and the working efficiency of aerial remote sensing can be obviously improved. The stability precision is one of the main technical indexes of the inertially stabilized platform, and reflects the inhibition capability of the stabilized platform on the disturbance moment.

The triaxial inertially stabilized platform can bear various loads, the mass center positions of different loads are different, the mass center can deviate in the working process, and in addition, the mass center position of the platform is uncertain due to machining, so that the mass eccentricity is difficult to overcome completely in a counterweight mode. Due to the fact that mass eccentricity exists, and the imaging load mass borne by the platform is large, under the action of gravity acceleration and airplane disturbance acceleration, large unbalanced moment can be generated. Besides the moment of mass unbalance, the platform is also influenced by other interference moments such as friction moment, frame coupling moment and the like, so that the stability and the precision of the platform are reduced, the imaging quality of imaging load is reduced, and the aerial remote sensing operation efficiency is reduced. Therefore, when the control system of the stable platform is designed, the interference torque of the stable platform must be compensated, and the control precision and the dynamic performance of the stable platform are improved. In the prior art, feedback control in a three-loop PID composite control system is usually used for compensating interference in the system, but the compensation method increases the suppression capability of the disturbance by increasing PID parameters, and the increase of the PID parameters can cause the stability of the system to be reduced and even cause the instability of the system, and if the interference is uncertain or time-varying, the compensation effect of conventional PID control is not ideal; the disturbance observer can observe the disturbance moment borne by the system in real time, the disturbance moment can be completely compensated through feedforward control, however, when the disturbance observer is used for observing the disturbance moment, the differentiation of information is generally required to be solved, the traditional differentiation mode is adopted for solving the differentiation, noise signal amplification is caused, the Kalman filtering is adopted for solving the differentiation signal, the phase lag of the differentiation signal is caused, a tracking-differentiator is adopted for solving the differentiation, a larger estimation error is caused in the initial stage of the differentiation signal, and the inaccurate calculation of the differentiation signal can cause poor disturbance moment compensation effect or even cause system instability.

Disclosure of Invention

The technical problems to be solved by the invention are as follows: the defect that the conventional three-ring PID composite control and a common disturbance observer are insufficient in platform disturbance torque inhibition capacity is overcome, and the design method of the inertially stabilized platform disturbance observer based on the angular acceleration is provided and used for improving the system stabilization precision.

The technical solution of the invention is as follows: a design method of an inertial stabilization platform disturbance observer based on angular acceleration estimates a disturbance moment value according to measurement values of a current sensor and each frame rate gyro and further calculates a compensation current value based on a traditional three-ring PID composite control system structure, and specifically comprises the following steps:

(1) the method is characterized by establishing a three-ring PID composite control system of the aerial remote sensing triaxial inertial stabilization platform, and comprises the following three steps:

firstly, establishing a current loop PID feedback loop;

the output of the stabilizing loop is used as the given current value I of the current loop_setThe current sensor in the motor power driving circuit outputs the frame motor current value as the current feedback value I of the current loop circuit_fbAnd the error value of the current loop PID controller and the frame motor is used as the input of the current loop PID controller, and the output value of the current loop PID controller is used as the input frame motor voltage value to enable the frame motor to generate a corresponding current value.

Establishing a stable loop PID feedback loop;

output of tracking loop as angular velocity of stabilizing loop given by ω_setRate gyros mounted on frames for measuring angular velocity feedback values omega of frames_fbThe error value of the two is used as the input of a PID controller of the stabilizing loop, and the output value of the PID controller of the stabilizing loop is used as the given value I of the current loop current_setThe drive frame rotates at a given angular velocity.

Establishing a tracking loop PID feedback loop;

the tracking loop circuit is divided into two modes, an autonomous working mode and a combined working mode with the POS. Autonomous operating mode for measuring position feedback value theta by using accelerometer as measuring element_fbAnd the attitude information output by the POS is used as a position feedback value theta in the combined working mode with the POS_fbThe position setting value of the autonomous working mode is 0, even if the platform keeps the local level, the position setting value theta is set by the upper computer in the working mode combined with the POS machine_setAnd the error value of the set value and the feedback value is used as the input of the tracking loop PID controller, the output value of the tracking loop PID controller is used as the given value of the angular velocity of the stabilizing loop, and the frame is driven to rotate to a specified angle.

(2) Measuring current value I of transverse rolling frame motor by using current sensor on each frame motor driving circuit board_xAnd the current value I of the pitching frame motor_yAnd azimuth frame motor current value I_z(ii) a Measuring roll frame angular velocity omega using rate gyros mounted on frames_xAngular velocity omega of pitching frame_yAnd azimuth frame angular velocity ω_z；

(3) According to the measured value of the rate gyro in the step (2) -the angular speed omega of the transverse rolling frame_xAngular velocity omega of pitching frame_yAnd azimuth frame angular velocity ω_zEstimating angular acceleration information of each frame-rolling frame angular acceleration by using Kalman filtering and tracking-differentiatorAngular acceleration of pitch frameAnd azimuth frame angular acceleration

(4) According to the direct measurement value I in the step (2) and the step (3)_x、I_y、I_zAnd indirect calculated valueCalculating the interference torque value M of the horizontal rolling frame_{d_x}Pitching frame interference moment value M_{d_y}And the value M of the disturbance moment of the azimuth frame_{d_z}；

(5) According to the interference torque value M of each frame calculated in the step (4)_{d_x}、M_{d_y}And M_{d_z}Further calculating the current value I of the torque motor required for counteracting the interference torque of each frame_{com_x}、I_{com_y}And I_{com_z}And compensating the calculated current value to a current given input value of the current loop, outputting a torque with the same magnitude and the opposite direction with the interference torque by the motor, and compensating the interference torque of the inertially stabilized platform.

In the step (3), angular acceleration information of each frame is estimated by using Kalman filtering and a tracking-differentiator according to the rate gyro measurement value of each frame in the step (2), and the method specifically comprises the following steps:

(31) estimating angular acceleration information of each frame according to Kalman filtering, as follows:

establishing a Kalman filtering motion equation and an observation equation:

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

secondly, establishing a Kalman filtering equation according to the established motion equation and the observation equation:

$\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 the definition: x is the number of_kAn n-dimensional state vector for the kth sampling period; y is_kIs a one-dimensional measurement value of the kth sampling period, A, H is a n × n and 1 × n dimensional coefficient matrix, w_k、v_kRespectively system noise and measurement noise which are independent of each other and have different values of k_kAnd v_kAre also independent of each other;a one-step predictive estimation of the system state;optimally estimating for the last state; p_k/k-1Is composed ofThe corresponding prior error; p_kIs composed ofCorresponding posterior error; q is a system noise variance matrix; r is a measurement noise variance matrix; k_kIs the kalman filter gain.

Thirdly, estimating the required angular acceleration information according to the angular velocity information, and setting Kalman filtering equation parameters 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, ω is_kAngular velocity measured for a gyro β_kIs the angular acceleration to be estimated; t is the sampling period.

(32) The angular acceleration information of each frame is estimated from the tracking-differentiator as follows:

firstly, establishing a second-order discrete tracking-differentiator equation 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 sampling time;is the estimation error of the input variable r (k), and M is a velocity factor which mainly influences the tracking velocity; h is filteringFactor, mainly affecting the filtering effect, x₁(k)、x₂(k) Is an output, where x₁(k) Tracking input r (k), x₂(k) For "approximate differentiation" of the input r (k), the 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.$

wherein sign (. cndot.) is a sign function; a and d are 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.$

② sets the input variable r (k) to angular velocity ω_kThen the variable x is output₁(k)、x₂(k) Respectively, input angular velocity omega_kOf the tracking signal and the differential signal, where x₂(k) Is angular acceleration information to be estimated;

(33) and (3) obtaining more ideal angular acceleration information of each frame through fusion according to the angular acceleration information of each frame estimated in the step (31) and the angular acceleration information of each frame estimated in the step (32), wherein the angular acceleration information of each frame is as follows:

selecting the error between the angular acceleration value estimated by the Kalman filtering and the angular acceleration value estimated by the tracking-differentiator to construct the following judgment index function:

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

in the formula, alpha is more than or equal to 0, beta is more than 0, lambda is more than 0, alpha + beta is 1, and t is the error between the angular acceleration estimated by Kalman filtering and the angular acceleration estimated by the tracking-differentiator, alpha represents the proportion of transient error in the judgment index function, beta represents the proportion of past error in the judgment index function, lambda is a forgetting factor, the length of memory is determined, parameters alpha and beta are reasonably selected according to the angular acceleration estimated by Kalman filtering and the angular acceleration value estimated by the tracking-differentiator, and the trade-off is carried out between instantaneous error and historical error, so that better angular acceleration estimation is obtained.

In the step (4), the direct measurement value I in the step (2) and the step (3) is obtained_x、I_y、I_zAnd indirect calculated valueCalculating the interference torque value M of the horizontal rolling frame_{d_x}Pitching frame interference moment value M_{d_y}And the value M of the disturbance moment of the azimuth frame_{d_z}The method comprises the following steps:

$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 the definition: j. the design is a square_{n_x}、J_{n_y}And J_{n_z}Respectively, respectively taking the rotational inertia nominal values of the roll frame, the pitch frame and the azimuth frame; c_{mn_x}、C_{mn_y}And C_{mn_z}Respectively the nominal values of the motor torque systems of the roll frame, the pitch frame and the azimuth frame; k_{gr_x}、K_{gr_y}And K_{gr_z}The transmission coefficients of the motor of the roll frame, the pitch frame and the azimuth frame are respectively.

The torque motor current value required by the interference torque is offset in the step (5), and the steps are as follows:

(51) calculating the current value of the torque motor required for counteracting the disturbance torque according to the estimated disturbance torque value, and the method comprises the following steps:

① counteracts the roll-frame disturbing moment M_{d_x}Required current value of torque motor

② canceling the pitch frame disturbance moment M_{d_y}Required current value of torque motor

③ offsetting azimuth frame disturbance moment M_{d_z}Required current value of torque motor

Wherein, C_{mn_x}、C_{mn_y}And C_{mn_z}Respectively the nominal values of the motor torque systems of the roll frame, the pitch frame and the azimuth frame; k_{gr_x}、K_{gr_y}And K_{gr_z}The transmission coefficients of the motor of the roll frame, the pitch frame and the azimuth frame are respectively.

(52) The current value I in the step (51) is compared_{com_x}、I_{com_y}、I_{com_z}Respectively performing feedforward compensation on the current given input values of the current loops of the roll frame, the pitch frame and the azimuth frame control system, outputting a torque with the same magnitude and the opposite direction as the disturbance torque by the motor, and compensating the disturbance torque of the stable platform.

The principle of the invention is as follows: establishing a traditional three-ring PID composite control system aiming at an aerial remote sensing triaxial inertial stabilization platform;

and obtaining the relation between the interference torque borne by each frame and the current of the motor of the stable platform and the angular acceleration of the frame based on the structure of the interference observer. The current I of each frame motor is measured by using a current sensor on a driving circuit board_x、I_yAnd I_z(ii) a Angular velocity information omega of each frame is measured by using rate gyro mounted on each frame_x、ω_yAnd ω_z(ii) a And estimating the angular acceleration information of each frame by comprehensively utilizing Kalman filtering and a tracking-differentiator according to the angular velocity information of the frame measured by the rate gyroscope. Calculating the interference moment value M of the roll frame according to the direct measurement value or the indirect calculation value_{d_x}Pitching frame interference moment value M_{d_y}And the value M of the disturbance moment of the azimuth frame_{d_z}(ii) a Further calculates the moment M for counteracting the interference of the roll frame_{d_x}Required current value I of torque motor_{com_x}Counteracting the disturbance moment M of the pitching frame_{d_y}Required current value I of torque motor_{com_y}Counteracting the azimuth frame disturbance moment M_{d_z}Required current value I of torque motor_{com_z}And compensating the calculated current value to electricityIn the current given input value of the current loop, the motor outputs a torque which is equal to the interference torque in magnitude and opposite in direction, and the platform interference torque is compensated;

compared with the prior art, the invention has the advantages that:

(1) the invention overcomes the defects of the conventional three-ring PID control because of the real-time estimation and compensation of the disturbance moment of the stable platform, so that the stable precision of the platform is improved;

(2) the compensation principle of the invention is clear, the compensation algorithm is simple, and the invention is easy to be realized in DSP programming;

(3) the invention does not need additional sensors, realizes the compensation of the interference torque through the improvement of the control algorithm, and has the characteristics of simple structure and convenient engineering realization.

Drawings

FIG. 1 is a flow chart of a disturbance torque compensation control method according to the present invention;

FIG. 2 is a schematic diagram of the present invention for estimating angular acceleration using Kalman filtering and a tracking-differentiator;

fig. 3 is a structural diagram of a disturbance observer based on angular acceleration estimation in the present invention.

Detailed Description

As shown in fig. 1, 2 and 3, the specific implementation method of the present invention is as follows:

(1) establishing a three-ring PID composite control system of an aerial remote sensing triaxial inertially stabilized platform, comprising the following steps of:

(11) establishing a current loop PID feedback loop;

the output of the stabilizing loop is used as the given current value I of the current loop_setCurrent sensor output frame in motor power driving circuitThe current value of the overhead motor is used as the current feedback value I of the current loop_fbAnd the error value of the current loop PID controller and the frame motor is used as the input of the current loop PID controller, and the output value of the current loop PID controller is used as the input frame motor voltage value to enable the frame motor to generate a corresponding current value.

(12) Establishing a stable loop PID feedback loop;

output of tracking loop as angular velocity of stabilizing loop given by ω_setRate gyros mounted on frames for measuring angular velocity feedback values omega of frames_fbThe error value of the two is used as the input of a PID controller of the stabilizing loop, and the output value of the PID controller of the stabilizing loop is used as the given value I of the current loop current_setThe drive frame rotates at a given angular velocity.

(13) Establishing a tracking loop PID feedback loop;

the tracking loop circuit is divided into two modes, an autonomous working mode and a combined working mode with the POS. Autonomous operating mode for measuring position feedback value theta by using accelerometer as measuring element_fbAnd the attitude information output by the POS is used as a position feedback value theta in the combined working mode with the POS_fbThe position setting value of the autonomous working mode is 0, even if the platform keeps the local level, the position setting value theta is set by the upper computer in the working mode combined with the POS machine_setAnd the error value of the set value and the feedback value is used as the input of the tracking loop PID controller, the output value of the tracking loop PID controller is used as the given value of the angular velocity of the stabilizing loop, and the frame is driven to rotate to a specified angle.

(2) The method comprises the following steps of obtaining information of a current sensor and each frame rate gyro sensor:

(21) current sensors on the driving circuit board measure the current I of the motor of each frame and the current of the motor of each transverse rolling frame_xPitching frame motor current I_yAnd azimuth frame motor current I_z；

(22) Rate gyro mounted on each frame for measuring angular speed information of each frame and rolling angular speed omega of each frame_xAngular velocity omega of pitching frame_yAnd azimuth frame angular velocity ω_z；

(3) As shown in FIG. 2, angular acceleration information of frames is estimated by Kalman filtering based on angular velocity information ω of frames measured by a rate gyroAnd estimating angular acceleration information of the frame using a tracking-differentiatorThen, fusion is carried out through evaluation criteria to obtain more accurate angular acceleration information, and the specific steps are as follows:

(31) estimating angular acceleration information of each frame according to Kalman filtering, as follows:

establishing a Kalman filtering motion equation and an observation equation:

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

secondly, establishing a Kalman filtering equation according to the established motion equation and the observation equation:

$\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 the definition: x is the number of_kAn n-dimensional state vector for the kth sampling period; y is_kIs a one-dimensional measurement value of the kth sampling period, A, H is a n × n and 1 × n dimensional coefficient matrix, w_k、v_kRespectively system noise and measurement noise which are independent of each other and have different values of k_kAnd v_kAre also independent of each other;a one-step predictive estimation of the system state;optimally estimating for the last state; p_k/k-1Is composed ofThe corresponding prior error; p_kIs composed ofCorresponding posterior error; q is a system noise variance matrix; r is a measurement noise variance matrix; k_kIs the kalman filter gain.

Thirdly, estimating the required angular acceleration information according to the angular velocity information, and setting Kalman filtering equation parameters 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, ω is_kAngular velocity measured for a gyro β_kIs the angular acceleration to be estimated; t is the sampling period.

(32) The angular acceleration information of each frame is estimated from the tracking-differentiator as follows:

firstly, establishing a second-order discrete tracking-differentiator equation 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 sampling time;is the estimation error of the input variable r (k), and M is a velocity factor which mainly influences the tracking velocity; h is a filtering factor, mainly affecting the filtering effect, x₁(k)、x₂(k) Is an output, where x₁(k) Tracking input r (k), x₂(k) For "approximate differentiation" of the input r (k), the 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.$

wherein sign (. cndot.) is a sign function; a and d are 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.$

② sets the input variable r (k) to angular velocity ω_kThen the variable x is output₁(k)、x₂(k) Respectively, input angular velocity omega_kOf the tracking signal and the differential signal, where x₂(k) Is the angular acceleration information to be estimated.

(33) And (4) obtaining more ideal angular acceleration information of each frame through fusion according to the angular acceleration information of each frame obtained in the step (31) and the step (32), wherein the angular acceleration information of each frame is as follows:

selecting the error between the angular acceleration value estimated by the Kalman filtering and the angular acceleration value estimated by the tracking-differentiator to construct the following judgment index function:

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

in the formula, alpha is more than or equal to 0, beta is more than 0, lambda is more than 0, alpha + beta is 1, and t is the error between the angular acceleration estimated by Kalman filtering and the angular acceleration estimated by the tracking-differentiator, alpha represents the proportion of transient error in the judgment index function, beta represents the proportion of past error in the judgment index function, lambda is a forgetting factor, the length of memory is determined, parameters alpha and beta are reasonably selected according to the angular acceleration estimated by Kalman filtering and the angular acceleration value estimated by the tracking-differentiator, and the trade-off is carried out between instantaneous error and historical error, so that better angular acceleration estimation is obtained. When the influence of the instantaneous error on the evaluation index function is large, a small alpha is selected, when the influence of the past error on the evaluation index function is large, a small beta is selected, and through experiments, alpha is 0.324, and beta is 0.676, so that the obtained angular acceleration is estimated ideally.

(4) As shown in FIG. 3, the disturbance observer structure based on angular acceleration estimation is based on the traditional three-loop PID compound control system, and outputs omega through angular velocity_outAnd a current output I_outObtaining the frame motor current value I required by compensating the interference torque through certain calculation_comThe current value is compensated to the input end of the current loop, so that the frame motor can generate a moment which is equal to the interference moment in magnitude and opposite in direction, and the interference moment is compensated. According to the direct measurement value I in the step (2) and the step (3)_x、I_y、I_zAnd indirect calculated valueCalculating the interference torque value M of the horizontal rolling frame_{d_x}Pitching frame interference moment value M_{d_y}And the value M of the disturbance moment of the azimuth frame_{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 the definition: j. the design is a square_{n_x}、J_{n_y}And J_{n_z}Respectively, respectively taking the rotational inertia nominal values of the roll frame, the pitch frame and the azimuth frame; c_{mn_x}、C_{mn_y}And C_{mn_z}Respectively the nominal values of the motor torque systems of the roll frame, the pitch frame and the azimuth frame; k_{gr_x}、K_{gr_y}And K_{gr_z}The transmission coefficients of the motor of the roll frame, the pitch frame and the azimuth frame are respectively.

(5) As shown in fig. 3, the steps of calculating the torque motor current value required for canceling the disturbance torque of each frame in (4) according to the calculated disturbance torque value of each frame are as follows:

(51) calculating the current value of the torque motor required for counteracting the disturbance torque according to the estimated disturbance torque value, and the method comprises the following steps:

① counteracts the roll-frame disturbing moment M_{d_x}Required current value of torque motor

② canceling the pitch frame disturbance moment M_{d_y}Required current value of torque motor

③ offsetting azimuth frame disturbance moment M_{d_z}Required current value of torque motor

Wherein, C_{mn_x}、C_{mn_y}And C_{mn_z}Respectively the nominal values of the motor torque systems of the roll frame, the pitch frame and the azimuth frame; k_{gr_x}、K_{gr_y}And K_{gr_z}The transmission coefficients of the motor of the roll frame, the pitch frame and the azimuth frame are respectively.

(52) The current value I in (51)_{com_x}、I_{com_y}、I_{com_z}Respectively performing feedforward compensation on the current given input values of the current loops of the roll frame, the pitch frame and the azimuth frame control system, outputting a torque with the same magnitude and the opposite direction as the disturbance torque by the motor, and compensating the disturbance torque of the stable platform.

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

Claims (4)

1. A design method of an inertial stabilization platform disturbance observer based on angular acceleration is characterized in that based on a traditional three-ring PID composite control system structure, a disturbance moment value of each frame is estimated according to a current sensor and a measured value of each frame rate gyro, and a compensation current value is further calculated, and specifically comprises the following steps:

(1) the method is characterized by establishing a three-ring PID composite control system of the aerial remote sensing triaxial inertial stabilization platform, and comprises the following three steps:

firstly, establishing a current loop PID feedback loop;

of stationary-ring circuitsOutputting a given current value I as a current loop circuit_setThe current sensor in the motor power driving circuit outputs the frame motor current value as the current feedback value I of the current loop circuit_fbThe error value of the current loop PID controller and the frame motor is used as the input of the current loop PID controller, and the output value of the current loop PID controller is used as the voltage value of the input frame motor to enable the frame motor to generate a corresponding current value;

establishing a stable loop PID feedback loop;

output of tracking loop as angular velocity of stabilizing loop given by ω_setRate gyros mounted on frames for measuring angular velocity feedback values omega of frames_fbThe error value of the two is used as the input of a PID controller of the stabilizing loop, and the output value of the PID controller of the stabilizing loop is used as the given value I of the current loop current_setDriving the frame to rotate at a given angular speed;

establishing a tracking loop PID feedback loop;

the tracking loop is divided into two modes, namely an autonomous working mode and a POS combined working mode, wherein the autonomous working mode takes an accelerometer as a measuring element to measure a position feedback value theta_fbAnd the attitude information output by the POS is used as a position feedback value theta in the combined working mode with the POS_fbThe position setting value of the autonomous working mode is 0, even if the platform keeps the local level, the position setting value theta is set by the upper computer in the working mode combined with the POS machine_setThe error value of the set value and the feedback value is used as the input of a tracking loop PID controller, the output value of the tracking loop PID controller is used as the given value of the angular velocity of the stabilizing loop, and the frame is driven to rotate to a specified angle;

(2) measuring current value I of transverse rolling frame motor by using current sensor on each frame motor driving circuit board_xAnd the current value I of the pitching frame motor_yAnd azimuth frame motor current I_z(ii) a Measuring roll frame angular velocity omega using rate gyros mounted on frames_xAngular velocity omega of pitching frame_yAnd azimuth frame angular velocity ω_z；

(3) According to the measured value of the rate gyro in the step (2) -the angular speed omega of the transverse rolling frame_xAngular velocity omega of pitching frame_yAnd azimuth frame angular velocity ω_zEstimating angular acceleration information of each frame-rolling frame angular acceleration by using Kalman filtering and tracking-differentiatorAngular acceleration of pitch frameAnd azimuth frame angular acceleration

(4) According to the direct measurement value I in the step (2) and the step (3)_x、I_y、I_zAnd indirect calculated valueCalculating the interference torque value M of the horizontal rolling frame_{d_x}Pitching frame interference moment value M_{d_y}And the value M of the disturbance moment of the azimuth frame_{d_z}；

(5) According to the interference torque value M of each frame calculated in the step (4)_{d_x}、M_{d_y}And M_{d_z}Further calculating the current value I of the torque motor required for counteracting the interference torque of each frame_{com_x}、I_{com_y}And I_{com_z}And compensating the calculated current value to a current given input value of the current loop, outputting a torque with the same magnitude and the opposite direction with the interference torque by the motor, and compensating the interference torque of the inertially stabilized platform.

2. The method for designing an inertially stabilized platform disturbance observer based on angular acceleration according to claim 1, characterized in that: in the step (3), angular acceleration information of each frame is estimated by using Kalman filtering and a tracking-differentiator according to the rate gyro measurement value of each frame in the step (2), and the specific method is as follows:

(1) estimating angular acceleration information of each frame according to Kalman filtering, as follows:

establishing a Kalman filtering motion equation and an observation equation:

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

secondly, establishing a Kalman filtering equation according to the established motion equation and the observation equation:

$\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 the definition: x is the number of_kAn n-dimensional state vector for the kth sampling period; y is_kIs a 1-dimensional measurement value of the kth sampling period, and A, H is a coefficient moment of n × n and 1 × n dimensionsArraying; w is a_k、v_kRespectively system noise and measurement noise which are independent of each other and have different values of k_kAnd v_kAre also independent of each other;a one-step predictive estimation of the system state;optimally estimating for the last state; p_k/k-1Is composed ofThe corresponding prior error; p_kIs composed ofCorresponding posterior error; q is a system noise variance matrix; r is a measurement noise variance matrix; k_kIs the Kalman filter gain;

thirdly, estimating the required angular acceleration information according to the angular velocity information, and setting Kalman filtering equation parameters 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, ω is_kAngular velocity measured for a gyro β_kIs the angular acceleration to be estimated; t is a sampling period;

(2) the angular acceleration information of each frame is estimated from the tracking-differentiator as follows:

firstly, establishing a second-order discrete tracking-differentiator equation 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 sampling time;is the estimation error of the input variable r (k), and M is a velocity factor which mainly influences the tracking velocity; h is a filtering factor, mainly affecting the filtering effect, x₁(k)、x₂(k) Is an output, where x₁(k) Tracking input r (k), x₂(k) For "approximate differentiation" of the input r (k), the 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.$

wherein sign (. cndot.) is a sign function; a and d are 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.$

② sets the input variable r (k) to angular velocity ω_kThen the variable x is output₁(k)、x₂(k) Respectively, input angular velocity omega_kOf the tracking signal and the differential signal, where x₂(k) Is angular acceleration information to be estimated;

(3) and (3) obtaining more ideal angular acceleration information of each frame through fusion according to the angular acceleration information of each frame estimated in the step (1) and the angular acceleration information of each frame estimated in the step (2), wherein the method comprises the following steps:

selecting the error between the angular acceleration value estimated by the Kalman filtering and the angular acceleration value estimated by the tracking-differentiator to construct the following judgment index function:

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

in the formula, alpha is more than or equal to 0, beta is more than 0, lambda is more than 0, alpha + beta is 1, and t is the error between the angular acceleration estimated by Kalman filtering and the angular acceleration estimated by the tracking-differentiator, alpha represents the proportion of transient error in the judgment index function, beta represents the proportion of historical error in the judgment index function, lambda is a forgetting factor, the length of memory is determined, parameters alpha and beta are reasonably selected according to the angular acceleration estimated by Kalman filtering and the angular acceleration value estimated by the tracking-differentiator, and the transient error and the historical error are subjected to compromise to obtain better angular acceleration estimation.

3. The method for designing an inertially stabilized platform disturbance observer based on angular acceleration according to claim 1, characterized in that: in the step (4), the direct measurement value I in the step (2) and the step (3) is obtained_x、I_y、I_zAnd indirect calculated valueCalculating the interference torque value M of the horizontal rolling frame_{d_x}Pitching frame interference moment value M_{d_y}And the value M of the disturbance moment of the azimuth frame_{d_z}The method comprises the following steps:

$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 the definition: j. the design is a square_{n_x}、J_{n_y}And J_{n_z}Respectively, respectively taking the rotational inertia nominal values of the roll frame, the pitch frame and the azimuth frame; c_{mn_x}、C_{mn_y}And C_{mn_z}Respectively the nominal values of the motor torque systems of the roll frame, the pitch frame and the azimuth frame; k_{gr_x}、K_{gr_y}And K_{gr_z}The transmission coefficients of the motor of the roll frame, the pitch frame and the azimuth frame are respectively.

4. The method for designing an inertially stabilized platform disturbance observer based on angular acceleration according to claim 1, characterized in that: the step of calculating the torque motor current value required for counteracting the disturbance torque in the step (5) is as follows:

(1) calculating the current value of the torque motor required for counteracting the disturbance torque according to the estimated disturbance torque value, and the method comprises the following steps:

① counteracts the roll-frame disturbing moment M_{d_x}Required current value of torque motor

② canceling the pitch frame disturbance moment M_{d_y}Required current value of torque motor

③ offsetting azimuth frame disturbance moment M_{d_z}Required current value of torque motor

Wherein, C_{mn_x}、C_{mn_y}And C_{mn_z}Respectively the nominal values of the motor torque systems of the roll frame, the pitch frame and the azimuth frame; k_{gr_x}、K_{gr_y}And K_{gr_z}Respectively the transmission coefficients of the motor of the roll frame, the pitch frame and the azimuth frame;

(2) the current value I in the step (1) is measured_{com_x}、I_{com_y}、I_{com_z}Respectively performing feedforward compensation on the current given input values of the current loops of the roll frame, the pitch frame and the azimuth frame control system, outputting a torque with the same magnitude and the opposite direction as the disturbance torque by the motor, and compensating the disturbance torque of the stable platform.