CN102508503A - Compensation method based on generalized inner module for eccentric torque of three-shaft inertially stabilized platform - Google Patents

Abstract

A compensation method based on a generalized internal module for eccentric torque of a three-shaft inertially stabilized platform includes establishing a state-space equation of a control system of the three-shaft inertially stabilized platform, measuring angular rate information of the platform in real time by the aid of a rate gyroscope, measuring current information in real time by the aid of a current sensor, implanting a joint unstable module of reference input and the eccentric torque into the system, and achieving the purpose of unsteady-state error tracking control by the aid of generalized inner module control algorithm. An internal module controller comprises a servo compensator and a stabilized compensator. By the aid of the compensation method, disturbance attenuation capacity is improved, unsteady-state error tracking is realized, and a high robust performance is realized.

Description

A kind of three inertially stabilized platform eccentric moments compensation method based on the broad sense internal mold

Technical field

The present invention relates to a kind of three inertially stabilized platform eccentric moments compensation method based on the broad sense internal mold; Belong to high resolving power aviation earth observation systems field; Can be used for the demanding three inertially stabilized platform floating tracking Control of lasting accuracy, be particularly suitable for three inertially stabilized platforms of small-sized high resolving power airborne remote sensing.

Background technology

Inertially stabilized platform is to realize the necessaries of high resolving power earth observation, and it can effectively isolate the disturbance and the imperfect attitude motion of flying platform, and it is stable to make optical axis sensing of observation load and course remain inertial space.At present, external mainstream product is the PAV30 and the PAV80 of Switzerland Leica company, and domestic correlative study is at the early-stage, no matured product.

The influence of interference-free moment under the ideal situation; It is stable that stable platform remains inertial space, but owing to actual mismachining tolerance, the unequal factor of counterweight, the barycenter of platform and framework axle center decentraction; There is certain eccentric throw; So under the effect of acceleration of gravity and motion artifacts acceleration, the athletic meeting of platform receives the influence of eccentric moment, thereby influence performance index such as its lasting accuracy; Quality, eccentric throw and motion artifacts acceleration are big more, and eccentric moment is big more, and lasting accuracy is poor more, suppress the effect of eccentric moment so something must be done to.Compensation method for the inertially stabilized platform eccentric moment; Applied for one piece of patent " a kind of unbalanced moment of aerial remote sensing inertially stabilized platform is estimated and compensation method " (application number 200910241242.9) at present; This method is measured the sky to acceleration and motion artifacts acceleration through the mems accelerometer that is installed on the platform, adopts low pass filtering method that current information is carried out filtering, and eccentric moment is estimated out; And adopt feed forward method to compensate; Can suppress the effect of eccentric moment to a certain extent, but the deficiency of three aspects below existing: the first, for three inertially stabilized platforms, adopt the method three mems accelerometers to be installed respectively at each framework; The all corresponding increasing of platform by volume quality is unfavorable for its small-sized structural design; The second, zero of mems accelerometer inclined to one side stability and repeatability are all relatively poor, and measured value contains big noise, when feedforward compensation, introduce unknown disturbance factor; Three, for the platform real-time control system, the Butterworth low-pass filter is difficult realizes that LPF property can influence the Disturbance Rejection ability simultaneously.So in sum, this method is physically difficult to realize that this has directly limited the application power in real work.

Summary of the invention

Technology of the present invention is dealt with problems and is: overcome prior art and realize the defective of the estimation and the compensation of eccentric moment through increasing measuring sensor; A kind of method that can be on the basis of original system assembly compensates eccentric moment through CONTROLLER DESIGN is provided; Do not increase volume mass, and simple, reliable.

Technical solution of the present invention is: a kind of three inertially stabilized platform eccentric moments compensation method based on the broad sense internal mold, and performing step is following:

(1) employing is installed in the angular speed information ω that three rate gyros on the inertially stabilized platform gimbal axis are measured three inertially stabilized platforms _Out, said angular speed information has comprised the angular speed information that Electric Machine Control moment and eccentric moment act on following three inertially stabilized platforms simultaneously;

(2) with the angular speed information ω that obtains in the step (1) _OutWith angular speed setting value ω _SetDiffer from, obtain angular speed error e=ω _Set-ω _Out

(3) the angular speed error e that obtains in the step (2) is brought in the servo compensator goes, obtain servo compensator controlled quentity controlled variable u _e, the state space equation of servo compensator does

Controlled quentity controlled variable u _e=K _ex _e, A wherein _eBe servo compensator system matrix, B _eBe servo compensator control input matrix, K _eBe servo compensator state feedback matrix, x _eBe servo compensator state variable, u _eBe the servo compensator controlled quentity controlled variable;

(4) adopt the current sensor measurement that is connected in the electric motor loop to go out to be installed in the current information i of three torque motors on the inertially stabilized platform framework _Out, the angular speed information ω that obtains in the integrating step (1) simultaneously _OutBe brought in the calm compensator and go, obtain calm compensator controlled quentity controlled variable u ₂=Kx, x=[ω _Outi _Out] ', wherein K is calm compensator state feedback matrix, u ₂Be calm compensator controlled quentity controlled variable;

(5) with the servo compensator controlled quentity controlled variable u that obtains in the step (3) _eWith the calm compensator controlled quentity controlled variable u that obtains in the step (5) ₂Differ from, obtain broad sense internal model control algorithm controls amount u=u _e-u ₂

(6) the broad sense internal model control algorithm controls amount u that obtains in the step (5) is brought at reference input r and eccentric moment T _dGo in the former speed open cycle system (like the former speed open cycle system in the accompanying drawing 1) under the effect, realize the compensation of eccentric moment, realize that finally the floating of system is followed the tracks of.

Former speed open cycle system i.e. three inertially stabilized platform speed open cycle systems, and its broad sense internal model control amount that is input as u is output as angular speed ω _Out, the state space equation of establishing former speed open cycle system does $\left\{\begin{array}{c}\stackrel{\·}{x}=\mathrm{Ax}+\mathrm{Bu}+B_d{T}_d\\ y=\mathrm{Cx}+\mathrm{Du}+D_d{T}_d\end{array}\right.;$ Wherein, x is the state variable of former speed open cycle system, x=[ω _Outi _Out] '; A is the system matrix of former speed open cycle system; B is the control input matrix of former speed open cycle system; B _dThe eccentric moment input matrix of former speed open cycle system; T _dBe eccentric moment; Y is the output variable of former speed open cycle system, y=ω _OutC is the output matrix of former speed open cycle system; D is the transmission matrix of former speed open cycle system; D _dEccentric moment output matrix for former speed open cycle system; U is the control input;

Servo compensator system matrix A in the said step (3) _e, servo compensator control input matrix B _e, servo compensator state feedback matrix K _eSpecifically to obtain step following with the state feedback matrix K of calm compensator:

(31) at first confirm with reference to input ω _SetWith eccentric moment T _dThe common unstable model of model obtains with reference to input ω _SetWith eccentric moment T _dThe least common multiple formula of two unstable models: φ (s)=s ^l+ α _L-1s ^L-1+ ... + α ₁s ¹+ α ₀, with reference to input ω _SetWith eccentric moment T _dModel is known, then alpha ₀～α _L-1Be known quantity; L is the high-order term of φ (s), and s is a frequency domain symbol, α ₀～α _L-1Each time coefficient for φ (s).

(32) by the alpha of φ (s) ₀～α _L-1Determine blocking factor matrix Γ _L*1And β _L*1, ${\mathrm{\Γ}}_{l*l}=\left[\begin{array}{cccc}0& & & \\ 0& & I_{l-1}& \\ .& & & \\ .& & & \\ .& & & \\ {\mathrm{\α}}_0& {\mathrm{\α}}_1& ...& {\mathrm{\α}}_{l-1}\end{array}\right],$ ${\mathrm{\β}}_{l*1}=\left[\begin{array}{c}0\\ 0\\ .\\ .\\ .\\ 1\end{array}\right];$ Wherein, l is the high-order term of φ (s), α ₀～α _L-1Be each time coefficient of φ (s), I _L-1Be l-1 rank unit matrix;

(33) the blocking factor matrix Γ that obtains by step (32) _L*1And β _L*1, obtain the coefficient matrices A of servo compensator state space equation _eWith control input matrix B _e, wherein,

So just obtain the state space equation of servo compensator

Servo compensator state feedback matrix K _eState feedback matrix K with calm compensator;

(34) the servo compensator state space equation and the former speed open cycle system state space equation that step (33) are obtained make up, and obtain the state space equation of final cascade system: $\left[\begin{array}{c}\stackrel{\·}{x}\\ {\stackrel{\·}{x}}_e\end{array}\right]=\left[\begin{array}{cc}A& 0\\ -B_{e}C& A_e\end{array}\right]\left[\begin{array}{c}x\\ x_e\end{array}\right]+\left[\begin{array}{c}B\\ -B_{e}D\end{array}\right]u+\left[\begin{array}{c}{B}_d\\ -B_e{D}_d\end{array}\right]T_d+\left[\begin{array}{c}0\\ B_e\end{array}\right]r,$ The definition front of each coefficient provides;

(35) the cascade system state space equation that obtains in the step (34) is adopted classical POLE PLACEMENT USING u=K _Tx _TMethod is carried out POLE PLACEMENT USING, obtains state feedback matrix K _T, wherein, $x_T={\left[\begin{array}{c}x\\ x_e\end{array}\right]}^{\′};$

(36) with the state feedback matrix K that obtains in the step (35) _TDecompose, make K _T=[K K _e], obtain the state feedback matrix K of servo compensator _eState feedback matrix K with calm compensator.

Principle of work of the present invention: three inertially stabilized platforms remain inertial space to be stablized, and under quiescent conditions, there is normal value eccentric moment T in platform _DCThe influence of=mgl, platform control system are through the effect of close-loop feedback, and the control moment motor is exported and T _DCEqual and opposite in direction, moment in the opposite direction keep the platform inertial space stable; Under current intelligence, there is the motion artifacts acceleration in platform framework, and then the eccentric moment formula is:

T _d＝m(g+a)l ①

In the formula, m is the framework quality, and g is an acceleration of gravity, and a is the motion artifacts acceleration, and l is an eccentric throw.

In the practical flight process, the most typical disturbance form of aircraft is sinusoidal perturbation, and the disturbed motion acceleration a with aircraft is regarded as the sinusoidal quantity that frequency is 1HZ here, and its frequency structure characteristic is:

${\mathrm{\&phi;}}_{T_d}\left(s\right)={\mathrm{<figure-callout\; id="2"\; label="s"\; filenames="HDA0000104328880000032.png"\; state="\{\{state\}\}">s</figure-callout>}}^2+4{\mathrm{\&pi;}}^2$ ②

Step with reference to the frequency structure characteristic of input is:

φ _r(s)＝s ③

By formula 2., 3. can be with reference to the input and the common unstable model of eccentric moment:

φ(s)＝s(s ²+4π ²) ④

4. can set up servo compensator state space equation in the broad sense internal model control algorithm by formula:

${\stackrel{\&CenterDot;}{x}}_e=A_e{x}_e+B_{e}e$ ⑤

Obtain the controlled quentity controlled variable form of servo compensator and calm compensator through the POLE PLACEMENT USING method of classics:

$\left\{\begin{array}{c}{u}_e=K_e{\mathrm{<figure-callout\; id="2"\; label="x\; e\; u"\; filenames="HDA0000104328880000032.png"\; state="\{\{state\}\}">x</figure-callout>}}_e& \\ u_{}{}_2=\mathrm{Kx}\end{array}\right.$ ⑥

In the formula, x is the state variable of former speed open cycle system, x=[ω _Outi _Out] ', ω _OutBe the mesa corners rate information, measure by rate gyro; ω _OutFor platform current of electric information, by current sensor measurement.

Finally, broad sense internal model control algorithm controls amount is u=u _e-u ₂, this controlled quentity controlled variable affacted in the former speed open cycle system go, the driving moment motor action, implementation platform is to reference to the asymptotic tracking of input and the compensation of eccentric moment.

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

(1) the present invention measures mesa corners rate information and current of electric information respectively through platform self assembly rate gyro and current sensor, realizes broad sense internal model control algorithm, and its process does not increase the platform by volume quality, helps small-sized structural design.

(2) broad sense internal model control of the present invention is based upon on the state space equation basis, is made up of common integrator, proportional component, and algorithm is simple, and realization and reliable has stronger actual application ability easily.

(3) the present invention has taken into account with reference to input and eccentric moment and has acted on the influence that brings simultaneously, and disturbances such as modeling error, parameter perturbation are had stronger insensitivity, has improved the robust performance of system.

Description of drawings

Fig. 1 is a broad sense internal model control algorithm implementation step synoptic diagram of the present invention;

Fig. 2 is servo compensator of the present invention and calm design of Compensator process flow diagram;

Fig. 3 is the eccentric moment compensation method structural drawing based on the broad sense internal mold of the present invention;

Fig. 4 is not for adopting the angular speed output of three inertially stabilized platforms of the present invention under the eccentric moment effect;

Fig. 5 has adopted the angular speed output of three inertially stabilized platforms of the present invention under the eccentric moment effect.

Embodiment

Concrete implementation step is as shown in Figure 1:

(1) system powers on, initialization, rate gyro signal acquisition circuit and motor current signal Acquisition Circuit life's work;

(2) employing is installed in the angular speed information ω that three rate gyros on the inertially stabilized platform gimbal axis are measured three inertially stabilized platforms _Out, said angular speed information has comprised the angular speed information that Electric Machine Control moment and eccentric moment act on following three inertially stabilized platforms simultaneously;

(3) with the angular speed information ω that obtains in the step (2) _OutWith angular speed setting value ω _SetDiffer from, obtain angular speed error e=ω _Set-ω _Out

(4) the angular speed error e that obtains in the step (3) is brought in the servo compensator removes controlled amount u _e, the state space equation of servo compensator does

Controlled quentity controlled variable u _e=K _ex _e

Wherein, A _eBe servo compensator system matrix, B _eBe servo compensator control input matrix, K _eBe servo compensator state feedback matrix, x _eBe servo compensator state variable, u _eBe the servo compensator controlled quentity controlled variable;

(5) adopt the current sensor measurement that is connected in the electric motor loop to go out to be installed in the current information i of three torque motors on the inertially stabilized platform framework _Out, the angular speed information ω that obtains in the integrating step (2) simultaneously _OutBe brought in the calm compensator and remove controlled amount u ₂=Kx, x=[ω _Outi _Out] ';

Wherein, K is calm compensator state feedback matrix, u ₂Be calm compensator controlled quentity controlled variable;

(6) with the controlled quentity controlled variable u that obtains in the step (4) _eAnd the controlled quentity controlled variable u that obtains in the step (5) ₂Differ from, obtain broad sense internal model control algorithm controls amount u=u _e-u ₂

(7) the broad sense internal model control algorithm controls amount u that obtains in the step (6) is brought at reference input r and eccentric moment T _dGo in the former speed open cycle system (like the former speed open cycle system in the accompanying drawing 1) under the effect, realize the compensation of eccentric moment, realize that finally the floating of system is followed the tracks of.

Former speed open cycle system i.e. three inertially stabilized platform speed open cycle systems, and its broad sense internal model control amount that is input as u is output as angular speed ω _Out, the state space equation of establishing former speed open cycle system is: $\left\{\begin{array}{c}\stackrel{\&CenterDot;}{x}=\mathrm{Ax}+\mathrm{Bu}+B_d{T}_d\\ y=\mathrm{Cx}+\mathrm{Du}+D_d{T}_d\end{array}\right.;$ Wherein, x is the state variable of former speed open cycle system, x=[ω _Outi _Out] '; A is the system matrix of former speed open cycle system; B is the control input matrix of former speed open cycle system; B _dThe eccentric moment input matrix of former speed open cycle system; T _dBe eccentric moment; Y is the output variable of former speed open cycle system, y=ω _OutC is the output matrix of former speed open cycle system; D is the transmission matrix of former speed open cycle system; D _dEccentric moment output matrix for former speed open cycle system; U is the control input;

Be illustrated in figure 2 as the state space equation system matrix A of servo compensator of the present invention _e, control input matrix B _e, state feedback matrix K _eFollowing with the concrete performing step of state feedback matrix K of calm compensator:

(1) at first confirms with reference to input ω _SetWith eccentric moment T _dThe common unstable model of model obtains with reference to input ω _SetWith eccentric moment T _dLeast common multiple formula φ (the s)=s of two unstable models ^l+ α _L-1s ^L-1+ ... + α ₁s ¹+ α ₀, with reference to input ω _SetWith eccentric moment T _dModel is known, then alpha ₀～α _L-1Be known quantity; Wherein, l is the high-order term of φ (s), and s is a frequency domain symbol, α ₀～α _L-1Each time coefficient for φ (s);

(2) by the alpha of φ (s) ₀～α _L-1Determine blocking factor matrix Γ _L*1And β _L*1, ${\mathrm{\&Gamma;}}_{l*l}=\left[\begin{array}{cccc}0& & & \\ 0& & I_{l-1}& \\ .& & & \\ .& & & \\ .& & & \\ {\mathrm{\&alpha;}}_0& {\mathrm{\&alpha;}}_1& ...& {\mathrm{\&alpha;}}_{l-1}\end{array}\right],$ ${\mathrm{\&beta;}}_{l*1}=\left[\begin{array}{c}0\\ 0\\ .\\ .\\ .\\ 1\end{array}\right];$ Wherein, l is the high-order term of φ (s), α ₀～α _L-1Be each time coefficient of φ (s), I _L-1Be l-1 rank unit matrix;

(3) the blocking factor matrix Γ that obtains by step (2) _L*1And β _L*1, obtain the coefficient matrices A of servo compensator state space equation _eWith control input matrix B _e, wherein,

So just obtain the state space equation of servo compensator

Servo compensator state feedback matrix K _eState feedback matrix K with calm compensator;

(4) the servo compensator state space equation and the former speed open cycle system state space equation that step (3) are obtained make up, and obtain the state space equation of final cascade system: $\left[\begin{array}{c}\stackrel{\&CenterDot;}{x}\\ {\stackrel{\&CenterDot;}{x}}_e\end{array}\right]=\left[\begin{array}{cc}A& 0\\ -B_{e}C& A_e\end{array}\right]\left[\begin{array}{c}x\\ x_e\end{array}\right]+\left[\begin{array}{c}B\\ -B_{e}D\end{array}\right]u+\left[\begin{array}{c}{B}_d\\ -B_e{D}_d\end{array}\right]T_d+\left[\begin{array}{c}0\\ B_e\end{array}\right]r,$ The definition front of each coefficient provides;

(5) the cascade system state space equation that obtains in the step (4) is adopted classical POLE PLACEMENT USING u=K _Tx _TMethod is carried out POLE PLACEMENT USING, obtains state feedback matrix K _T, wherein, $x_T={\left[\begin{array}{c}x\\ x_e\end{array}\right]}^{\&prime;};$

(6) with the state feedback matrix K that obtains in the step (5) _TDecompose, make K _T=[K K _e], obtain the state feedback matrix K of servo compensator _eState feedback matrix K with calm compensator.

Be illustrated in figure 3 as the eccentric moment compensation method structural drawing based on the broad sense internal mold of the present invention.Will be with reference to input ω _SetThe rate information ω that measures in real time with rate gyro _OutMake difference and obtain margin of error e=ω _Set-ω _OutE affacts servo compensator with the margin of error $\left\{\begin{array}{c}{\stackrel{\&CenterDot;}{x}}_e=A_e{x}_e+B_{e}e\\ u_e=K_e{x}_e\end{array}\right.$ In controlled amount u _eWith servo compensator controlled quentity controlled variable u _e=K _ex _eWith calm compensator controlled quentity controlled variable u ₂=Kx makes difference and obtains overhead control amount u=u _e-u ₂Controlled quentity controlled variable u affacted in the former speed open cycle system go, can realize with reference to input ω _SetAsymptotic tracking and eccentric moment T _dCompensation, the state space equation of its Central Plains speed open cycle system is:

$\left\{\begin{array}{c}\stackrel{\&CenterDot;}{x}=\mathrm{Ax}+\mathrm{Bu}+B_d{T}_d\\ y=\mathrm{Cx}+\mathrm{Du}+D_d{T}_d\end{array}\right..$

For verifying validity of the present invention, carried out emulation experiment.Simulation parameter is: framework quality 100Kg; Airplane motion disturbing acceleration a is that 1g, frequency are 1Hz; The platform framework eccentric throw is 1cm, and then under the effect of the airplane motion disturbing acceleration of acceleration of gravity and alternation, the expression formula of eccentric moment is (10+10sin (2 π t)) Nm.The system state matrix: $A=\left[\begin{array}{cc}0& 1.25\\ -130& -588\end{array}\right],$ $B=\left[\begin{array}{c}0\\ 130\end{array}\right],$ C=[0 1], D=0, $A_e=\left[\begin{array}{ccc}0& 1& 0\\ 0& 0& 1\\ 0& -4{\mathrm{\&pi;}}^2& 0\end{array}\right],$ $B_e=\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right],$ System state feedback matrix K _e=[3882.7 670.6 56], K=[3.129-3.826].

Be illustrated in figure 4 as and do not adopt the angular speed output of three inertially stabilized platforms of the present invention under the eccentric moment effect under these conditions; Can find out that angular speed output appears with frequency and the bigger sinusoidal fluctuation of amplitude under sinusoidal eccentric moment effect; Obviously can make system can't keep stable, platform is with ineffective.

Be illustrated in figure 5 as and adopt the angular speed output of three inertially stabilized platforms of the present invention under the eccentric moment effect under these conditions; Can find out under broad sense internal mold algorithm controls; Angular speed is exported very rapid convergence to steady-state value, and finally maintains near the null value, and eccentric moment is played the good restraining effect; Guaranteed the rate stabilization of platform, realized that the floating of system is followed the tracks of.

The present invention does not set forth part in detail and belongs to techniques well known.

Claims (2)

1. three inertially stabilized platform eccentric moments compensation method based on the broad sense internal mold is characterized in that performing step is following:

(1) employing is installed in the angular speed information ω that three rate gyros on the inertially stabilized platform gimbal axis are measured three inertially stabilized platforms _Out, said angular speed information has comprised the angular speed information that Electric Machine Control moment and eccentric moment act on following three inertially stabilized platforms simultaneously;

(2) with the angular speed information ω that obtains in the step (1) _OutWith angular speed setting value ω _SetDiffer from, obtain angular speed error e=ω _Set-ω _Out

(3) the angular speed error e that obtains in the step (2) is brought in the servo compensator goes, obtain servo compensator controlled quentity controlled variable u _e, the state space equation of servo compensator does

Controlled quentity controlled variable u _e=K _ex _e, A wherein _eBe servo compensator system matrix, B _eBe servo compensator control input matrix, K _eBe servo compensator state feedback matrix, x _eBe servo compensator state variable, u _eBe the servo compensator controlled quentity controlled variable;

(4) adopt the current sensor measurement that is connected in the electric motor loop to go out to be installed in the current information i of three torque motors on the inertially stabilized platform framework _Out, the angular speed information ω that obtains in the integrating step (1) simultaneously _OutBe brought in the calm compensator and go, obtain calm compensator controlled quentity controlled variable u ₂=Kx, x=[ω _Outi _Out] ', wherein K is calm compensator state feedback matrix, u ₂Be calm compensator controlled quentity controlled variable;

(5) with the servo compensator controlled quentity controlled variable u that obtains in the step (3) _eWith the calm compensator controlled quentity controlled variable u that obtains in the step (5) ₂Differ from, obtain broad sense internal model control algorithm controls amount u=u _e-u ₂

(6) the broad sense internal model control algorithm controls amount u that obtains in the step (5) is brought at reference input r and eccentric moment T _dGo in the former speed open cycle system under the effect, realize the compensation of eccentric moment, realize that finally the floating of system is followed the tracks of.

2. the three inertially stabilized platform eccentric moments compensation method based on the broad sense internal mold according to claim 1 is characterized in that: the servo compensator system matrix A in the said step (3) _e, servo compensator control input matrix B _e, servo compensator state feedback matrix K _eSpecifically to obtain step following with the state feedback matrix K of calm compensator:

(31) at first confirm with reference to input ω _SetWith eccentric moment T _dThe common unstable model of model obtains with reference to input ω _SetWith eccentric moment T _dThe least common multiple formula of two unstable models: φ (s)=s ^l+ α _L-1s ^L-1+ ... + α ₁s ¹+ α ₀, with reference to input ω _SetWith eccentric moment T _dModel is known, then alpha ₀～α _L-1Be known quantity; L is the high-order term of φ (s), and s is a frequency domain symbol, α ₀～α _L-1Each time coefficient for φ (s);

(32) by the alpha of φ (s) ₀～α _L-1Determine blocking factor matrix Γ _L*1And β _L*1, ${\mathrm{\&Gamma;}}_{l*l}=\left[\begin{array}{cccc}0& & & \\ 0& & I_{l-1}& \\ .& & & \\ .& & & \\ .& & & \\ {\mathrm{\&alpha;}}_0& {\mathrm{\&alpha;}}_1& ...& {\mathrm{\&alpha;}}_{l-1}\end{array}\right],$ ${\mathrm{\&beta;}}_{l*1}=\left[\begin{array}{c}0\\ 0\\ .\\ .\\ .\\ 1\end{array}\right];$ Wherein, l is the high-order term of φ (s), α ₀～α _L-1Be each time coefficient of φ (s), I _L-1Be l-1 rank unit matrix;

(33) the blocking factor matrix Γ that obtains by step (32) _L*1And β _L*1, obtain the coefficient matrices A of servo compensator state space equation _eWith control input matrix B _e, wherein,

So just obtain the state space equation of servo compensator

Servo compensator state feedback matrix K _eState feedback matrix K with calm compensator;

(34) the servo compensator state space equation and the former speed open cycle system state space equation that step (33) are obtained make up, and obtain the state space equation of final cascade system: $\left[\begin{array}{c}\stackrel{\&CenterDot;}{x}\\ {\stackrel{\&CenterDot;}{x}}_e\end{array}\right]=\left[\begin{array}{cc}A& 0\\ -B_{e}C& A_e\end{array}\right]\left[\begin{array}{c}x\\ x_e\end{array}\right]+\left[\begin{array}{c}B\\ -B_{e}D\end{array}\right]u+\left[\begin{array}{c}{B}_d\\ -B_e{D}_d\end{array}\right]T_d+\left[\begin{array}{c}0\\ B_e\end{array}\right]r,$ The definition front of each coefficient provides;

(35) the cascade system state space equation that obtains in the step (34) is adopted classical POLE PLACEMENT USING u=K _Tx _TMethod is carried out POLE PLACEMENT USING, obtains state feedback matrix K ^T, wherein, $x_T={\left[\begin{array}{c}x\\ x_e\end{array}\right]}^{\&prime;};$

(36) with the state feedback matrix K that obtains in the step (35) _TDecompose, make K _T=[K K _e], obtain the state feedback matrix K of servo compensator _eState feedback matrix K with calm compensator.

Images