CN112729012A - Equivalent closed loop interference rate compensation self-stabilization control method under geodetic coordinates - Google Patents

Abstract

The invention provides an equivalent closed loop interference rate compensation self-stabilization control method under geodetic coordinates, which comprises the steps of collecting a course angle, a pitch angle and a roll angle of an SINS (strapdown inertial navigation system), and altitude and azimuth angles, altitude and azimuth motor shaft angles and angular velocities of an artillery; collecting angular velocity values of an SINS gyroscope group, a turret gyroscope group and an artillery pitching gyroscope; calculating the pointing, direction and height control strategies of the artillery barrel under the geodetic coordinate system; then, carrying out filtering correction on the interference angular rate, and combining a rate control command for stable control and an interference compensation angular rate control command as a speed total command for servo drive; and finally, calculating to obtain a current loop control instruction so as to drive the motor to control the gun turning motion according to the given control quantity. The invention can improve the stability and control precision of the control system.

Description

Equivalent closed loop interference rate compensation self-stabilization control method under geodetic coordinates

Technical Field

The invention belongs to the field of artillery stability control systems, and mainly relates to a control method of an artillery follow-up system requiring accurate stability and tracking under a high dynamic base.

Background

With the development and evolution of military combat, new military operation needs to suppress the capability of shooting weapons during advancing urgently, and self-propelled artillery needs to be capable of following the operation to the static and dynamic operation tasks. Then the artillery needs to overcome the disturbance of the vehicle body caused by the walking road surface under the control of the stabilizing system, and the directional stability of the artillery is kept. Such functions have long been realized in the gun control systems of tank weapons and amphibious assault guns. However, the firing angle of the tank gun and the amphibious assault gun is lower than 20 degrees (under a vehicle body coordinate system), the absolute speed and the turning acceleration of gun turning are not large, the direction and the height are small, the full-closed loop control is adopted, the maximum error of the direction control is 1.2mil, and the effective direct-aiming range is generally 2-3 km. In order to meet the requirement of an amphibious pressed artillery designed for aiming between more than 10km and under a high dynamic state of a movable base, the direction of an artillery barrel is directly measured by using Strapdown Inertial Navigation (SINS), aiming accuracy is ensured, the stable system of the artillery adopts the navigation attitude of the SINS as the spatial angular position feedback of the follow-up system aiming control of the artillery, a transmission mechanism is contained in a control closed ring in a nonlinear mode, and the system generates oscillation within a small control error due to the influence of nonlinear factors, so that the accuracy of a conventional firing point is seriously influenced. Therefore, the speed reducing mechanism of the type can not realize the stability of speed closed-loop control by using a gyroscope, can only realize the stability by adopting the interference rate compensation, exerts the advantage of open-loop control and is slightly influenced by the nonlinearity of the mechanism. But the fully closed loop controlled artillery realizes the stability of the artillery with the firing angle larger than 45 degrees under the geodetic coordinate system, the relative rotation speed of the azimuth artillery is close to 40 degrees/s, and the rotation angle acceleration even exceeds 160 degrees/s²The higher the acceleration is, the harder the transmission mechanism is to bear the self elastic impact to cause vibration, and the stability of the control system is influenced. To realize high-precision and stable artillery, high requirements are put forward on the transmission performance (rigidity and tooth clearance) of a system speed reducing mechanism. The performance of the speed reducer has a decisive effect on the stability and the precision of the artillery.

In summary, in the self-stabilization control system based on the full closed-loop interference rate under the geodetic coordinate system under the condition of the movable base in the prior art, the accuracy of the stabilization system is greatly influenced by the nonlinear backlash and elasticity in the transmission mechanism, and the self-stabilization control system is difficult to apply under the conditions of high speed and high angular speed.

Disclosure of Invention

In order to overcome the defects of the prior art, the invention provides an equivalent closed-loop interference rate compensation self-stabilization control method under a geodetic coordinate, which can improve the stability and the control precision of a control system.

The technical scheme adopted by the invention for solving the technical problem comprises the following steps:

(1) setting an initial value of a control step number i as 0; setting control or sampling periods, and adding 1 to the step number i of each control or sampling period;

(2) acquiring attitude heading angle psi of SINS on vehicle body_b(i),θ_b(i),

Wherein psi_b(i)、θ_b(i)、

Respectively is a course angle, a pitch angle and a roll angle of inertial navigation;

(3) collecting the measured value epsilon of high and low signal receiving instrument_b(i) And the measured value beta of the orientation receiver_b(i)；

(4) Collecting the angle value epsilon of the high and low motor shafts_m(i) And its angular velocity omega_ε(i) Azimuth motor shaft angle value beta_m(i) And its angular velocity omega_β(i)；

(5) Three-axis angular rate omega measured by gyro set for acquiring SINS_b(j)＝[ω_b1(j),ω_b2(j),ω_b3(j)]^T；

(6) Acquiring triaxial angular rate omega measured by turret gyroscope group_h(j)＝[ω_h1(j),ω_h2(j),ω_h3(j)]^T；

(7) Collecting angular rate omega measured by artillery pitching gyroscope_p(j)；

(8) Calculating the direction of the gun barrel in the space through the equivalent closed-loop angle of the height and direction signal receiving instrument;

(9) judging whether artillery aiming azimuth control command psi under geodetic coordinates is received at the same time_ref(j)、High-low control instruction theta_ref(j) If yes, entering the step (10); otherwise, turning to the step (12);

(10) calculating an azimuth position control strategy, which comprises an azimuth position control error, an azimuth speed instruction, an azimuth sliding mode equivalent control quantity, an azimuth sliding mode variable structure control quantity and an azimuth sliding mode control switching control quantity;

(11) calculating a high-low position control strategy, which comprises a high-low position control error, a high-low speed instruction, a high-low sliding mode equivalent control quantity, a high-low sliding mode variable structure control quantity and a high-low sliding mode control switching control quantity;

(12) calculating high and low compensation angular rates and azimuth interference compensation angular rates;

(13) calculating a high-low interference filtering correction value and an azimuth interference filtering correction value;

(14) calculating the total speed command of high and low servo and the total speed command of azimuth servo drive;

(15) calculating a current instruction of azimuth driving;

(16) calculating a current instruction of high and low driving;

(17) and (3) setting the driver to work in a torque mode, sending a current instruction to the servo driver through the CAN bus, and turning to the step (1).

The step (8) calculates the direction of the gun barrel in the space through the equivalent closed-loop angle of the height and direction receiver,

wherein psi (i) is the heading angle of the gun barrel, theta (i) is the pitch angle of the gun barrel,

is the roll angle, psi' of the gun barrel,

In order to calculate the intermediate variables in the process,

is a matrix of the postures of the cannon barrel,

wherein the content of the first and second substances,

is a matrix of the posture of the vehicle body,

is a vehicle body course attitude matrix,

is a matrix of the pitching attitude of the vehicle body,

is a matrix of the rolling attitude of the vehicle body,

is a matrix of the orientation and the posture of the artillery,

and the pitching attitude matrix of the artillery is obtained.

Said step (10) calculating an azimuth position control strategy,

e_β(i)＝ψ_ref(i)-ψ(i)

v_β(i)＝-k_dβsgn(s_1β)-κ_1βs_1β

z_0β(j)＝T_sv_0β+z_0β(j-1)

v_0β＝z_1β(j-1)-λ_0β|z_0lβ(j-1)-e_β(j-1)|^0.5sgn(z_0β(j-1)-e_β(j-1))

z_1β(j)＝T_sv_1β+z_1β(j-1)

v_1β＝z_1β(j-1)-λ_1β|z_1β(j-1)-v_0β|^0.5sgn(z_1β(j-1)-v_0β)

wherein: e.g. of the type_β(i) Controlling the error for the azimuth position;

is an azimuth speed instruction; u. of_pβmax,u_pβminThe upper and lower limit amplitude values of the azimuth rotating speed are obtained; j. the design is a square_1βThe azimuth turret moment of inertia; eta_βAn azimuth drive ratio; k is a radical of_βAzimuth drive stiffness; u. of_eqβ(i) Is an azimuth sliding mode equivalent control quantity; u. of_swβ(i) Azimuth sliding mode variable structure control quantity; b_βIs the azimuthal friction coefficient; alpha is alpha_βDriving the backlash for azimuth; ε (Δ β)_m) Fitting a non-linear function for the azimuth backlash; delta beta_mIs the difference between the azimuth motor shaft and the azimuth angle measurement value; beta is a_m(i) Is the azimuth motor shaft angle; c. C_1β,c_2βAre the first and second order coefficients of the orientation sliding mode, alpha_1β,α_2βRespectively are a first order index and a second order index of the azimuth sliding mode; t is_sIs a sampling period; tau is a backlash fitting coefficient; s_1βA sliding surface defined for orientation control; v. of_β(i) Controlling the switching control quantity for the direction sliding mode; z is a radical of_0β(j),z_1β(j),z_2β(j) Respectively, azimuth position control error e_β(i) 0, first, second order estimate of (lambda)_0β,λ_1β,λ_2βRespectively 0 th, first and second order coefficients, v, of its state estimate_0β,v_1βRespectively, intermediate variables in the state estimation; t is_βIs the azimuth filter time constant; k is a radical of_dβIs the orientation sliding mode stability factor, kappa_1βIs the orientation sliding mode convergence coefficient.

The step (11) calculates the high-low position control strategy

e_ε(i)＝θ_ref(i)-θ(i)

v_ε(i)＝-k_dεsgn(s_1ε)-κ_1εs_1ε

z_0ε(j)＝T_sv_0ε+z_0ε(j-1)

v_0ε＝z_1ε(j-1)-λ_0ε|z_0lε(j-1)-e_ε(j-1)|^0.5sgn(z_0ε(j-1)-e_ε(j-1))

z_1ε(j)＝T_sv_1ε+z_1ε(j-1)

v_1ε＝z_1ε(j-1)-λ_1ε|z_1ε(j-1)-v_0ε|^0.5sgn(z_1ε(j-1)-v_0ε)

z_2ε(j)＝T_s[-λ_2εsgn(z_2ε(j-1)-v_1ε)]+z_2ε(j-1)

Wherein: e.g. of the type_ε(i) Controlling the error for the high and low positions;

a high and low speed command; u. of_pεmax,u_pεminThe amplitude values of the upper limit and the lower limit of the high and low rotating speeds are obtained; j. the design is a square_1εHigh and low turret moment of inertia; eta_εThe transmission speed ratio is high and low; k is a radical of_εHigh and low transmission stiffness; u. of_eqε(i) The method comprises the steps of (1) obtaining equivalent control quantity of a high-low sliding mode; u. of_swε(i) Variable structure control variable of high and low sliding modes; b_εHigh and low friction coefficients; alpha is alpha_εThe gear backlash is high-low transmission gear backlash; ε (Δ ε)_m) Fitting a nonlinear function for the high and low backlash; epsilon_m(i) The angle of the motor shaft is high or low; delta epsilon_mThe angle difference between the high and low motor shafts and the high and low angle measurement is obtained; c. C_1ε,c_2εRespectively a first and a second order coefficient of high and low sliding modes, alpha_1ε,α_2εRespectively a first-order index and a second-order index of the sliding mode; s_1εA sliding mode surface defined for high-low control; v. of_ε(i) Controlling switching control quantity for high and low sliding modes;

z_0ε(j),z_1ε(j),z_2ε(j) respectively a high and low position control error e_ε(i) 0, first, second order estimate of (lambda)_0ε,λ_1ε,λ_2εRespectively 0 th, first and second order coefficients, v, of its state estimate_0ε,v_1εRespectively, intermediate variables in the state estimation; t is_εHigh and low filter time constants; k is a radical of_dεIs a high and low slip form stability factor, kappa_1εThe convergence coefficients of high and low sliding modes.

The step (12) calculates the high-low compensation angular rate d_ε(j) Compensating angular rate d for sum azimuth interference_β(j)；

The step (13) calculates the high-low interference filtering correction amount u_dε(j) Sum-of-azimuth interference filter correction u_dβ(j)；

u_dε(j)＝c₁₁d_ε(j)+c₁₂d_ε(j-1)-d₁₁u_dε(j-1)

u_dβ(j)＝c₂₁d_β(j)+c₂₂d_β(j-1)-d₂₁u_dβ(j-1)

Wherein, c₁₁,c₁₂,d₁₁Correcting coefficients for high and low interference filtering; c. C₂₁,c₂₂,d₂₁Correcting coefficients for the azimuth interference filter;

wherein: t is_sA speed control period; t is_ε1，T_β1Respectively, high-low and azimuth filtering time coefficients; t is_ε2，T_β2Respectively, a high-low time characteristic constant and an azimuth time characteristic constant; k is a radical of_ε1，k_β1Respectively high and low and an azimuth gain constant.

The step (14) calculates the total speed command of high and low servo

And the overall velocity command of the azimuth servo drive

The step (15) of calculating the current command of the azimuth drive

z_0lβ(j)＝T_sv_0lβ+z_0lβ(j-1)

v_0lβ＝z_1lβ(j-1)-λ_0lβ|z_0lβ(j-1)-l_eβ(j-1)|^0.5sgn(z_0lβ(j-1)-l_eβ(j-1))

z_0ωβ(j)＝T_sv_0ωβ+z_0ωβ(j-1)

l_eβ＝e_βω+γz_1eβ(j)^p/q

z_0eβ(j)＝T_sv_0eβ+z_0eβ(j-1)

v_0eβ＝z_1eβ(j-1)-λ_0eβ|z_0eβ(j-1)-e_βω(j-1)|^0.5sgn(z_0eβ(j-1)-e_βω(j-1))

Wherein: i.e. i_qeqβ,i_qnβRespectively an azimuth terminal sliding mode equivalent control quantity and a sliding mode integral control quantity; i.e. i_qβmax,i_qβminRespectively are azimuth current amplitude limiting values; j. the design is a square_βThe azimuth motor load moment of inertia; p is a radical of_βThe number of pole pairs of the azimuth motor is; psi_fβThe azimuth motor flux linkage coefficient; b is_βThe comprehensive viscous friction coefficient of the azimuth system; gamma ray_β,q_β,P_βA position terminal sliding mode coefficient; k is a radical of_β,η_β1,η_β2A direction terminal sliding mode control coefficient; z is a radical of_0lβ(j)，z_1lβ(j) Respectively, terminal sliding form_eβ(j) The estimated value and the first-order estimated value of (c); lambda [ alpha ]_0lβ，λ_1lβAre each l_eβ(j) Estimated 0 th and first order estimation coefficients; z is a radical of_0ωβ(j)，z_1ωβ(j) Are respectively the direction and speed commands

The estimated value and the first-order estimated value of (c); lambda [ alpha ]_0ωβ，λ_1ωβAre respectively as

Estimated 0 th and first order estimation coefficients; z is a radical of_0eβ(j)，z_1eβ(j) Respectively, an azimuth velocity control error e_βω(j) The estimated value and the first-order estimated value of (c); lambda [ alpha ]_0eβ，λ_1eβAre each e_βω(j) Estimated 0 th and first order estimation coefficients; v. of_0lβ,v_0ωβ,v_0eβRespectively, intermediate variables of the orientation state estimate.

The step (16) calculates the current instruction of high-low driving

z_0lε(j)＝T_sv_0lε+z_0lε(j-1)

v_0lε＝z_1lε(j-1)-λ_0lε|z_0lε(j-1)-l_eε(j-1)|^0.5sgn(z_0lε(j-1)-l_eε(j-1))

z_1lε(j)＝T_s[-λ_lε1sgn(z_1lε(j-1)-v_0lε]+z_1lε(j-1)

z_0ωε(j)＝T_sv_0ωε+z_0ωε(j-1)

z_1ωε(j)＝T_s[-λ_1ωεsgn(z_1ωε(j-1)-v_0ωε)]+z_1ωε(j-1)

l_eε＝e_εω+γz_1eε(j)^p/q

z_0eε(j)＝T_sv_0eε+z_0eε(j-1)

v_0eε＝z_1eε(j-1)-λ_0eε|z_0eε(j-1)-e_εω(j-1)|^0.5sgn(z_0eε(j-1)-e_εω(j-1))

z_1eε(j)＝T_s[-λ_1eεsgn(z_1eε(j-1)-v_0eε)]+z_1eε(j-1)

Wherein: i.e. i_qeqε,i_qnεRespectively obtaining high and low terminal sliding mode equivalent control quantity and sliding mode integral control quantity; i.e. i_qεmax,i_qεminRespectively high and low current limiting values; j. the design is a square_εThe load moment of inertia of the high-low motor is obtained; p is a radical of_εThe number of the pole pairs of the square high-low motor is counted; psi_fεThe flux linkage coefficient of the high-low motor is obtained; b is_εHigh and low system comprehensive viscous friction coefficients; gamma ray_ε,q_ε,P_εHigh-low terminal sliding mode coefficients; k is a radical of_ε,η_ε1,η_ε2A high-low terminal sliding mode control coefficient; z is a radical of_0lε(j)，z_1lε(j) Respectively a high-low terminal sliding form_eε(j) The estimated value and the first-order estimated value of (c); lambda [ alpha ]_0lε，λ_1lεAre each l_eε(j) Estimated 0 th and first order estimation coefficients; z is a radical of_0ωε(j)，z_1ωε(j) Respectively high and low speed commands

The estimated value and the first-order estimated value of (c); lambda [ alpha ]_0ωε，λ_1ωεAre respectively as

Estimated 0 th and first order estimation coefficients; z is a radical of_0eε(j)，z_1eε(j) Respectively high and low speed control error e_εω(j) The estimated value and the first-order estimated value of (c); lambda [ alpha ]_0eε，λ_1eεAre each e_εω(j) Estimated 0 th and first order estimation coefficients; v. of_0lε,v_0ωε,v_0eεRespectively, the intermediate variables of the high and low state estimation.

The invention has the beneficial effects that: the stability control method ensures that the suppression control of the stability system on the interference is completely determined by the sensitive interference rate, has high response bandwidth and accurate interference rate compensation, effectively overcomes the interference of the carrier attitude on the directional control of the gun barrel, and realizes the high-precision stability control of the gun barrel directional under the condition of large firing angle of the movable base. Meanwhile, the backlash is compensated by adopting backstepping high-order sliding mode control, the position stable aiming control error is reduced, the error characteristic is improved, a shooting window is convenient to implement, and the shooting precision is improved. The control model can be expanded to stable tracking control under the condition of high dynamic large firing angle of the self-propelled antiaircraft gun, and can adapt to the control application occasions of large turning speed and acceleration.

Drawings

FIG. 1 is a control schematic of the present invention;

FIG. 2 is a diagram of the control transfer function architecture of the present invention;

FIG. 3 is a computational flow diagram of the present invention.

Detailed Description

The present invention will be further described with reference to the following drawings and examples, which include, but are not limited to, the following examples.

Aiming at the fact that a control system is sensitive to tooth gaps and elasticity, the SINS is installed on a chassis of a vehicle body, a turret gyro set is still installed on a turret, and the vehicle body gyro which is originally stable in a closed loop is installed on a cannon cradle and pitching along with the cannon. The height and direction signal receiving instrument indirectly measures the height and azimuth angle (also called semi-closed loop) of the gun in a vehicle body coordinate system in an equivalent closed loop mode to control, and calculates the direction of the gun barrel in a geodetic coordinate system, so that a control system excludes a larger tooth gap and an elastic structure resonance part of a transmission mechanism from a control closed loop. Meanwhile, the control method adopts a backstepping high-order sliding mode backlash compensation control method, so that the stability of the control system is stronger, and the stability control precision is further improved. The method comprises the following steps:

(1) starting control, controlling the number of steps i, i.e. controlling or sampling period T_sAdding 1 to the step number i for 1ms, wherein the initial value of i is 0;

(2) acquiring attitude heading angle psi of SINS (strapdown inertial navigation System) arranged on vehicle body_b(i),θ_b(i),

Wherein psi_b(i) The course angle of inertial navigation is obtained; theta_b(i) A pitch angle of inertial navigation;

the roll angle of inertial navigation;

(3) collecting the measured value epsilon of high and low signal receiving instrument_b(i) And the measured value beta of the orientation receiver_b(i)；

(4) Collecting the angle value epsilon of the high and low motor shafts_m(i) And ω_ε(i) Angular velocity, azimuth motor shaft angle value beta_m(i) And angular velocity ω_β(i)；

(5) Acquisition of the three-axis angular rate omega measured by a gyro group of the SINS_b(j) Wherein ω is_b(j)＝[ω_b1(j),ω_b2(j),ω_b3(j)]^T，ω_b1(j),ω_b2(j),ω_b3(j) Respectively measuring the angular rate gyros of the axis 1, the axis 2 and the axis 3 of the inertial navigation three-axis gyroscope;

(6) acquiring the three-axis angular rate omega measured by a turret gyroscope group_h(j) Wherein ω is_h(j)＝[ω_h1(j),ω_h2(j),ω_h3(j)]^T，ω_h1(j),ω_h2(j),ω_h3(j) Three-axis of turret gyroscopeMeasurement of gyros of angular rate of axes 1, 2, 3 of the gyro;

(7) collecting angular rate omega measured by artillery pitching gyroscope_p(j)；

(8) Calculating the direction of the gun barrel in the space through the equivalent closed-loop angle of the height and direction signal receiving instrument;

wherein psi (i) is the heading angle of the gun barrel; theta (i) is the pitch angle of the gun barrel;

the roll angle, psi' of the gun barrel,

respectively intermediate variables in the calculation process;

is a matrix of the orientation and the posture of the artillery,

is a pitching attitude matrix of the artillery,

wherein:

is a vehicle body attitude matrix;

is a gun barrel attitude matrix;

is a vehicle body course attitude matrix,

is a matrix of the pitching attitude of the vehicle body,

a vehicle body roll attitude matrix;

(9) judging whether a gun aiming azimuth control instruction psi under geodetic coordinates is received_ref(j) High-low control command theta_ref(j) If yes, entering the step (10); otherwise, turning to the step (12);

(10) position location control strategy calculation

e_β(i)＝ψ_ref(i)-ψ(i)

v_β(i)＝-k_dβsgn(s_1β)-κ_1βs_1β

z_0β(j)＝T_sv_0β+z_0β(j-1)

v_0β＝z_1β(j-1)-λ_0β|z_0lβ(j-1)-e_β(j-1)|^0.5sgn(z_0β(j-1)-e_β(j-1))

z_1β(j)＝T_sv_1β+z_1β(j-1)

v_1β＝z_1β(j-1)-λ_1β|z_1β(j-1)-v_0β|^0.5sgn(z_1β(j-1)-v_0β)

Wherein: e.g. of the type_β(i) Controlling the error for the azimuth position;

is an azimuth speed instruction; u. of_pβmax,u_pβminThe upper and lower limit amplitude values of the azimuth rotating speed are obtained; j. the design is a square_1βThe azimuth turret moment of inertia; eta_βAn azimuth drive ratio; k is a radical of_βAzimuth drive stiffness; u. of_eqβ(i) Is an azimuth sliding mode equivalent control quantity; u. of_swβ(i) Azimuth sliding mode variable structure control quantity; b_βIs the azimuthal friction coefficient; alpha is alpha_βDriving the backlash for azimuth; ε (Δ β)_m) Fitting a non-linear function for the azimuth backlash; delta beta_mIs the difference between the azimuth motor shaft and the azimuth angle measurement value; beta is a_m(i) Is the azimuth motor shaft angle; c. C_1β,c_2βAre the first and second order coefficients of the orientation sliding mode, alpha_1β,α_2βRespectively a first-order index and a second-order index of the sliding mode; t is_sIs a sampling period; tau is a fitting coefficient; s_1βA sliding surface defined for orientation control; v. of_β(i) Controlling the switching control quantity for the direction sliding mode; z is a radical of_0β(j),z_1β(j),z_2β(j) Respectively, azimuth position control error e_β(i) 0, first, second order estimate of (lambda)_0β,λ_1β,λ_2βRespectively 0 th, first and second order coefficients, v, of its state estimate_0β,v_1βRespectively, intermediate variables in the state estimation; t is_βIs the azimuth filter time constant; k is a radical of_dβIs a high and low slip form stability factor, kappa_1βThe convergence coefficients of high and low sliding modes.

(11) High and low position control strategy calculation

e_ε(i)＝θ_ref(i)-θ(i)

v_ε(i)＝-k_dεsgn(s_1ε)-κ_1εs_1ε

z_0ε(j)＝T_sv_0ε+z_0ε(j-1)

v_0ε＝z_1ε(j-1)-λ_0ε|z_0lε(j-1)-e_ε(j-1)|^0.5sgn(z_0ε(j-1)-e_ε(j-1))

z_1ε(j)＝T_sv_1ε+z_1ε(j-1)

v_1ε＝z_1ε(j-1)-λ_1ε|z_1ε(j-1)-v_0ε|^0.5sgn(z_1ε(j-1)-v_0ε)

z_2ε(j)＝T_s[-λ_2εsgn(z_2ε(j-1)-v_1ε)]+z_2ε(j-1)

Wherein: e.g. of the type_ε(i) Controlling the error for the high and low positions;

a high and low speed command; u. of_pεmax,u_pεminThe amplitude values of the upper limit and the lower limit of the high and low rotating speeds are obtained; j. the design is a square_1εHigh and low turret moment of inertia; eta_εThe transmission speed ratio is high and low; k is a radical of_εHigh and low transmission stiffness; u. of_eqε(i) The method comprises the steps of (1) obtaining equivalent control quantity of a high-low sliding mode; u. of_swε(i) Variable structure control variable of high and low sliding modes; b_εHigh and low friction coefficients; alpha is alpha_εThe gear backlash is high-low transmission gear backlash; ε (Δ ε)_m) Fitting a nonlinear function for the high and low backlash; epsilon_m(i) The angle of the motor shaft is high or low; delta epsilon_mThe angle difference between the high and low motor shafts and the high and low angle measurement is obtained; c. C_1ε,c_2εRespectively a first and a second order coefficient of high and low sliding modes, alpha_1ε,α_2εRespectively a first-order index and a second-order index of the sliding mode; s_1εA sliding mode surface defined for high-low control; v. of_ε(i) Controlling switching control quantity for high and low sliding modes;

z_0ε(j),z_1ε(j),z_2ε(j) respectively a high and low position control error e_ε(i) 0, first, second order estimate of (lambda)_0ε,λ_1ε,λ_2εRespectively 0 th, first and second order coefficients, v, of its state estimate_0ε,v_1εRespectively, intermediate variables in the state estimation; t is_εHigh and low filter time constants; k is a radical of_dεIs a high and low slip form stability factor, kappa_1εThe convergence coefficients of high and low sliding modes. .

(12) Calculating high and low compensation angular rate d_ε(j) Compensating angular rate d for sum azimuth interference_β(j)；

(13) High-low interference filtering correction value u_dε(j) Sum-of-azimuth interference filter correction u_dβ(j) Calculating;

u_dε(j)＝c₁₁d_ε(j)+c₁₂d_ε(j-1)-d₁₁u_dε(j-1)

u_dβ(j)＝c₂₁d_β(j)+c₂₂d_β(j-1)-d₂₁u_dβ(j-1)

wherein, c₁₁,c₁₂,d₁₁Correcting coefficients for high and low interference filtering; c. C₂₁,c₂₂,d₂₁Correcting coefficients for the azimuth interference filter;

wherein: t is_sA speed control period; t is_ε1，T_β1Respectively, high-low and azimuth filtering time coefficients; t is_ε2，T_β2Respectively, a high-low time characteristic constant and an azimuth time characteristic constant; k is a radical of_ε1，k_β1Respectively high and low and an azimuth gain constant.

(14) Calculating total speed command of high and low servo

And the overall velocity command of the azimuth servo drive

(15) Calculating azimuth-driven current commands

z_0lβ(j)＝T_sv_0lβ+z_0lβ(j-1)

v_0lβ＝z_1lβ(j-1)-λ_0lβ|z_0lβ(j-1)-l_eβ(j-1)|^0.5sgn(z_0lβ(j-1)-l_eβ(j-1))

z_0ωβ(j)＝T_sv_0ωβ+z_0ωβ(j-1)

l_eβ＝e_βω+γz_1eβ(j)^p/q

z_0eβ(j)＝T_sv_0eβ+z_0eβ(j-1)

v_0eβ＝z_1eβ(j-1)-λ_0eβ|z_0eβ(j-1)-e_βω(j-1)|^0.5sgn(z_0eβ(j-1)-e_βω(j-1))

Wherein: i.e. i_qeqβ,i_qnβRespectively an azimuth terminal sliding mode equivalent control quantity and a sliding mode integral control quantity; i.e. i_qβmax,i_qβminRespectively are azimuth current amplitude limiting values; j. the design is a square_βThe azimuth motor load moment of inertia; p is a radical of_βThe number of pole pairs of the azimuth motor is; psi_fβThe azimuth motor flux linkage coefficient; b is_βThe comprehensive viscous friction coefficient of the azimuth system; gamma ray_β,q_β,P_βA position terminal sliding mode coefficient; k is a radical of_β,η_β1,η_β2A direction terminal sliding mode control coefficient; z is a radical of_0lβ(j)，z_1lβ(j) Respectively, terminal sliding form_eβ(j) The estimated value and the first-order estimated value of (c); lambda [ alpha ]_0lβ，λ_1lβAre each l_eβ(j) Estimated 0 th and first order estimation coefficients; z is a radical of_0ωβ(j)，z_1ωβ(j) Are respectively the direction and speed commands

The estimated value and the first-order estimated value of (c); lambda [ alpha ]_0ωβ，λ_1ωβAre respectively as

Estimated 0 th and first order estimation coefficients; z is a radical of_0eβ(j)，z_1eβ(j) Respectively, an azimuth velocity control error e_βω(j) The estimated value and the first-order estimated value of (c); lambda [ alpha ]_0eβ，λ_1eβAre each e_βω(j) Estimated 0 th and first order estimation coefficients; v. of_0lβ,v_0ωβ,v_0eβRespectively, intermediate variables of the orientation state estimate.

(16) Calculating current command of high-low driving

z_0lε(j)＝T_sv_0lε+z_0lε(j-1)

v_0lε＝z_1lε(j-1)-λ_0lε|z_0lε(j-1)-l_eε(j-1)|^0.5sgn(z_0lε(j-1)-l_eε(j-1))

z_1lε(j)＝T_s[-λ_lε1sgn(z_1lε(j-1)-v_0lε]+z_1lε(j-1)

z_0ωε(j)＝T_sv_0ωε+z_0ωε(j-1)

z_1ωε(j)＝T_s[-λ_1ωεsgn(z_1ωε(j-1)-v_0ωε)]+z_1ωε(j-1)

l_eε＝e_εω+γz_1eε(j)^p/q

z_0eε(j)＝T_sv_0eε+z_0eε(j-1)

v_0eε＝z_1eε(j-1)-λ_0eε|z_0eε(j-1)-e_εω(j-1)|^0.5sgn(z_0eε(j-1)-e_εω(j-1))

z_1eε(j)＝T_s[-λ_1eεsgn(z_1eε(j-1)-v_0eε)]+z_1eε(j-1)

Wherein: i.e. i_qeqε,i_qnεRespectively obtaining high and low terminal sliding mode equivalent control quantity and sliding mode integral control quantity; i.e. i_qεmax,i_qεminRespectively high and low current limiting values; j. the design is a square_εThe load moment of inertia of the high-low motor is obtained; p is a radical of_εThe number of the pole pairs of the square high-low motor is counted; psi_fεThe flux linkage coefficient of the high-low motor is obtained; b is_εHigh and low system comprehensive viscous friction coefficients; gamma ray_ε,q_ε,P_εHigh-low terminal sliding mode coefficients; k is a radical of_ε,η_ε1,η_ε2A high-low terminal sliding mode control coefficient; z is a radical of_0lε(j)，z_1lε(j) Respectively a high-low terminal sliding form_eε(j) The estimated value and the first-order estimated value of (c); lambda [ alpha ]_0lε，λ_1lεAre each l_eε(j) Estimated 0 th and first order estimation coefficients; z is a radical of_0ωε(j)，z_1ωε(j) Respectively high and low speed commands

The estimated value and the first-order estimated value of (c); lambda [ alpha ]_0ωε，λ_1ωεAre respectively as

Estimated 0 th and first order estimation coefficients; z is a radical of_0eε(j)，z_1eε(j) Respectively high and low speed control error e_εω(j) The estimated value and the first-order estimated value of (c); lambda [ alpha ]_0eε，λ_1eεAre each e_εω(j) Estimated 0 th and first order estimation coefficients; v. of_0lε,v_0ωε,v_0eεRespectively, the intermediate variables of the high and low state estimation.

(17) And (3) setting the driver to work in a torque mode, sending a current instruction to the servo driver through the CAN bus, and turning to the step (1).

The control principle of the embodiment of the invention is shown in figure 1. In the figure, the height and direction receiver is connected with the motor shaft through an instrument chain with the same power transmission ratio, and the instrument chain has larger transmission rigidity and smaller backlash. The pointing direction of the gun barrel in the geodetic coordinate system is calculated by the navigation attitude, the height and the measured value of the receiving instrument of the strapdown inertial navigation installed on the chassis of the vehicle body, and the pointing value of the gun barrel is used as the feedback of the aiming position control loop, so that the aiming of the gun barrel can be realized. Meanwhile, the non-linearity such as backlash of power transmission is excluded from position closed-loop control. The stabilizing system can calculate the azimuth and the high-low interference angular rate of the cannon under the geodetic coordinates according to the angular rate, the high-low angle, the turret gyro set and the cannon pitching gyro of the SINS gyro set. The stabilizing system eliminates the interference of a high control channel, a low control channel and an azimuth control channel through the cascade control of the position control outer ring, thereby achieving ideal stable aiming precision.

The control method comprises the following steps: firstly, collecting a course angle, a pitch angle and a roll angle of SINS, and a high-low angle and an azimuth angle, a high-low and azimuth motor shaft angle and angular speed of an artillery; then, collecting angular velocity values of the SINS gyroscope group, the turret gyroscope group and the artillery pitching gyroscope; secondly, calculating the direction of the gun barrel under a geodetic coordinate system; thirdly, calculating a direction and a height control strategy; thirdly, carrying out filtering correction on the interference angular rate to obtain azimuth and high-low compensation angular rate control quantity; thirdly, combining a speed control command of stable control and an interference compensation angular speed control command as a speed total command of servo drive; and finally, calculating a high-order sliding mode speed control strategy according to the speed control instruction to obtain a current loop control instruction, so that the driving motor controls the gun to rotate according to the given control quantity.

The control transfer function structure of an embodiment of the present invention is shown in fig. 2. To simplify the transfer function, the SINS course and pitch measurements can be reduced by

Treating the coupling interference as various passing interferences; simplifying a closed loop formed by a current controller, an inverter, current conditioning, a current moment coefficient and the like driven by high-low and azimuth servo into a first-order inertia link

The direction and high and low speed ring control is high-order sliding mode control; the direction and high-low position ring control is also high-order sliding mode control; the disturbance rate feedforward controller is

f_dβ,f_dεRespectively, a gain factor, T_dβ,T_dεRespectively, time constants, discretized using a bi-linear transformation.

The gun stable aiming system implementing the control method mainly comprises a stable control system, a driving speed regulation system, a power supply system, a turret gyro set, a vehicle body gyro, a height and azimuth side angle device and the like. The stable aiming control system adopts an embedded computer based on x 86. The driving speed regulation system takes a DSP28335+ FPGA as a core control panel to drive a power electronic IPM (intelligent drive) to control the motor to rotate. A Permanent Magnet Synchronous Motor (PMSM) with a bus voltage of 56VDC and a number n of pole pairs_p3, rated power of 4kW, stator inductance of 0.0098mH, stator resistance of 3.5 milliohm, rated rotating speed of 3000RPM, rated torque of 7.4Nm, and equivalent moment of inertia J sum of motor rotor and transmission gear train of 0.013 kg.m²(ii) a High and low PMSM (permanent magnet synchronous Motor), the bus voltage is 56VDC, and the number n of pole pairs_p3, rated power of 2kW, rated torque of 3.2Nm, stator inductance of 0.032mH, stator resistance of 0.0105 ohm, rated rotating speed of 3000RPM, and equivalent moment of inertia J sum of motor rotor and transmission gear train of 0.0075 kg.m². The azimuthal load moment of inertia is about 2700kg m²The transmission ratio is 470. The high and low load moment of inertia is 700 kg.m². The transmission ratio is 450. The angular speed measurement range of the SINS is +/-300 degrees/s, the course measurement precision is not more than 0.3mil, and the attitude measurement precision is not more than 0.1 mil.

Fig. 3 is a flowchart of calculation according to an embodiment of the present invention, and details of a specific implementation process will be described below with reference to the flowchart.

(1) Starting control, controlling the number of steps i, i.e. controlling or sampling period T_s＝1ms

i＝i+1

Wherein the initial value of i is 0;

(2) acquiring attitude heading angle psi of SINS (strapdown inertial navigation System) arranged on vehicle body_b(i),θ_b(i),

Wherein psi_b(i) The course angle of inertial navigation is obtained; theta_b(i) A pitch angle of inertial navigation;

the roll angle of inertial navigation;

(3) collecting the measured value epsilon of high and low signal receiving instrument_b(i) And the measured value beta of the orientation receiver_b(i)；

(4) Collecting the angle value epsilon of the high and low motor shafts_m(i) And ω_ε(i) Angular velocity, azimuth motor shaft angle value beta_m(i) And angular velocity ω_β(i)；

(5) Acquisition of the three-axis angular rate omega measured by a gyro group of the SINS_b(j) Wherein ω is_b(j)＝[ω_b1(j),ω_b2(j),ω_b3(j)]^T，ω_b1(j),ω_b2(j),ω_b3(j) Respectively measuring the angular rate gyros of the axis 1, the axis 2 and the axis 3 of the inertial navigation three-axis gyroscope;

(6) acquiring the three-axis angular rate omega measured by a turret gyroscope group_h(j) Wherein ω is_h(j)＝[ω_h1(j),ω_h2(j),ω_h3(j)]^T，ω_h1(j),ω_h2(j),ω_h3(j) Measuring values of the three-axis gyroscope of the turret gyroscope, namely a shaft 1, a shaft 2 and a shaft 3, of angular rate gyroscopes respectively;

(7) collecting angular rate omega measured by artillery pitching gyroscope_p(j)；

(8) Calculating the direction of the gun barrel in the space through the equivalent closed-loop angle of the height and direction signal receiving instrument;

converting the amplitude into a secret bit;

(9) whether a gun aiming azimuth control command psi under geodetic coordinates is received_ref(j) High-low control command theta_ref(j) Is there a If yes, entering the step (10); otherwise, turning to the step (12);

(10) position location control strategy calculation

e_β(i)＝ψ_ref(i)-ψ(i)

v_β(i)＝-k_dβsgn(s_1β)-κ_1βs_1β

z_0β(j)＝T_sv_0β+z_0β(j-1)

v_0β＝z_1β(j-1)-λ_0β|z_0lβ(j-1)-e_β(j-1)|^0.5sgn(z_0β(j-1)-e_β(j-1))

z_1β(j)＝T_sv_1β+z_1β(j-1)

v_1β＝z_1β(j-1)-λ_1β|z_1β(j-1)-v_0β|^0.5sgn(z_1β(j-1)-v_0β)

Wherein: given azimuth motor rated speed u_pβmax＝3000，u_pβmin-3000; turret moment of inertia J_1β2700 is selected; given aSpeed ratio η_β470; rigidity k_β＝2×10⁷(ii) a Coefficient of friction b_β0.005; taking the measured value of backlash alpha_β1.2. The following design parameters: c. C_1β＝15，c_2β＝9，τ＝27，λ_0β＝5，λ_1β＝21，λ_2β＝250，T_β＝0.001，k_dβ＝15，κ_1β＝150。

(11) High and low position control strategy calculation

e_ε(i)＝θ_ref(i)-θ(i)

v_ε(i)＝-k_dεsgn(s_1ε)-κ_1εs_1ε

z_0ε(j)＝T_sv_0ε+z_0ε(j-1)

v_0ε＝z_1ε(j-1)-λ_0ε|z_0lε(j-1)-e_ε(j-1)|^0.5sgn(z_0ε(j-1)-e_ε(j-1))

z_1ε(j)＝T_sv_1ε+z_1ε(j-1)

v_1ε＝z_1ε(j-1)-λ_1ε|z_1ε(j-1)-v_0ε|^0.5sgn(z_1ε(j-1)-v_0ε)

z_2ε(j)＝T_s[-λ_2εsgn(z_2ε(j-1)-v_1ε)]+z_2ε(j-1)

Wherein: setting the rated speed u of the motor_pεmax＝3000，u_pεmin-3000; cannon pitching inertia J_1ε700; given speed ratio η_ε450; rigidity k_ε＝1.8×10⁷(ii) a Coefficient of friction b_ε0.008 percent; taking the measured value of backlash alpha_ε0.9. The following design parameters: c. C_1ε＝23，c_2ε＝5，τ＝27，λ_0ε＝8.15，λ_1ε＝36.7，λ_2ε＝430，T_ε＝0.0013，k_dε＝17.1，，κ_1ε＝203。

(12) Calculating high and low compensation angular rate d_ε(j) Compensating angular rate d for sum azimuth interference_β(j)；

(13) High-low interference filtering correction value u_dε(j) Sum-of-azimuth interference filter correction u_dβ(j) Calculating;

u_dε(j)＝c₁₁d_ε(j)+c₁₂d_ε(j-1)-d₁₁u_dε(j-1)

u_dβ(j)＝c₂₁d_β(j)+c₂₂d_β(j-1)-d₂₁u_dβ(j-1)

wherein, c₁₁,c₁₂,d₁₁Correcting coefficients for high and low interference filtering; c. C₂₁,c₂₂,d₂₁Correcting coefficients for the azimuth interference filter;

wherein: t is_s＝0.001；T_ε1＝0.01，T_β1＝0.01；T_ε2＝0.025，T_β2＝0.036；k_ε1＝4.5，k_β1＝4.7；

(14) Calculating total speed command of high and low servo

And the overall velocity command of the azimuth servo drive

(15) Calculating azimuth-driven current commands

z_0lβ(j)＝T_sv_0lβ+z_0lβ(j-1)

v_0lβ＝z_1lβ(j-1)-λ_0lβ|z_0lβ(j-1)-l_eβ(j-1)|^0.5sgn(z_0lβ(j-1)-l_eβ(j-1))

z_0ωβ(j)＝T_sv_0ωβ+z_0ωβ(j-1)

l_eβ＝e_βω+γz_1eβ(j)^p/q

z_0eβ(j)＝T_sv_0eβ+z_0eβ(j-1)

v_0eβ＝z_1eβ(j-1)-λ_0eβ|z_0eβ(j-1)-e_βω(j-1)|^0.5sgn(z_0eβ(j-1)-e_βω(j-1))

Wherein: given azimuth motor current limit value i_qβmax＝150,i_qβmin-150; azimuth motor shaft J_β0.013, its pole pair number p_β3, its flux linkage coefficient psi_fβ0.0031, coefficient of friction B_β0.005. The design parameters are as follows: gamma ray_β＝0.02，q_β＝3，P_β＝5；k_β＝250，η_β1＝300，η_β2＝2000；λ_0lβ＝5.321，λ_1lβ＝15.487；λ_0ωβ＝2.56，λ_1ωβ＝11.654；λ_0eβ＝7.662，λ_1eβ＝18.956。

(16) Calculating current command of high-low driving

z_0lε(j)＝T_sv_0lε+z_0lε(j-1)

v_0lε＝z_1lε(j-1)-λ_0lε|z_0lε(j-1)-l_eε(j-1)|^0.5sgn(z_0lε(j-1)-l_eε(j-1))

z_1lε(j)＝T_s[-λ_lε1sgn(z_1lε(j-1)-v_0lε]+z_1lε(j-1)

z_0ωε(j)＝T_sv_0ωε+z_0ωε(j-1)

z_1ωε(j)＝T_s[-λ_1ωεsgn(z_1ωε(j-1)-v_0ωε)]+z_1ωε(j-1)

l_eε＝e_εω+γz_1eε(j)^p/q

z_0eε(j)＝T_sv_0eε+z_0eε(j-1)

v_0eε＝z_1eε(j-1)-λ_0eε|z_0eε(j-1)-e_εω(j-1)|^0.5sgn(z_0eε(j-1)-e_εω(j-1))

z_1eε(j)＝T_s[-λ_1eεsgn(z_1eε(j-1)-v_0eε)]+z_1eε(j-1)

Wherein: giving high and low motor current limiting value i_qεmax＝75,i_qεmin-75; high-low motor shaft J_ε0.0075, its number of pole pairs p_ε3, its flux linkage coefficient psi_fε0.02, given a coefficient of friction B_ε0.008. The design parameters are as follows: gamma ray_ε＝0.01，q_ε＝5，P_ε＝7；k_ε＝300，η_ε1＝450，η_ε2＝1500；λ_0lε＝4.365，λ_1lε＝13.78；λ_0ωε＝5.862，λ_1ωε＝14.953；λ_0eε＝7.632，λ_1eε＝19.564。

(17) And (3) setting the driver to work in a torque mode, sending a current instruction to the servo driver through the CAN bus, and turning to the step (1).

The ranges of parameters used in this example are shown in Table 1.

TABLE 1 parameter value ranges

Claims (9)

1. An equivalent closed loop interference rate compensation self-stabilization control method under geodetic coordinates is characterized by comprising the following steps:

(1) setting an initial value of a control step number i as 0; setting control or sampling periods, and adding 1 to the step number i of each control or sampling period;

(2) acquiring attitude heading angle psi of SINS on vehicle body_b(i),θ_b(i),

Wherein psi_b(i)、θ_b(i)、

Respectively is a course angle, a pitch angle and a roll angle of inertial navigation;

(3) collecting the measured value epsilon of high and low signal receiving instrument_b(i) And the measured value beta of the orientation receiver_b(i)；

(4) Collecting the angle value epsilon of the high and low motor shafts_m(i) And its angular velocity omega_ε(i) Azimuth motor shaft angle value beta_m(i) And its angular velocity omega_β(i)；

(5) Three-axis angular rate omega measured by gyro set for acquiring SINS_b(j)＝[ω_b1(j),ω_b2(j),ω_b3(j)]^T；

(6) Acquiring triaxial angular rate omega measured by turret gyroscope group_h(j)＝[ω_h1(j),ω_h2(j),ω_h3(j)]^T；

(7) Collecting angular rate omega measured by artillery pitching gyroscope_p(j)；

(8) Calculating the direction of the gun barrel in the space through the equivalent closed-loop angle of the height and direction signal receiving instrument;

(9) judging whether artillery aiming azimuth control command psi under geodetic coordinates is received at the same time_ref(j) High-low control command theta_ref(j) If yes, entering the step (10); otherwise, turning to the step (12);

(10) calculating an azimuth position control strategy, which comprises an azimuth position control error, an azimuth speed instruction, an azimuth sliding mode equivalent control quantity, an azimuth sliding mode variable structure control quantity and an azimuth sliding mode control switching control quantity;

(11) calculating a high-low position control strategy, which comprises a high-low position control error, a high-low speed instruction, a high-low sliding mode equivalent control quantity, a high-low sliding mode variable structure control quantity and a high-low sliding mode control switching control quantity;

(12) calculating high and low compensation angular rates and azimuth interference compensation angular rates;

(13) calculating a high-low interference filtering correction value and an azimuth interference filtering correction value;

(14) calculating the total speed command of high and low servo and the total speed command of azimuth servo drive;

(15) calculating a current instruction of azimuth driving;

(16) calculating a current instruction of high and low driving;

(17) and (3) setting the driver to work in a torque mode, sending a current instruction to the servo driver through the CAN bus, and turning to the step (1).

2. The method for controlling rate compensation and self-stabilization of equivalent closed-loop interference under geodetic coordinates according to claim 1, wherein the step (8) calculates the orientation of the gun barrel in space through the equivalent closed-loop angles of the altitude and direction receiving instruments,

wherein psi (i) is the heading angle of the gun barrel, theta (i) is the pitch angle of the gun barrel,

is the roll angle, psi' of the gun barrel,

In order to calculate the intermediate variables in the process,

is a matrix of the postures of the cannon barrel,

wherein the content of the first and second substances,

is a matrix of the posture of the vehicle body,

is a vehicle body course attitude matrix,

is a matrix of the pitching attitude of the vehicle body,

is a matrix of the rolling attitude of the vehicle body,

is a matrix of the orientation and the posture of the artillery,

and the pitching attitude matrix of the artillery is obtained.

3. The method for equivalent closed-loop interference rate compensation self-stabilization control under geodetic coordinates according to claim 1, wherein the step (10) calculates an azimuth position control strategy,

e_β(i)＝ψ_ref(i)-ψ(i)

v_β(i)＝-k_dβsgn(s_1β)-κ_1βs_1β

z_0β(j)＝T_sv_0β+z_0β(j-1)

v_0β＝z_1β(j-1)-λ_0β|z_0lβ(j-1)-e_β(j-1)|^0.5sgn(z_0β(j-1)-e_β(j-1))

z_1β(j)＝T_sv_1β+z_1β(j-1)

v_1β＝z_1β(j-1)-λ_1β|z_1β(j-1)-v_0β|^0.5sgn(z_1β(j-1)-v_0β)

wherein: e.g. of the type_β(i) Controlling the error for the azimuth position;

is an azimuth speed instruction; u. of_pβmax,u_pβminThe upper and lower limit amplitude values of the azimuth rotating speed are obtained; j. the design is a square_1βThe azimuth turret moment of inertia; eta_βAn azimuth drive ratio; k is a radical of_βAzimuth drive stiffness; u. of_eqβ(i) Is an azimuth sliding mode equivalent control quantity; u. of_swβ(i) Azimuth sliding mode variable structure control quantity; b_βIs the azimuthal friction coefficient; alpha is alpha_βDriving the backlash for azimuth; ε (Δ β)_m) Fitting a non-linear function for the azimuth backlash; delta beta_mIs the difference between the azimuth motor shaft and the azimuth angle measurement value; beta is a_m(i) Is the azimuth motor shaft angle; c. C_1β,c_2βAre the first and second order coefficients of the orientation sliding mode, alpha_1β,α_2βRespectively are a first order index and a second order index of the azimuth sliding mode; t is_sIs a sampling period; tau is a backlash fitting coefficient; s_1βA sliding surface defined for orientation control; v. of_β(i) Controlling the switching control quantity for the direction sliding mode; z is a radical of_0β(j),z_1β(j),z_2β(j) Respectively, azimuth position control error e_β(i) 0, first, second order estimate of (lambda)_0β,λ_1β,λ_2βRespectively 0 th, first and second order coefficients, v, of its state estimate_0β,v_1βRespectively, intermediate variables in the state estimation; t is_βIs the azimuth filter time constant; k is a radical of_dβIs the orientation sliding mode stability factor, kappa_1βIs the orientation sliding mode convergence coefficient.

4. The method for self-stabilization control of equivalent closed-loop interference rate compensation under geodetic coordinates according to claim 1, wherein the step (11) calculates a high-low position control strategy

e_ε(i)＝θ_ref(i)-θ(i)

v_ε(i)＝-k_dεsgn(s_1ε)-κ_1εs_1ε

z_0ε(j)＝T_sv_0ε+z_0ε(j-1)

v_0ε＝z_1ε(j-1)-λ_0ε|z_0lε(j-1)-e_ε(j-1)|^0.5sgn(z_0ε(j-1)-e_ε(j-1))

z_1ε(j)＝T_sv_1ε+z_1ε(j-1)

v_1ε＝z_1ε(j-1)-λ_1ε|z_1ε(j-1)-v_0ε|^0.5sgn(z_1ε(j-1)-v_0ε)

z_2ε(j)＝T_s[-λ_2εsgn(z_2ε(j-1)-v_1ε)]+z_2ε(j-1)

Wherein: e.g. of the type_ε(i) Controlling the error for the high and low positions;

a high and low speed command; u. of_pεmax,u_pεminThe amplitude values of the upper limit and the lower limit of the high and low rotating speeds are obtained; j. the design is a square_1εHigh and low turret moment of inertia; eta_εThe transmission speed ratio is high and low; k is a radical of_εHigh and low transmission stiffness; u. of_eqε(i) The method comprises the steps of (1) obtaining equivalent control quantity of a high-low sliding mode; u. of_swε(i) Variable structure control variable of high and low sliding modes; b_εHigh and low friction coefficients; alpha is alpha_εThe gear backlash is high-low transmission gear backlash; ε (Δ ε)_m) Fitting a nonlinear function for the high and low backlash; epsilon_m(i) The angle of the motor shaft is high or low; delta epsilon_mThe angle difference between the high and low motor shafts and the high and low angle measurement is obtained; c. C_1ε,c_2εRespectively a first and a second order coefficient of high and low sliding modes, alpha_1ε,α_2εRespectively a first-order index and a second-order index of the sliding mode; s_1εA sliding mode surface defined for high-low control; v. of_ε(i) Controlling switching control quantity for high and low sliding modes; z is a radical of_0ε(j),z_1ε(j),z_2ε(j) Respectively a high and low position control error e_ε(i) 0, first, second order estimate of (lambda)_0ε,λ_1ε,λ_2εRespectively 0 th, first and second order coefficients, v, of its state estimate_0ε,v_1εRespectively, intermediate variables in the state estimation; t is_εHigh and low filter time constants; k is a radical of_dεIs a high and low slip form stability factor, kappa_1εThe convergence coefficients of high and low sliding modes.

5. The method for self-stabilization of equivalent closed-loop interference rate compensation under geodetic coordinates according to claim 1, wherein said step (12) of calculating the high-low compensation angular rate d_ε(j) Compensating angular rate d for sum azimuth interference_β(j)；

6. The method for self-stabilization of equivalent closed-loop interference rate compensation under geodetic coordinates according to claim 1, wherein said step (13) calculates the correction u of high-low interference filter_dε(j) Sum-of-azimuth interference filter correction u_dβ(j)；

u_dε(j)＝c₁₁d_ε(j)+c₁₂d_ε(j-1)-d₁₁u_dε(j-1)

u_dβ(j)＝c₂₁d_β(j)+c₂₂d_β(j-1)-d₂₁u_dβ(j-1)

Wherein, c₁₁,c₁₂,d₁₁Correcting coefficients for high and low interference filtering; c. C₂₁,c₂₂,d₂₁Correcting coefficients for the azimuth interference filter;

wherein: t is_sA speed control period; t is_ε1，T_β1Respectively, high-low and azimuth filtering time coefficients; t is_ε2，T_β2Respectively, a high-low time characteristic constant and an azimuth time characteristic constant; k is a radical of_ε1，k_β1Respectively high and low and an azimuth gain constant.

7. The method for self-stabilization of equivalent closed-loop disturbance rate compensation under geodetic coordinates according to claim 1, wherein the step (14) of calculating the total speed command of high and low servo

And the overall velocity command of the azimuth servo drive

8. The method for equivalent closed-loop interference rate compensation self-stabilization control under geodetic coordinates according to claim 1, wherein the step (15) of calculating the current command of azimuth drive

z_0lβ(j)＝T_sv_0lβ+z_0lβ(j-1)

v_0lβ＝z_1lβ(j-1)-λ_0lβ|z_0lβ(j-1)-l_eβ(j-1)|^0.5sgn(z_0lβ(j-1)-l_eβ(j-1))

z_0ωβ(j)＝T_sv_0ωβ+z_0ωβ(j-1)

l_eβ＝e_βω+γz_1eβ(j)^p/q

z_0eβ(j)＝T_sv_0eβ+z_0eβ(j-1)

v_0eβ＝z_1eβ(j-1)-λ_0eβ|z_0eβ(j-1)-e_βω(j-1)|^0.5sgn(z_0eβ(j-1)-e_βω(j-1))

Wherein: i.e. i_qeqβ,i_qnβRespectively an azimuth terminal sliding mode equivalent control quantity and a sliding mode integral control quantity; i.e. i_qβmax,i_qβminRespectively are azimuth current amplitude limiting values; j. the design is a square_βThe azimuth motor load moment of inertia; p is a radical of_βThe number of pole pairs of the azimuth motor is; psi_fβThe azimuth motor flux linkage coefficient; b is_βThe comprehensive viscous friction coefficient of the azimuth system; gamma ray_β,q_β,P_βA position terminal sliding mode coefficient; k is a radical of_β,

A direction terminal sliding mode control coefficient; z is a radical of_0lβ(j)，z_1lβ(j) Respectively, terminal sliding form_eβ(j) Is estimatedEvaluating and first order evaluation values; lambda [ alpha ]_0lβ，λ_1lβAre each l_eβ(j) Estimated 0 th and first order estimation coefficients; z is a radical of_0ωβ(j)，z_1ωβ(j) Are respectively the direction and speed commands

The estimated value and the first-order estimated value of (c); lambda [ alpha ]_0ωβ，λ_1ωβAre respectively as

Estimated 0 th and first order estimation coefficients; z is a radical of_0eβ(j)，z_1eβ(j) Respectively, an azimuth velocity control error e_βω(j) The estimated value and the first-order estimated value of (c); lambda [ alpha ]_0eβ，λ_1eβAre each e_βω(j) Estimated 0 th and first order estimation coefficients; v. of_0lβ,v_0ωβ,v_0eβRespectively, intermediate variables of the orientation state estimate.

9. The method for self-stabilization control of equivalent closed-loop interference rate compensation under geodetic coordinates according to claim 1, wherein the step (16) of calculating the current command of high-low driving

z_0lε(j)＝T_sv_0lε+z_0lε(j-1)

v_0lε＝z_1lε(j-1)-λ_0lε|z_0lε(j-1)-l_eε(j-1)|^0.5sgn(z_0lε(j-1)-l_eε(j-1))

z_1lε(j)＝T_s[-λ_lε1sgn(z_1lε(j-1)-v_0lε]+z_1lε(j-1)

z_0ωε(j)＝T_sv_0ωε+z_0ωε(j-1)

z_1ωε(j)＝T_s[-λ_1ωεsgn(z_1ωε(j-1)-v_0ωε)]+z_1ωε(j-1)

l_eε＝e_εω+γz_1eε(j)^p/q

z_0eε(j)＝T_sv_0eε+z_0eε(j-1)

v_0eε＝z_1eε(j-1)-λ_0eε|z_0eε(j-1)-e_εω(j-1)|^0.5sgn(z_0eε(j-1)-e_εω(j-1))

z_1eε(j)＝T_s[-λ_1eεsgn(z_1eε(j-1)-v_0eε)]+z_1eε(j-1)

Wherein: i.e. i_qeqε,i_qnεRespectively obtaining high and low terminal sliding mode equivalent control quantity and sliding mode integral control quantity; i.e. i_qεmax,i_qεminRespectively high and low current limiting values; j. the design is a square_εThe load moment of inertia of the high-low motor is obtained; p is a radical of_εThe number of the pole pairs of the square high-low motor is counted; psi_fεThe flux linkage coefficient of the high-low motor is obtained; b is_εHigh and low system comprehensive viscous friction coefficients; gamma ray_ε,q_ε,P_εHigh-low terminal sliding mode coefficients; k is a radical of_ε,η_ε1,η_ε2A high-low terminal sliding mode control coefficient; z is a radical of_0lε(j)，z_1lε(j) Respectively a high-low terminal sliding form_eε(j) The estimated value and the first-order estimated value of (c); lambda [ alpha ]_0lε，λ_1lεAre each l_eε(j) Estimated 0 th and first order estimation coefficients; z is a radical of_0ωε(j)，z_1ωε(j) Respectively high and low speed commands

The estimated value and the first-order estimated value of (c); lambda [ alpha ]_0ωε，λ_1ωεAre respectively as

Estimated 0 th and first order estimation coefficients; z is a radical of_0eε(j)，z_1eε(j) Respectively high and low speed control error e_εω(j) The estimated value and the first-order estimated value of (c); lambda [ alpha ]_0eε，λ_1eεAre each e_εω(j) Estimated 0 th and first order estimation coefficients; v. of_0lε,v_0ωε,v_0eεRespectively, the intermediate variables of the high and low state estimation.

Images