CN112729012B - Equivalent closed-loop interference rate compensation self-stabilization control method under geodetic coordinates - Google Patents
Equivalent closed-loop interference rate compensation self-stabilization control method under geodetic coordinates Download PDFInfo
- Publication number
- CN112729012B CN112729012B CN202011517103.7A CN202011517103A CN112729012B CN 112729012 B CN112729012 B CN 112729012B CN 202011517103 A CN202011517103 A CN 202011517103A CN 112729012 B CN112729012 B CN 112729012B
- Authority
- CN
- China
- Prior art keywords
- low
- azimuth
- control
- sliding mode
- order
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 27
- 238000011105 stabilization Methods 0.000 title claims abstract description 17
- 238000001914 filtration Methods 0.000 claims abstract description 18
- 238000012937 correction Methods 0.000 claims abstract description 16
- 238000011217 control strategy Methods 0.000 claims abstract description 15
- 239000011159 matrix material Substances 0.000 claims description 22
- 230000005540 biological transmission Effects 0.000 claims description 21
- 238000013461 design Methods 0.000 claims description 12
- 230000005251 gamma ray Effects 0.000 claims description 8
- 238000005070 sampling Methods 0.000 claims description 8
- 238000005259 measurement Methods 0.000 claims description 7
- 230000036544 posture Effects 0.000 claims description 7
- 230000004907 flux Effects 0.000 claims description 5
- 230000008569 process Effects 0.000 claims description 4
- 238000012886 linear function Methods 0.000 claims description 3
- 238000005096 rolling process Methods 0.000 claims description 2
- 230000007246 mechanism Effects 0.000 description 7
- 238000004364 calculation method Methods 0.000 description 6
- 230000001133 acceleration Effects 0.000 description 4
- 238000010304 firing Methods 0.000 description 4
- 230000000087 stabilizing effect Effects 0.000 description 4
- 230000006641 stabilisation Effects 0.000 description 3
- 238000012546 transfer Methods 0.000 description 3
- 230000033228 biological regulation Effects 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000001629 suppression Effects 0.000 description 2
- 230000001360 synchronised effect Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008901 benefit Effects 0.000 description 1
- 239000003638 chemical reducing agent Substances 0.000 description 1
- 230000003750 conditioning effect Effects 0.000 description 1
- 230000008878 coupling Effects 0.000 description 1
- 238000010168 coupling process Methods 0.000 description 1
- 238000005859 coupling reaction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000010355 oscillation Effects 0.000 description 1
- 230000004044 response Effects 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
Images
Classifications
-
- F—MECHANICAL ENGINEERING; LIGHTING; HEATING; WEAPONS; BLASTING
- F41—WEAPONS
- F41G—WEAPON SIGHTS; AIMING
- F41G5/00—Elevating or traversing control systems for guns
- F41G5/06—Elevating or traversing control systems for guns using electric means for remote control
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
- G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
- G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
- G05B13/042—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a parameter or coefficient is automatically adjusted to optimise the performance
Landscapes
- Engineering & Computer Science (AREA)
- Health & Medical Sciences (AREA)
- Artificial Intelligence (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Evolutionary Computation (AREA)
- Medical Informatics (AREA)
- Software Systems (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- General Engineering & Computer Science (AREA)
- Feedback Control In General (AREA)
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; acquiring angular velocity values of an SINS gyroscope group, a turret gyroscope group and a cannon pitching gyroscope; calculating the pointing, direction and height control strategies of the artillery barrel under a 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 of 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
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 car 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. To satisfy the largeThe amphibious suppression artillery designed for aiming between 10km under a moving base state and high dynamic state adopts Strapdown Inertial Navigation (SINS) to directly measure the orientation of an artillery barrel so as to ensure the aiming precision, a stabilizing system of the artillery adopts the navigation attitude of the SINS as the feedback of a spatial angular position controlled by an artillery follow-up system, a transmission mechanism is contained in a control closed ring in a nonlinear mode, the system generates oscillation in a small control error due to the influence of nonlinear factors, and the precision of a conventional ejection 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 2 The higher the acceleration is, the more difficult 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) Triaxial 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 to simultaneously receive artillery aiming azimuth control command psi under geodetic coordinates ref (j) High-low control command theta ref (j) If yes, entering the step (10); otherwise, switching 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 commands 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 cannon barrel in the space through the equivalent closed-loop angles of the height and direction receiving instruments,
wherein psi (i) is a course angle of the gun barrel, theta (i) is a pitch angle of the gun barrel,
is the roll angle psi' of the barrel of the artillery,
In order to calculate the intermediate variables in the process,
is a matrix of the postures of the cannon barrel,
wherein,
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 a bearing position control strategy,
e β (i)＝ψ ref (i)-ψ(i)
v β (i)＝-k dβ sgn(s 1β )-κ 1β s 1β
z 0β (j)＝T s v 0β +z 0β (j-1)
v 0β ＝z 1β (j-1)-λ 0β |z 0lβ (j-1)-e β (j-1)| 0.5 sgn(z 0β (j-1)-e β (j-1))
z 1β (j)＝T s v 1β +z 1β (j-1)
v 1β ＝z 1β (j-1)-λ 1β |z 1β (j-1)-v 0β | 0.5 sgn(z 1β (j-1)-v 0β )
wherein: e.g. of a cylinder β (i) Controlling the error for the azimuth position;
is an azimuth speed instruction; u. of pβmax ,u pβmin The upper and lower limit amplitude values of the azimuth rotating speed are obtained; j is a unit of 1β Is 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) Controlling quantity of the azimuth sliding mode variable structure; 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 m Is 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 s Is a sampling period; tau is a backlash fitting coefficient; s is 1β A sliding mode surface defined for orientation control; v. of β (i) Controlling the switching control quantity for the direction sliding mode control; 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 azimuthal 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 s v 0ε +z 0ε (j-1)
v 0ε ＝z 1ε (j-1)-λ 0ε |z 0lε (j-1)-e ε (j-1)| 0.5 sgn(z 0ε (j-1)-e ε (j-1))
z 1ε (j)＝T s v 1ε +z 1ε (j-1)
v 1ε ＝z 1ε (j-1)-λ 1ε |z 1ε (j-1)-v 0ε | 0.5 sgn(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. u pεmax ,u pεmin The 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-low sliding mode；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 motor shaft angle is high and low; delta epsilon m The 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 a 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 and low compensation angular speed d ε (j) Compensating angular rate d for sum azimuth interference β (j)；
The step (13) calculates the correction value u of the high-low interference filter dε (j) Sum-of-azimuth interference filter correction u dβ (j)；
u dε (j)＝c 11 d ε (j)+c 12 d ε (j-1)-d 11 u dε (j-1)
u dβ (j)＝c 21 d β (j)+c 22 d β (j-1)-d 21 u dβ (j-1)
Wherein, c 11 ,c 12 ,d 11 Correcting coefficients for high and low interference filtering; c. C 21 ,c 22 ,d 21 Filtering correction coefficients for the azimuth disturbances;
wherein: t is s A speed control period; t is ε1 ，T β1 Respectively, high-low and azimuth filtering time coefficients; t is ε2 ，T β2 Respectively a high-low time characteristic constant and an azimuth time characteristic constant; k is a radical of formula ε1 ，k β1 Respectively 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 s v 0lβ +z 0lβ (j-1)
v 0lβ ＝z 1lβ (j-1)-λ 0lβ |z 0lβ (j-1)-l eβ (j-1)| 0.5 sgn(z 0lβ (j-1)-l eβ (j-1))
z 0ωβ (j)＝T s v 0ωβ +z 0ωβ (j-1)
l eβ ＝e βω +γz 1eβ (j) p/q
z 0eβ (j)＝T s v 0eβ +z 0eβ (j-1)
v 0eβ ＝z 1eβ (j-1)-λ 0eβ |z 0eβ (j-1)-e βω (j-1)| 0.5 sgn(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βmin Respectively 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 ,η β2 A sliding mode control coefficient of the azimuth terminal; z is a radical of formula 0lβ (j)，z 1lβ (j) Respectively, terminal sliding form eβ (j) The estimated value and the first-order estimated value of (c); lambda 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 (a); lambda [ alpha ] 0eβ ，λ 1eβ Are respectively 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 s v 0lε +z 0lε (j-1)
v 0lε ＝z 1lε (j-1)-λ 0lε |z 0lε (j-1)-l eε (j-1)| 0.5 sgn(z 0lε (j-1)-l eε (j-1))
z 1lε (j)＝T s [-λ lε1 sgn(z 1lε (j-1)-v 0lε ]+z 1lε (j-1)
z 0ωε (j)＝T s v 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 s v 0eε +z 0eε (j-1)
v 0eε ＝z 1eε (j-1)-λ 0eε |z 0eε (j-1)-e εω (j-1)| 0.5 sgn(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 all right angle qεmax ,i qεmin Respectively 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 ε Is the number of pole pairs of the square Gao Di motor; psi fε The flux linkage coefficient of the motor is high and low; b ε High and low system comprehensive viscous friction coefficients; gamma ray ε ,q ε ,P ε High-low terminal sliding mode coefficients; k is a radical of formula ε ,η ε1 ,η ε2 A 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 (a); 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 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, intermediate variables of the high and low state estimation.
The beneficial effects of the invention are: the stabilization control method ensures that the suppression control of the stabilization system on the interference is completely determined by the sensitive interference rate, has high response bandwidth and accurate compensation of the interference rate, effectively overcomes the interference of the carrier attitude on the directional control of the cannon barrel, and realizes the high-precision stable control of the cannon barrel direction 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 s =1ms, the initial value of step number i plus 1,i is 0;
(2) Collecting navigation attitude angle 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 omega ε (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 triaxial angular rate omega measured by 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 a three-axis gyroscope shaft 1, a three-axis gyroscope shaft 2 and a three-axis gyroscope shaft 3 of the turret gyroscope respectively;
(7) Collecting angular rate omega measured by artillery pitching gyroscope p (j)；
(8) Calculating the direction of the cannon 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 artillery 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;
an artillery barrel attitude matrix is formed;
is a vehicle body course attitude matrix which 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 s v 0β +z 0β (j-1)
v 0β ＝z 1β (j-1)-λ 0β |z 0lβ (j-1)-e β (j-1)| 0.5 sgn(z 0β (j-1)-e β (j-1))
z 1β (j)＝T s v 1β +z 1β (j-1)
v 1β ＝z 1β (j-1)-λ 1β |z 1β (j-1)-v 0β | 0.5 sgn(z 1β (j-1)-v 0β )
Wherein: e.g. of a cylinder β (i) Controlling the error for the azimuth position;
is an azimuth speed instruction; u. of pβmax ,u pβmin The upper and lower limit amplitude values of the azimuth rotating speed are obtained; j is a unit of 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 (alpha) ("alpha") β Driving the backlash for azimuth; ε (Δ β) m ) Fitting a non-linear function for the azimuth backlash; delta beta m Is the difference between the azimuth motor shaft and the azimuth angle measurement value; beta is a m (i) For azimuth motor shaft angle；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 s Is 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 their state estimation; t is β Is the azimuth filter time constant; k is a radical of formula 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 s v 0ε +z 0ε (j-1)
v 0ε ＝z 1ε (j-1)-λ 0ε |z 0lε (j-1)-e ε (j-1)| 0.5 sgn(z 0ε (j-1)-e ε (j-1))
z 1ε (j)＝T s v 1ε +z 1ε (j-1)
v 1ε ＝z 1ε (j-1)-λ 1ε |z 1ε (j-1)-v 0ε | 0.5 sgn(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 a cylinder ε (i) Controlling the error for the high and low positions;
a high and low speed command; u. of pεmax ,u pεmin The 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 formula ε 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) The control quantity of the high-low sliding mode variable structure; 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 motor shaft angle is high and low; delta epsilon m The 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ε Are respectively its stateAn intermediate variable in the estimation; t is ε Is the 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) And azimuth disturbance compensating angular rate d β (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 11 d ε (j)+c 12 d ε (j-1)-d 11 u dε (j-1)
u dβ (j)＝c 21 d β (j)+c 22 d β (j-1)-d 21 u dβ (j-1)
wherein, c 11 ,c 12 ,d 11 Correcting coefficients for high and low interference filtering; c. C 21 ,c 22 ,d 21 Correcting coefficients for the azimuth interference filter;
wherein: t is s A speed control period; t is ε1 ，T β1 Respectively, high-low and azimuth filtering time coefficients; t is a unit of ε2 ，T β2 Respectively a high-low time characteristic constant and an azimuth time characteristic constant; k is a radical of ε1 ，k β1 Respectively 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 s v 0lβ +z 0lβ (j-1)
v 0lβ ＝z 1lβ (j-1)-λ 0lβ |z 0lβ (j-1)-l eβ (j-1)| 0.5 sgn(z 0lβ (j-1)-l eβ (j-1))
z 0ωβ (j)＝T s v 0ωβ +z 0ωβ (j-1)
l eβ ＝e βω +γz 1eβ (j) p/q
z 0eβ (j)＝T s v 0eβ +z 0eβ (j-1)
v 0eβ ＝z 1eβ (j-1)-λ 0eβ |z 0eβ (j-1)-e βω (j-1)| 0.5 sgn(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βmin Respectively are azimuth current amplitude limiting values; j is a unit of β The azimuth motor load moment of inertia; p is a radical of β The number of pole pairs of the azimuth motor is set; psi fβ The magnetic linkage coefficient of the azimuth motor; b β 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 ,η β2 A 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 azimuth velocity command
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 (a); 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 s v 0lε +z 0lε (j-1)
v 0lε ＝z 1lε (j-1)-λ 0lε |z 0lε (j-1)-l eε (j-1)| 0.5 sgn(z 0lε (j-1)-l eε (j-1))
z 1lε (j)＝T s [-λ lε1 sgn(z 1lε (j-1)-v 0lε ]+z 1lε (j-1)
z 0ωε (j)＝T s v 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 s v 0eε +z 0eε (j-1)
v 0eε ＝z 1eε (j-1)-λ 0eε |z 0eε (j-1)-e εω (j-1)| 0.5 sgn(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εmin Respectively 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 ε Is the number of pole pairs of the square Gao Di motor; 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 and low terminal sliding mode coefficients; k is a radical of ε ,η ε1 ,η ε2 A 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 formula 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 formula 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, 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 azimuth 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, which are discretized using a bi-linear transformation.
The stable gun aiming system for implementing the control method mainly comprises a stable control system, a driving speed regulation system, a power supply system,The device comprises a turret gyroscope group, a vehicle body gyroscope, a height and azimuth 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 p =3, rated power 4kW, stator inductance 0.0098mH, stator resistance 3.5 milliohm, rated rotation speed 3000RPM, rated torque 7.4Nm, and equivalent moment of inertia J of motor rotor and transmission gear train sum to 0.013kg · m 2 (ii) a High and low PMSM (permanent magnet synchronous Motor), the bus voltage is 56VDC, and the number n of pole pairs p =3, rated power 2kW, rated torque 3.2Nm, stator inductance 0.032mH, stator resistance 0.0105 ohms, rated speed 3000RPM, equivalent moment of inertia J of the motor rotor and the transmission gear train 0.0075kg · m 2 . The azimuthal load moment of inertia is about 2700kg m 2 The transmission speed ratio is 470. The high and low load moment of inertia is 700kg m 2 . The transmission ratio is 450. The angular speed measuring range of the SINS is +/-300 degrees/s, the heading measuring precision is not more than 0.3mil, and the attitude measuring precision is not more than 0.1mil.
Fig. 3 is a flowchart of the calculation according to the embodiment of the present invention, and the detailed implementation process will be described in detail 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 motor shaft of high and low m (i) And ω ε (i) Angular velocity, azimuth Motor shaft Angle value β m (i) And angular velocity ω β (i)；
(5) Acquisition of the triaxial angular rate omega measured by a gyro group of an 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 cannon 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) Azimuth control strategy calculation
e β (i)＝ψ ref (i)-ψ(i)
v β (i)＝-k dβ sgn(s 1β )-κ 1β s 1β
z 0β (j)＝T s v 0β +z 0β (j-1)
v 0β ＝z 1β (j-1)-λ 0β |z 0lβ (j-1)-e β (j-1)| 0.5 sgn(z 0β (j-1)-e β (j-1))
z 1β (j)＝T s v 1β +z 1β (j-1)
v 1β ＝z 1β (j-1)-λ 1β |z 1β (j-1)-v 0β | 0.5 sgn(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; given speed ratio η β =470; rigidity k β ＝2×10 7 (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 s v 0ε +z 0ε (j-1)
v 0ε ＝z 1ε (j-1)-λ 0ε |z 0lε (j-1)-e ε (j-1)| 0.5 sgn(z 0ε (j-1)-e ε (j-1))
z 1ε (j)＝T s v 1ε +z 1ε (j-1)
v 1ε ＝z 1ε (j-1)-λ 1ε |z 1ε (j-1)-v 0ε | 0.5 sgn(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 7 (ii) a Coefficient of friction b ε =0.008; 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 11 d ε (j)+c 12 d ε (j-1)-d 11 u dε (j-1)
u dβ (j)＝c 21 d β (j)+c 22 d β (j-1)-d 21 u dβ (j-1)
wherein, c 11 ,c 12 ,d 11 Correcting coefficients for high and low interference filtering; c. C 21 ,c 22 ,d 21 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 s v 0lβ +z 0lβ (j-1)
v 0lβ ＝z 1lβ (j-1)-λ 0lβ |z 0lβ (j-1)-l eβ (j-1)| 0.5 sgn(z 0lβ (j-1)-l eβ (j-1))
z 0ωβ (j)＝T s v 0ωβ +z 0ωβ (j-1)
l eβ ＝e βω +γz 1eβ (j) p/q
z 0eβ (j)＝T s v 0eβ +z 0eβ (j-1)
v 0eβ ＝z 1eβ (j-1)-λ 0eβ |z 0eβ (j-1)-e βω (j-1)| 0.5 sgn(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, number of pole pairs p of its motor β =3, magnetic linkage coefficient ψ 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) Calculate the altitudeCurrent command for driving
z 0lε (j)＝T s v 0lε +z 0lε (j-1)
v 0lε ＝z 1lε (j-1)-λ 0lε |z 0lε (j-1)-l eε (j-1)| 0.5 sgn(z 0lε (j-1)-l eε (j-1))
z 1lε (j)＝T s [-λ lε1 sgn(z 1lε (j-1)-v 0lε ]+z 1lε (j-1)
z 0ωε (j)＝T s v 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 s v 0eε +z 0eε (j-1)
v 0eε ＝z 1eε (j-1)-λ 0eε |z 0eε (j-1)-e εω (j-1)| 0.5 sgn(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, number of pole pairs p thereof ε =3, magnetic linkage coefficient ψ fε =0.02, given 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 the SINS;
(3) Collecting the measured value epsilon of high and low receiving instruments 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 cannon barrel in the space through the equivalent closed-loop angle of the height and direction signal receiving instrument;
(9) Judging whether to simultaneously receive artillery aiming azimuth control command psi under geodetic coordinates ref (j) High-low control command theta ref (j) If yes, entering the step (10); otherwise, switching 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 a course angle of the gun barrel, theta (i) is a 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,
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 equivalent closed-loop disturbance rate compensation self-stabilization control method in geodetic coordinates according to claim 2, characterized in that 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 s v 0β +z 0β (j-1)
v 0β ＝z 1β (j-1)-λ 0β |z 0β (j-1)-e β (j-1)| 0.5 sgn(z 0β (j-1)-e β (j-1))
z 1β (j)＝T s v 1β +z 1β (j-1)
v 1β ＝z 1β (j-1)-λ 1β |z 1β (j-1)-v 0β | 0.5 sgn(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βmin The 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. u eqβ (i) Is an azimuth sliding mode equivalent control quantity; u. u 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 m Is 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 an orientation sliding mode first-order index and a second-order index; t is s A speed control period; tau is a backlash fitting coefficient; s is 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 formula 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 azimuth sliding mode convergence factor.
4. The method for self-stabilization control of equivalent closed-loop interference rate compensation under geodetic coordinates according to claim 3, 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 s v 0ε +z 0ε (j-1)
v 0ε ＝z 1ε (j-1)-λ 0ε |z 0ε (j-1)-e ε (j-1)| 0.5 sgn(z 0ε (j-1)-e ε (j-1))
z 1ε (j)＝T s v 1ε +z 1ε (j-1)
v 1ε ＝z 1ε (j-1)-λ 1ε |z 1ε (j-1)-v 0ε | 0.5 sgn(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εmin The upper and lower limit amplitude values of high and low rotating speeds; 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 m The 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 is 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, first, second order coefficients, v, of their state estimates 0ε ,v 1ε Respectively, intermediate variables in their 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 coefficient of the high and low sliding modes.
5. The method for self-stabilization of equivalent closed-loop interference rate compensation under geodetic coordinates according to claim 4, wherein the 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 5, wherein said step (13) calculates the correction u of filtering interference dε (j) Sum-of-azimuth interference filter correction u dβ (j)；
u dε (j)＝c 11 d ε (j)+c 12 d ε (j-1)-d 11 u dε (j-1)
u dβ (j)＝c 21 d β (j)+c 22 d β (j-1)-d 21 u dβ (j-1)
Wherein, c 11 ,c 12 ,d 11 Correcting coefficients for high and low interference filtering; c. C 21 ,c 22 ,d 21 Filtering correction coefficients for the azimuth disturbances;
wherein: t is s A speed control period; t is a unit of ε1 ，T β1 Respectively, high-low and azimuth filtering time coefficients; t is ε2 ，T β2 Respectively a high-low time characteristic constant and an azimuth time characteristic constant; k is a radical of ε1 ，k β1 Respectively 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 6, wherein the step (14) of calculating the total speed command of high and low servo
And overall velocity command of azimuth servo drive
8. The method as claimed in claim 7, wherein the step (15) of calculating the azimuth-driven current command
z 0lβ (j)＝T cs v 0lβ +z 0lβ (j-1)
v 0lβ ＝z 1lβ (j-1)-λ 0lβ |z 0lβ (j-1)-l eβ (j-1)| 0.5 sgn(z 0lβ (j-1)-l eβ (j-1))
z 0ωβ (j)＝T cs v 0ωβ +z 0ωβ (j-1)
z 0eβ (j)＝T cs v 0eβ +z 0eβ (j-1)
v 0eβ ＝z 1eβ (j-1)-λ 0εβ |z 0eβ (j-1)-e βω (j-1)| 0.5 sgn(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βmin Respectively as azimuth current 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 β 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 sliding mode control coefficient of the azimuth terminal; 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 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, the overall speed command of the azimuth servo drive
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 respectively 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 equivalent closed-loop disturbance rate compensation self-stabilization control under geodetic coordinates according to claim 8, wherein the step (16) of calculating the current command of high-low driving
z 0lε (j)＝T cs v 0lε +z 0lε (j-1)
v 0lε ＝z 1lε (j-1)-λ 0lε |z 0lε (j-1)-l eε (j-1)| 0.5 sgn(z 0lε (j-1)-l eε (j-1))
z 1lε (j)＝T cs [-λ 1lε sgn(z 1lε (j-1)-v 0lε ]+z 1lε (j-1)
z 0ωε (j)＝T cs v 0ωε +z 0ωε (j-1)
z 1ωε (j)＝T cs [-λ 1ωε sgn(z 1ωε (j-1)-v 0ωε )]+z 1ωε (j-1)
z 0eε (j)＝T cs v 0eε +z 0eε (j-1)
v 0eε ＝z 1wε (j-1)-λ 0eε |z 0eε (j-1)-e εω (j-1)| 0.5 sgn(z 0eε (j-1)-e εω (j-1))
z 1eε (j)＝T cs [-λ 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 pεmin Respectively high and low current limiting values; j is a unit of a group ε The motor load rotational inertia is high or low; p is a radical of ε Is Gao Di motor pole pair number; psi fε The flux linkage coefficient of the high-low motor is obtained; b ε The comprehensive viscous friction coefficient of high and low systems; gamma ray ε ,q ε ,P ε High and low terminal sliding mode coefficients; k is a radical of ε ,η ε1 ,η ε2 A 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 formula 0ωε (j)，z 1ωε (j) Respectively high and low servo total speed command
The estimated value and the first-order estimated value of (c); lambda 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 (a); lambda [ alpha ] 0eε ，λ 1eε Are respectively 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.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011517103.7A CN112729012B (en) | 2020-12-21 | 2020-12-21 | Equivalent closed-loop interference rate compensation self-stabilization control method under geodetic coordinates |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011517103.7A CN112729012B (en) | 2020-12-21 | 2020-12-21 | Equivalent closed-loop interference rate compensation self-stabilization control method under geodetic coordinates |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112729012A CN112729012A (en) | 2021-04-30 |
| CN112729012B true CN112729012B (en) | 2022-12-23 |
Family
ID=75603795
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011517103.7A Active CN112729012B (en) | 2020-12-21 | 2020-12-21 | Equivalent closed-loop interference rate compensation self-stabilization control method under geodetic coordinates |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112729012B (en) |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN2511937Y (en) * | 2002-01-14 | 2002-09-18 | 华腾数控有限公司 | A.C. servo device for gun |
| CN107783422A (en) * | 2017-10-20 | 2018-03-09 | 西北机电工程研究所 | Using the gun laying systems stabilisation control method of inertial navigation |
| CN109669480A (en) * | 2019-01-03 | 2019-04-23 | 西安航天动力技术研究所 | A kind of guiding head controlling method of future position |
Family Cites Families (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US8005635B2 (en) * | 2007-08-14 | 2011-08-23 | Ching-Fang Lin | Self-calibrated azimuth and attitude accuracy enhancing method and system (SAAAEMS) |
| GB2539587B (en) * | 2014-02-20 | 2020-06-03 | Flir Systems | Acceleration corrected attitude estimation systems and methods |
| US10877487B2 (en) * | 2018-04-25 | 2020-12-29 | The Boeing Company | Pitch and thrust control for compound aircraft |
-
2020
- 2020-12-21 CN CN202011517103.7A patent/CN112729012B/en active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN2511937Y (en) * | 2002-01-14 | 2002-09-18 | 华腾数控有限公司 | A.C. servo device for gun |
| CN107783422A (en) * | 2017-10-20 | 2018-03-09 | 西北机电工程研究所 | Using the gun laying systems stabilisation control method of inertial navigation |
| CN109669480A (en) * | 2019-01-03 | 2019-04-23 | 西安航天动力技术研究所 | A kind of guiding head controlling method of future position |
Non-Patent Citations (2)
| Title |
|---|
| 基于捷联惯导的火炮随动系统应用研究;李伟 等;《火炮发射与控制学报》;20090331(第1期);35-38 * |
| 行进间火炮指向稳定跟踪系统建模与控制;李伟等;《兵工学报》;20180831;第39卷(第08期);1479-1490 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112729012A (en) | 2021-04-30 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107783422B (en) | Control method of gun aiming stabilization system adopting strapdown inertial navigation | |
| CN102981407B (en) | A kind of Tank gun control method of servo-controlling based on Auto Disturbances Rejection Control Technique | |
| CN106123921B (en) | The unknown Alignment Method of the latitude of Strapdown Inertial Navigation System under the conditions of dynamic disturbance | |
| CN109573105B (en) | Attitude control method for terminal sub-level orbit-reserving application subsystem | |
| CN108333938B (en) | Multi-closed-loop composite gyro stability control method | |
| CN110119089B (en) | Immersion constant flow pattern self-adaptive quad-rotor control method based on integral sliding mode | |
| CN111338391A (en) | Two-axis four-frame photoelectric turntable control system | |
| CN105785762B (en) | Two-axis inertially stabilized platform high-precision control method based on self-adaptive backstepping sliding mode | |
| CN110554603A (en) | photoelectric stabilized sighting platform control method based on disturbance observation fractional order controller | |
| CN110658854B (en) | Photoelectric turret video tracking feedforward compensation method based on combined inertial navigation information application | |
| CN102288133B (en) | Installation deflection angle calibration method of gyro indirect stable system | |
| CN110057382A (en) | A kind of inertial navigation numerical value update method based on launching coordinate system | |
| CN106500733B (en) | The non-orthogonal angle self-calibration of three axis Rotating Inertial Navigation System frames of one kind and compensation method | |
| CN113335567B (en) | Wheel magnetic hybrid attitude control method and system for microsatellite | |
| CN104932517A (en) | Underwater unmanned aircraft track tracking dynamic surface control optimization method under environment interference | |
| CN113419431B (en) | Stratospheric airship trajectory tracking control method and system based on event triggering | |
| CN114879511A (en) | High-precision control method for triaxial inertially stabilized platform based on self-adaptive extended state observer and global fast terminal sliding mode | |
| CN107607128B (en) | Method for compensating aiming line precision of two-axis two-frame stable platform | |
| CN112696981B (en) | Full closed loop interference rate compensation self-stabilization control method under geodetic coordinate system | |
| CN113525415A (en) | Automatic-driving automobile layered anti-interference path tracking system based on rack force observation | |
| CN112729012B (en) | Equivalent closed-loop interference rate compensation self-stabilization control method under geodetic coordinates | |
| CN208384428U (en) | Stable turntable system based on reversed bit shift compensation | |
| CN108896045B (en) | Inertial navigation system without accelerometer and navigation method | |
| CN112537463B (en) | Satellite attitude control method and system | |
| CN111142380A (en) | Control method of position servo system for rocket gun launching during traveling |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |