CN105865272A - Integrated control method used for semi-strapdown guided missile - Google Patents

Abstract

The invention discloses an integrated control method used for a semi-strapdown guided missile. The method includes the following steps that firstly, missile guider stable tracking and missile body posture control integrated mathematical models are established and include a missile body posture kinematic model, a frame kinematic model, an angle tracking system model and an integrated mathematical model; secondly, an integrated controller configuration is designed; and thirdly, controller design based on non-linear dynamic inverse is conducted. According to the integrated models, the integrated control configuration based on an inner ring and an outer ring is designed, the coupling problem between control loops is solved, the conservative property of design of a control system is reduced, and the comprehensive performance of the system is improved.

Description

A kind of integrated control method for half strapdown guidance guided missile

Technical field

The invention belongs to weapon technologies, target seeker technology, control method field, relate to air-to-air missile Control System Design and grind Study carefully, be specifically related to the integrated control method of a kind of half strapdown guidance guided missile.

Background technology

The control of target seeker tenacious tracking is the core technology that half strapdown seeker controls, and follows the tracks of to study half strap down stability Algorithm, it is to be understood that body and the kinetics of servo framework, kinematic relation, and double strapdown attitude algorithm deeply grind Study carefully.Half traditional strapdown seeker stabilizing control system and body attitude Control System Design, for ease of each point of analysis and investigation The performance of system, generally using target seeker stable tracking control system, body attitude control system as two independent parts, cuts Split and study respectively, use two loop independent design thoughts.Under this separate design thought, generally assume that Half strap down stability control system and body attitude control system can decouple, and can be thus low to two PROBLEM DECOMPOSITION The design of rank subsystem, greatly reduces design difficulty.

It practice, the special construction of half strapdown seeker makes to couple between body with target seeker stabilized platform framework sternly Weight.When target carries out high maneuver, for relying on pneumatic rudder to realize the half strap-down imaging guided missile that normal g-load controls, lead Bullet needs big pose adjustment to tackle the high maneuver of target to obtain big overload.If half strapdown seeker tenacious tracking control The response of system processed is not prompt enough, and missile attitude adjusts and may result in target disengaging target seeker visual field under half strapdown system, Especially at guidance latter end along with playing the close of mesh relative distance, this phenomenon is even more serious.This is mainly due to parasitic loop Exist so that target seeker tenacious tracking controls loop and body attitude controls loop and produces serious coupling.Therefore, half strapdown is infrared Imaging Guidance system to realize the tenacious tracking to target, needs by half strapdown seeker stable tracking control system and body Attitude control system is coordinated to control to realize jointly, could fundamentally solve the engineer applied problem of half strapdown seeker.

Summary of the invention

The invention aims to solve the problems referred to above, have employed integrated design scheme, lead by setting up half strapdown Leader controls Integrated Model with body attitude, uses nonlinear dynamic inversion control technology, has reached between two subsystems Coordinate to control effect, there is the advantages such as body attitude fast response time, target seeker tracking error are little, double strapdown STT missile system The engineering design of system has guiding significance.

A kind of integrated control method for half strapdown guidance guided missile of the present invention, point following steps:

Step 1: set up target seeker tenacious tracking and control integrated mathematical model with body attitude, move including body attitude Learn model, frame movement model, angle tracking system model, integration mathematical model；

Step 2: design integration controller configuration；

Step 3: controller based on nonlinear dynamic inverse designs.

The present invention passes through three above step, i.e. initially sets up Integrated Model, designs the most on this basis based on interior The integrated controller configuration of outer shroud, finally separately designs inner and outer ring and controls the nonlinear dynamic inverse control law in loop.Last solution Certainly target seeker tenacious tracking controls loop and body attitude controls the coupled problem between loop, improves half strapdown guidance guided missile Control effect.

It is an advantage of the current invention that:

(1) by analyzing the coupled relation between target seeker tenacious tracking control loop and body attitude control loop, On the basis of this, establish target seeker tenacious tracking control integrated mathematical model with body attitude, for follow-up integrated controller Design provides mathematical model and theoretical basis；

(2) according to Integrated Model, the overall-in-one control schema configuration based on inner and outer ring of design, solve between control loop Coupled problem, reduce the conservative of Control System Design, improve the combination property of system；

(3) using the control law of dynamic inversion control Design Theory, this control law ensure that target seeker tenacious tracking and bullet Rapidity, convergence and the stability of body attitude Integrated Model.

Accompanying drawing explanation

Fig. 1 is the method flow diagram of the present invention；

Fig. 2 is the graph of a relation between missile coordinate system and detection coordinate system；

Fig. 3 is half strapdown seeker optical axis pointing space geometrical relationship figure；

Fig. 4 is double loop nonlinear dynamic inverse integrated controller block diagram；

Fig. 5 is nonlinear dynamic inversion control schematic flow sheet；

Fig. 6 is α, β, γ_vTracking error figure；

Fig. 7 is actual angle of rudder reflection figure；

Fig. 8 is actual error angle figure；

Fig. 9 is that actual frame angular velocity is intended to；

Figure 10 is actual frame angle figure.

Detailed description of the invention

Below in conjunction with drawings and Examples, the present invention is described in further detail.

Present invention employs integrated design method, control integrated mould by setting up half strapdown seeker with body attitude Type, uses nonlinear dynamic inversion control technology, has reached the coordination between two subsystems and has controlled effect, has body attitude and rings Answering the advantages such as speed is fast, target seeker tracking error is little, the engineering design of double strapdown missile control system has guiding significance.

The present invention is a kind of integrated control method for half strapdown guidance guided missile, and flow process is as it is shown in figure 1, include following Several steps:

Step 1: set up target seeker tenacious tracking and control integrated mathematical model with body attitude, move including body attitude Learn model, frame movement model, angle tracking system model, integration mathematical model.

(1) body attitude kinematics model

Playing in mesh relative motion, guided missile provides required overload instruction, needed for calculating required overload according to guidanceing command α、β、γ_vCan directly be tried to achieve by kinetics relation.Derivation in the middle of omitting, directly gives the guided missile flow angle differential equation (α、β、γ_vThe differential equation):

$\left[\begin{array}{c}\stackrel{\·}{\mathrm{\α}}\\ \stackrel{\·}{\mathrm{\β}}\\ {\stackrel{\·}{\mathrm{\γ}}}_v\end{array}\right]=\left[\begin{array}{c}\frac{{\mathrm{mgcos\γ}}_v\cos \mathrm{\θ}-L}{mV\cos \mathrm{\β}}\\ \frac{{\mathrm{mgsin\γ}}_v\cos \mathrm{\θ}+Y}{mV}\\ -{\stackrel{\·}{\mathrm{\ψ}}}_v\sin \mathrm{\θ}-\frac{({\mathrm{mgcos\γ}}_v\cos \mathrm{\θ}-L)\tan \mathrm{\β}}{mV}\end{array}\right]+\left[\begin{array}{ccc}-\cos \mathrm{\α}\tan \mathrm{\β}& \sin \mathrm{\α}\tan \mathrm{\β}& 1\\ \sin \mathrm{\α}& \cos \mathrm{\α}& 0\\ \cos \mathrm{\α}\sec \mathrm{\β}& -\sin \mathrm{\α}\sec \mathrm{\β}& 0\end{array}\right]\left[\begin{array}{c}{\mathrm{\ω}}_{mx}\\ {\mathrm{\ω}}_{my}\\ {\mathrm{\ω}}_{mz}\end{array}\right]---\left(1\right)$

In formula, α, beta, gamma_vIt is respectively the angle of attack, yaw angle, angle of heel；θ,ψ_vIt is respectively trajectory tilt angle, trajectory deflection angle；L, Y divide Wei lift and side force；ω_mx,ω_my,ω_mzFor body body axle attitude angular velocity；M is guided missile quality, and g is acceleration of gravity, V For missile flight speed.

The profile of tactical missile is the most axisymmetric, and the product of inertia of guided missile axle each to missile coordinate system is zero.Can arrange Go out the attitude angular velocity differential equation as follows:

$\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\ω}}}_{mx}\\ {\stackrel{\·}{\mathrm{\ω}}}_{my}\\ {\stackrel{\·}{\mathrm{\ω}}}_{mz}\end{array}\right]=\left[\begin{array}{c}\frac{M_{x0}}{I_x}-\frac{\left(I_z{I}_y\right){\mathrm{\ω}}_{mz}{\mathrm{\ω}}_{my}}{I_x}\\ \frac{M_{y0}}{I_y}-\frac{(I_x-I_z){\mathrm{\ω}}_{mx}{\mathrm{\ω}}_{mz}}{I_y}\\ \frac{M_{z0}}{I_z}-\frac{(I_y-I_x){\mathrm{\ω}}_{my}{\mathrm{\ω}}_{mx}}{I_z}\end{array}\right]+\left[\begin{array}{ccc}\frac{{\mathrm{QSL}}_r{m}_x^{{\mathrm{\δ}}_x}}{I_x}& 0& 0\\ 0& \frac{{\mathrm{QSL}}_r{m}_y^{{\mathrm{\δ}}_y}}{I_y}& 0\\ 0& 0& \frac{{\mathrm{QSL}}_r{m}_z^{{\mathrm{\δ}}_z}}{I_z}\end{array}\right]\left[\begin{array}{c}{\mathrm{\δ}}_x\\ {\mathrm{\δ}}_y\\ {\mathrm{\δ}}_z\end{array}\right]---\left(2\right)$

In formula, M_x0,M_y0,M_z0It it is the aerodynamic moment under the zero inclined state of rudder；For rolling moment coefficient to δ_xDerivative, For rolling moment coefficient to δ_yDerivative,For rolling moment coefficient to δ_zDerivative；Dynamic pressure Q=0.5 ρ V², ρ is atmospheric density； S is guided missile area of reference；L_rFor guided missile reference length；δ_x,δ_y,δ_zFor rolling, driftage, the three-channel angle of rudder reflection of pitching；I_x,I_y, I_zIt is respectively the rotary inertia in three directions of guided missile.

(2) frame movement model

Seeker's light axis inside casing is arranged on housing, and outside framework pedestal connects firmly with body.According to rigid body kinematics principle, lead The space motion of leader optical axis is the motion compound motion with frame member of pedestal, and the motion of housing is base motion and housing The synthesis of own rotation, the motion of inside casing is that housing coupled motions cause jointly with inside casing own rotation.The attitude motion of body Being coupled in target seeker motion by geometrical constraint and friction, the middle geometry movement relation transmission that there is complexity, such as Fig. 2 institute Show.

The motion in space of Seeker's light axis center is:

ω_d=ω_dm+ω_ds (3)

In formula,

${\mathrm{\ω}}_{dm}=\left[\begin{array}{c}{\mathrm{\ω}}_{mx}{\mathrm{cos\λ}}_z{\mathrm{cos\λ}}_y+{\mathrm{\ω}}_{my}{\mathrm{sin\λ}}_z-{\mathrm{\ω}}_{mz}{\mathrm{cos\λ}}_z{\mathrm{sin\λ}}_y\\ -{\mathrm{\ω}}_{mx}{\mathrm{sin\λ}}_z{\mathrm{cos\λ}}_y+{\mathrm{\ω}}_{my}{\mathrm{cos\λ}}_z+{\mathrm{\ω}}_{mz}{\mathrm{sin\λ}}_z{\mathrm{sin\λ}}_y\\ {\mathrm{\ω}}_{mx}{\mathrm{sin\λ}}_y+{\mathrm{\ω}}_{mz}{\mathrm{cos\λ}}_y\end{array}\right]{\mathrm{\ω}}_{ds}=\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\λ}}}_y{\mathrm{sin\λ}}_z\\ {\stackrel{\·}{\mathrm{\λ}}}_y{\mathrm{cos\λ}}_z\\ {\stackrel{\·}{\mathrm{\λ}}}_z\end{array}\right]$

Wherein, ω_dFor the projection in detection coordinate system of the optic angle speed, ω_dmFor body angle speed in detection coordinate system In projection, ω_dsFor the projection in detection coordinate system of the target seeker servo frame corners speed, ω_mx,ω_my,ω_mzFor body body axle Angular velocity, λ_y,λ_zIt it is the internal and external frame angle of half strap down stability platform.

(3) angle tracking system model

In order to be briefly described, as a example by two-dimensional space, target seeker Space Angle relation such as Fig. 3 shows, q is the visual line angle of bullet, and λ is Frame corners, θ is body attitude angle,For optical axis and datum line angle, ε is the angle of optical axis and line of sight, i.e. error angle.

So-called angle tracking system, by controlling the rolling on target seeker and the frame member of pitching both direction so that lead Leader optical axis points to follow the tracks of all the time and plays line of sight direction, that is final purpose is to make Seeker's light axis overlap with playing line of sight.But Owing to target is constantly in maneuvering condition, therefore always there is deviation between Seeker's light axis and the visual line angle of bullet, this deviation is also It it is exactly error angle.Optical axis error angle along the line and that play between line of sight line can be measured by Infrared Imaging Seeker and obtain. According to following principle, use small angle approximation and ignore three rank events, the available three-dimensional tracking angle error differential equation:

$\left\{\begin{array}{c}{\stackrel{\·}{\mathrm{\ϵ}}}_y={\mathrm{\ω}}_y-{\mathrm{\ω}}_{dy}+{\mathrm{\ω}}_{dx}{\mathrm{\ϵ}}_z\\ {\stackrel{\·}{\mathrm{\ϵ}}}_z={\mathrm{\ω}}_z-{\mathrm{\ω}}_{dz}-{\mathrm{\ω}}_{dx}{\mathrm{\ϵ}}_y\end{array}\right.---\left(4\right)$

In formula, ε_y,ε_zFor error angle, ω_y,ω_zFor line of sight rate, ω_dx,ω_dy,ω_dzFor optic angle speed.

(4) target seeker tenacious tracking controls integrated mathematical model with body attitude

The present invention proposes gesture stability and controls to carry out integrated design method with target seeker tenacious tracking.Refer to according to guidance Make, target seeker is measured plays body attitude information and the target seeker frame corners position that mesh relative movement information and inertial navigation components are measured Putting metrical information, by control law, directly the output angle of rudder reflection change motor-driven overload of body attitude generation strikes target, and drives simultaneously Dynamic target seeker frame movement, controls optical axis and realizes the accurate tracking to target.The most this mapping relations are linear, also right and wrong Linear, all can by guidanceing command, relative motion metrical information is tracked reaching to coordinate to control body attitude simultaneously Produce permissible load factor and make the targeted purpose of optical axis stable.

On the basis of first three walks, set up half strapdown seeker control mathematical model integrated with gesture stability, by integration Model is written as cascaded affine nonlinear system:

$\left\{\begin{array}{c}{\stackrel{\·}{x}}_1=f_1\left(x_1\right)+g_1\left(x_1\right)\left[\begin{array}{c}{x}_2\\ u_1\end{array}\right]\\ {\stackrel{\·}{x}}_2=f_2\left(x_2\right)+g_2\left(x_2\right)u_2\end{array}\right.---\left(5\right)$

In formula,

x₁=[α β γ_v ε_y ε_z]^T,

x₂=[ω_mx ω_my ω_mz]^T, u₂=[δ_x δ_y δ_z]^T,

$f_1\left(x_1\right)=\left[\begin{array}{c}\frac{{\mathrm{mgcos\γ}}_v\cos \mathrm{\θ}-L}{mV\cos \mathrm{\β}}\\ \frac{{\mathrm{mgsin\γ}}_v\cos \mathrm{\θ}+Y}{mV}\\ -{\stackrel{\·}{\mathrm{\ψ}}}_v\sin \mathrm{\θ}-\frac{({\mathrm{mgcos\γ}}_v\cos \mathrm{\θ}-L)\tan \mathrm{\β}}{mV}\\ {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z\end{array}\right],\begin{array}{c}{g}_1\left(x\right)=\left[\begin{array}{cc}-\cos \mathrm{\α}\tan \mathrm{\β}& \sin \mathrm{\α}\tan \mathrm{\β}\\ \sin \mathrm{\α}& \cos \mathrm{\α}\\ \cos \mathrm{\α}\sec \mathrm{\β}& -\sin \mathrm{\α}\sec \mathrm{\β}\\ {\mathrm{sin\λ}}_z{\mathrm{cos\λ}}_y+{\mathrm{\ϵ}}_z{\mathrm{cos\λ}}_z{\mathrm{cos\λ}}_y& -{\mathrm{cos\λ}}_z+{\mathrm{\ϵ}}_z{\mathrm{sin\λ}}_z\\ -({\mathrm{sin\λ}}_y+{\mathrm{\ϵ}}_y{\mathrm{cos\λ}}_z{\mathrm{cos\λ}}_y)& -{\mathrm{\ϵ}}_y{\mathrm{sin\λ}}_z\end{array}\right.\\ \left.\begin{array}{ccc}1& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ -({\mathrm{sin\λ}}_z{\mathrm{sin\λ}}_y+{\mathrm{\ϵ}}_z{\mathrm{cos\λ}}_z{\mathrm{sin\λ}}_y)& -{\mathrm{cos\λ}}_z+{\mathrm{\ϵ}}_z{\mathrm{sin\λ}}_z& 0\\ -{\mathrm{cos\λ}}_y+{\mathrm{\ϵ}}_y{\mathrm{cos\λ}}_z{\mathrm{sin\λ}}_y& -{\mathrm{\ϵ}}_y{\mathrm{sin\λ}}_z& -1\end{array}\right]\end{array},$

$f_2\left(x\right)=\left[\begin{array}{c}\frac{M_{x0}}{I_x}-\frac{(I_z-I_y){\mathrm{\ω}}_{mz}{\mathrm{\ω}}_{my}}{I_x}\\ \frac{M_{y0}}{I_y}-\frac{(I_x-I_z){\mathrm{\ω}}_{mx}{\mathrm{\ω}}_{mz}}{I_y}\\ \frac{M_{z0}}{I_z}-\frac{(I_y-I_x){\mathrm{\ω}}_{my}{\mathrm{\ω}}_{mx}}{I_z}\end{array}\right],g_2\left(x\right)=\left[\begin{array}{ccc}\frac{{\mathrm{QSL}}_r{m}_x^{{\mathrm{\δ}}_x}}{I_x}& 0& 0\\ 0& \frac{{\mathrm{QSL}}_r{m}_y^{{\mathrm{\δ}}_y}}{I_y}& 0\\ 0& 0& \frac{{\mathrm{QSL}}_r{m}_z^{{\mathrm{\δ}}_z}}{I_z}\end{array}\right].$

Step 2: integrated controller configuration designs

Based on singular perturbation theory design target seeker tenacious tracking with body attitude overall-in-one control schema configuration as shown in Figure 4. The flow angle instruction α that guidance system is given_c,β_c,γ_vc, frame corners position λ that frame corners position measurement sensor records_y,λ_z, bullet Visually line angle q_y,q_zThe bullet line of sight rate of rotation ω that device obtains after filtering_y,ω_z, and the trail angle that target seeker receiver obtains Error ε_y,ε_z, by outer ring controller output framework angular velocity control signal ω_λyc,ω_λzc, control optical axis and point to real-time tracking bullet Line of sight, provides pseudo-controlled quentity controlled variable ω of attitude angular velocity simultaneously_mxc,ω_myc,ω_mzc, then give internal ring attitude angular velocity control system System, controls pneumatic rudder and realizes ω_mxc,ω_myc,ω_mzcQuick tracking.

Step 3: controller based on nonlinear dynamic inverse designs

Design dynamic inversion control rule:

To following affine nonlinear system:

$\left\{\begin{array}{c}\stackrel{\·}{x}=f\left(x\right)+g\left(x\right)u\\ y=h\left(x\right)\end{array}\right.---\left(6\right)$

Design nonlinear dynamic inversion control device:

$u=g^{-1}\left(x\right)\[{\stackrel{\·}{x}}_{des}-f\left(x\right)\]---\left(7\right)$

In formula, it is desirable to dynamicHere it is also called pseudo-controlled quentity controlled variable, is designated asAbout choosing of pseudo-controlled quentity controlled variable, this Invention is chosen in the following way:

$v={\stackrel{\·}{x}}_{des}=K_c(x_c-x)---\left(8\right)$

What Fig. 5 schematically illustrated dynamic inverse realizes process.

The inner and outer ring loop of design integration model carries out Nonlinear Dynamic controller below.

(1) design outer shroud dynamic inversion control device

The state variable of definition external loop is [α β γ_v ε_y ε_z]^T, input control variable is, [ω_mx ω_my ω_mz ω_λy ω_λz]^T.Design outer shroud dynamic inversion control rule is as follows:

$\left[\begin{array}{c}{\mathrm{\ω}}_{mxc}\\ {\mathrm{\ω}}_{myc}\\ {\mathrm{\ω}}_{mzc}\\ {\mathrm{\ω}}_{\mathrm{\λ}y}\\ {\mathrm{\ω}}_{\mathrm{\λ}z}\end{array}\right]=g_1^{-1}\left(x_1\right)\left(\left[\begin{array}{c}{v}_{\mathrm{\α}}\\ v_{\mathrm{\β}}\\ v_{{\mathrm{\γ}}_v}\\ v_{{\mathrm{\ϵ}}_y}\\ v_{{\mathrm{\ϵ}}_z}\end{array}\right]-f\left(x_1\right)\right)---\left(9\right)$

In formula: v_α,v_β,It is respectively state variable α, β, γ_v,ε_y,ε_zDesired dynamic form such as formula (8), ω_mxc,ω_myc,ω_mzcInstruct for internal ring desired body attitude angular velocity, as the pseudo-controlled quentity controlled variable of internal ring.Thus obtain The dynamic inversion control device form of external loop.

(2) design internal ring dynamic inversion control device

Inner looping control instruction is external loop puppet controlled quentity controlled variable ω_mxc,ω_myc,ω_mzc, input control variable is δ_x,δ_y,δ_z.If Meter internal ring dynamic control law is as follows:

$\left[\begin{array}{c}{\mathrm{\δ}}_x\\ {\mathrm{\δ}}_y\\ {\mathrm{\δ}}_z\end{array}\right]=g_2^{-1}\left(x_2\right)\left(\left[\begin{array}{c}{v}_{{\mathrm{\ω}}_{mx}}\\ v_{{\mathrm{\ω}}_{my}}\\ v_{{\mathrm{\ω}}_{mz}}\end{array}\right]-f\left(x_2\right)\right)---\left(10\right)$

In formula:It is respectively state variable ω_mxc,ω_myc,ω_mzcDesired dynamic form such as formula (8).

Above two steps have obtained target seeker tenacious tracking based on inner-outer loop nonlinear dynamic inverse method for designing and body Attitude integrated controller.

Emulation case

If bullet visual line angle drift angle rate of rotation is frequency 0.1Hz, the cosine curve of 5 °/s of amplitude, playing line of sight inclination angle rate of rotation is Frequency 0.1Hz, the cosine curve of 10 °/s of amplitude.The angle of attack, yaw angle, the instruction of roll angle give 15 ° at emulation initial time, 10 ° With the step signal of 5 °.Emulation initial parameter is as shown in table 1.

Table 1 emulates initial condition

Simulation algorithm uses fixed step size Fourth order Runge-Kutta, and simulation step length is 1ms, and simulation time is 5s.Simulation result As shown in Fig. 6, Fig. 7, Fig. 8, Fig. 9, Figure 10.

From fig. 6, it can be seen that the angle of attack, yaw angle, speed roll angle α, β, γ_vTracking error restrain in 0.7s, partially Differential is bordering on 0；From figure 7 it can be seen that owing to initial error is bigger, the drift angle causing initial time rudder face is relatively big, but along with The reduction of tracking error, angle of rudder reflection gradually decreases to normal operation interval.This shows that the integrated controller of design is to body appearance The control of state has satisfied dynamic effect and less tracking error.

From Fig. 8, Fig. 9, Figure 10 it can be seen that target seeker error angle restrains in 0.7s and levels off to 0, position marker can be the most accurate True tracking target, target is positioned at the center of Seeker's light axis.During whole tenacious tracking, frame corners momentary rate is to the maximum 150deg/s, frame corners is 40deg to the maximum, and this all meets target seeker servo Frame Design and requires.Consider body attitude The overall-in-one control schema coupled between moving with position marker that moves ensure that the Seeker's light axis quick tracking to target, has relatively Little error angle.

Claims (1)

1. for an integrated control method for half strapdown guidance guided missile, including following step:

Step 1: set up target seeker tenacious tracking and control integrated mathematical model with body attitude, including body attitude kinesiology mould Type, frame movement model, angle tracking system model, integration mathematical model；

(1) body attitude kinematics model；

The guided missile flow angle differential equation is:

$\left[\begin{array}{c}\stackrel{\·}{\mathrm{\α}}\\ \stackrel{\·}{\mathrm{\β}}\\ {\stackrel{\·}{\mathrm{\γ}}}_v\end{array}\right]=\left[\begin{array}{c}\frac{mg{\mathrm{cos\γ}}_{v}cos\mathrm{\θ}-L}{mV\cos \mathrm{\β}}\\ \frac{mg{\mathrm{sin\γ}}_{v}cos\mathrm{\θ}+Y}{mV}\\ -{\stackrel{\·}{\mathrm{\ψ}}}_{v}sin\mathrm{\θ}-\frac{(mg{\mathrm{cos\γ}}_{v}cos\mathrm{\θ}-L)tan\mathrm{\β}}{mV}\end{array}\right]+\left[\begin{array}{ccc}-cos\mathrm{\α}tan\mathrm{\β}& \sin \mathrm{\α}tan\mathrm{\β}& 1\\ \sin \mathrm{\α}& cos\mathrm{\α}& 0\\ \cos \mathrm{\α}\sec \mathrm{\β}& -\sin \mathrm{\α}\sec \mathrm{\β}& 0\end{array}\right]\left[\begin{array}{c}{\mathrm{\ω}}_{mx}\\ {\mathrm{\ω}}_{my}\\ {\mathrm{\ω}}_{mz}\end{array}\right]---\left(1\right)$

In formula, α, beta, gamma_vIt is respectively the angle of attack, yaw angle, angle of heel；θ,ψ_vIt is respectively trajectory tilt angle, trajectory deflection angle；L, Y are respectively Lift and side force；ω_mx,ω_my,ω_mzFor body body axle attitude angular velocity；M is guided missile quality, and g is acceleration of gravity, and V is for leading Play flight speed；

The attitude angular velocity differential equation is:

$\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\ω}}}_{mx}\\ {\stackrel{\·}{\mathrm{\ω}}}_{my}\\ {\stackrel{\·}{\mathrm{\ω}}}_{mz}\end{array}\right]=\left[\begin{array}{c}\frac{M_{x0}}{I_x}-\frac{(I_z-I_y){\mathrm{\ω}}_{mz}{\mathrm{\ω}}_{my}}{I_x}\\ \frac{M_{y0}}{I_y}-\frac{(I_x-I_z){\mathrm{\ω}}_{mx}{\mathrm{\ω}}_{mz}}{I_y}\\ \frac{M_{z0}}{I_z}-\frac{(I_y-I_x){\mathrm{\ω}}_{my}{\mathrm{\ω}}_{mx}}{I_z}\end{array}\right]+\left[\begin{array}{ccc}\frac{{\mathrm{QSL}}_r{m}_x^{{\mathrm{\δ}}_x}}{I_x}& 0& 0\\ 0& \frac{{\mathrm{QSL}}_r{m}_y^{{\mathrm{\δ}}_y}}{I_y}& 0\\ 0& 0& \frac{{\mathrm{QSL}}_r{m}_z^{{\mathrm{\δ}}_z}}{I_z}\end{array}\right]\left[\begin{array}{c}{\mathrm{\δ}}_x\\ {\mathrm{\δ}}_y\\ {\mathrm{\δ}}_z\end{array}\right]---\left(2\right)$

In formula, M_x0,M_y0,M_z0It it is the aerodynamic moment under the zero inclined state of rudder；For rolling moment coefficient to δ_xDerivative,For rolling Torque coefficient is to δ_yDerivative,For rolling moment coefficient to δ_zDerivative；Dynamic pressure Q=0.5 ρ V², ρ is atmospheric density；S is Guided missile area of reference；L_rFor guided missile reference length；δ_x,δ_y,δ_zFor rolling, driftage, the three-channel angle of rudder reflection of pitching；I_x,I_y,I_zPoint Wei the rotary inertia in three directions of guided missile；

(2) frame movement model；

The motion in space of Seeker's light axis center is:

ω_d=ω_dm+ω_ds (3)

In formula,

${\mathrm{\ω}}_{dm}=\left[\begin{array}{c}{\mathrm{\ω}}_{mx}{\mathrm{cos\λ}}_z{\mathrm{cos\λ}}_y+{\mathrm{\ω}}_{my}{\mathrm{sin\λ}}_z-{\mathrm{\ω}}_{mz}{\mathrm{cos\λ}}_z{\mathrm{sin\λ}}_y\\ -{\mathrm{\ω}}_{mx}{\mathrm{sin\λ}}_z{\mathrm{cos\λ}}_y+{\mathrm{\ω}}_{my}{\mathrm{cos\λ}}_z+{\mathrm{\ω}}_{mz}{\mathrm{sin\λ}}_z{\mathrm{sin\λ}}_y\\ {\mathrm{\ω}}_{mx}{\mathrm{sin\λ}}_y+{\mathrm{\ω}}_{mz}{\mathrm{cos\λ}}_y\end{array}\right]{\mathrm{\ω}}_{dx}=\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\λ}}}_y{\mathrm{sin\λ}}_z\\ {\stackrel{\·}{\mathrm{\λ}}}_y{\mathrm{cos\λ}}_z\\ {\stackrel{\·}{\mathrm{\λ}}}_z\end{array}\right]$

Wherein, ω_dFor the projection in detection coordinate system of the optic angle speed, ω_dmFor body angle speed in detection coordinate system Projection, ω_dsFor the projection in detection coordinate system of the target seeker servo frame corners speed, ω_mx,ω_my,ω_mzFor body body shaft angle speed Degree, λ_y,λ_zIt it is the internal and external frame angle of half strap down stability platform；

(3) angle tracking system model；

The three-dimensional tracking angle error differential equation is:

$\left\{\begin{array}{c}{\stackrel{\·}{\mathrm{\ϵ}}}_y={\mathrm{\ω}}_y-{\mathrm{\ω}}_{dy}+{\mathrm{\ω}}_{dx}{\mathrm{\ϵ}}_z\\ {\stackrel{\·}{\mathrm{\ϵ}}}_z={\mathrm{\ω}}_z-{\mathrm{\ω}}_{dz}-{\mathrm{\ω}}_{dx}{\mathrm{\ϵ}}_y\end{array}\right.---\left(4\right)$

In formula, ε_y,ε_zFor error angle, ω_y,ω_zFor line of sight rate, ω_dx,ω_dy,ω_dzFor optic angle speed；

(4) target seeker tenacious tracking controls integrated mathematical model with body attitude；

Set up half strapdown seeker control mathematical model integrated with gesture stability:

$\left\{\begin{array}{c}{\stackrel{\·}{x}}_1=f_1\left(x_1\right)+g_1\left(x_1\right)\left[\begin{array}{c}{x}_2\\ u_1\end{array}\right]\\ {\stackrel{\·}{x}}_2=f_2\left(x_2\right)+g_2\left(x_2\right)u_2\end{array}\right.---\left(5\right)$

In formula,

x₁=[α β γ_v ε_y ε_z]^T,

x₂=[ω_mx ω_my ω_mz]^T, u₂=[δ_x δ_y δ_z]^T,

$f_1\left(x_1\right)=\left[\begin{array}{c}\frac{mg{\mathrm{cos\γ}}_v\cos \mathrm{\θ}-L}{mV\cos \mathrm{\β}}\\ \frac{mg{\mathrm{sin\γ}}_v\cos \mathrm{\θ}+Y}{mV}\\ -{\stackrel{\·}{\mathrm{\ψ}}}_v\sin \mathrm{\θ}-\frac{(mg{\mathrm{cos\γ}}_v\cos \mathrm{\θ}-L)\tan \mathrm{\β}}{mV}\\ {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z\end{array}\right],\begin{array}{c}{g}_1\left(x\right)=\left[\begin{array}{cc}-\cos \mathrm{\α}\tan \mathrm{\β}& \sin \mathrm{\α}\tan \mathrm{\β}\\ \sin \mathrm{\α}& \cos \mathrm{\α}\\ \cos \mathrm{\α}\sec \mathrm{\β}& -\sin \mathrm{\α}\sec \mathrm{\β}\\ {\mathrm{sin\λ}}_z{\mathrm{cos\λ}}_y+{\mathrm{\ϵ}}_z{\mathrm{cos\λ}}_z{\mathrm{cos\λ}}_y& -{\mathrm{cos\λ}}_z+{\mathrm{\ϵ}}_z{\mathrm{sin\λ}}_z\\ -({\mathrm{sin\λ}}_y+{\mathrm{\ϵ}}_y{\mathrm{cos\λ}}_z{\mathrm{cos\λ}}_y)& -{\mathrm{\ϵ}}_y{\mathrm{sin\λ}}_z\end{array}\right.\\ \left.\begin{array}{ccc}1& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ -({\mathrm{sin\λ}}_z{\mathrm{cos\λ}}_y+{\mathrm{\ϵ}}_z{\mathrm{cos\λ}}_z{\mathrm{sin\λ}}_y)& -{\mathrm{cos\λ}}_z+{\mathrm{\ϵ}}_z{\mathrm{sin\λ}}_z& 0\\ -{\mathrm{cos\λ}}_y+{\mathrm{\ϵ}}_y{\mathrm{cos\λ}}_z{\mathrm{sin\λ}}_y& -{\mathrm{\ϵ}}_y{\mathrm{sin\λ}}_z& -1\end{array}\right],\end{array}$

$f_2\left(x\right)=\left[\begin{array}{c}\frac{M_{x0}}{I_x}-\frac{(I_z-I_y){\mathrm{\ω}}_{mz}{\mathrm{\ω}}_{my}}{I_x}\\ \frac{M_{y0}}{I_y}-\frac{(I_x-I_z){\mathrm{\ω}}_{mx}{\mathrm{\ω}}_{mz}}{I_y}\\ \frac{M_{z0}}{I_z}-\frac{(I_y-I_x){\mathrm{\ω}}_{my}{\mathrm{\ω}}_{mx}}{I_z}\end{array}\right],g_2\left(x\right)=\left[\begin{array}{ccc}\frac{{\mathrm{QSL}}_r{m}_x^{{\mathrm{\δ}}_x}}{I_x}& 0& 0\\ 0& \frac{{\mathrm{QSL}}_r{m}_y^{{\mathrm{\δ}}_y}}{I_y}& 0\\ 0& 0& \frac{{\mathrm{QSL}}_r{m}_z^{{\mathrm{\δ}}_z}}{I_z}\end{array}\right];$

Step 2: design integration controller configuration；

It is equipped with the flow angle instruction α that guiding systems provides_c,β_c,γ_vc, frame corners position that frame corners position measurement sensor records λ_y,λ_z, bullet visual line angle q_y,q_zThe bullet line of sight rate of rotation ω that device obtains after filtering_y,ω_z, and target seeker receiver obtains Tracking angle error ε_y,ε_z, by outer ring controller output framework angular velocity control signal ω_λyc,ω_λzc, control optical axis and point in real time Follow the tracks of and play line of sight, provide pseudo-controlled quentity controlled variable ω of attitude angular velocity simultaneously_mxc,ω_myc,ω_mzc, then give internal ring attitude angular velocity Control system, controls pneumatic rudder and realizes ω_mxc,ω_myc,ω_mzcQuick tracking；

Step 3: controller based on nonlinear dynamic inverse designs；

(1) design outer shroud dynamic inversion control device；

If the state variable of external loop is [α β γ_v ε_y ε_z]^T, input control variable is, [ω_mx ω_my ω_mz ω_λy ω_λz ]^T；Then outer shroud dynamic inversion control device is:

$\left[\begin{array}{c}{\mathrm{\ω}}_{mxc}\\ {\mathrm{\ω}}_{myc}\\ {\mathrm{\ω}}_{mzc}\\ {\mathrm{\ω}}_{\mathrm{\λ}y}\\ {\mathrm{\ω}}_{\mathrm{\λ}z}\end{array}\right]=g_1^{-1}\left(x_1\right)(\left[\begin{array}{c}{v}_{\mathrm{\α}}\\ v_{\mathrm{\β}}\\ v_{{\mathrm{\γ}}_v}\\ v_{{\mathrm{\ϵ}}_y}\\ v_{{\mathrm{\ϵ}}_z}\end{array}\right]-f(x_1\left)\right)---\left(9\right)$

In formula:It is respectively state variable α, β, γ_v,ε_y,ε_zDesired dynamic form, ω_mxc,ω_myc,ω_mzcFor Internal ring desired body attitude angular velocity instructs, as the pseudo-controlled quentity controlled variable of internal ring；

(2) design internal ring dynamic inversion control device；

If inner looping control instruction is external loop puppet controlled quentity controlled variable ω_mxc,ω_myc,ω_mzc, input control variable is δ_x,δ_y,δ_z；

Then internal ring dynamic inversion control device:

$\left[\begin{array}{c}{\mathrm{\δ}}_x\\ {\mathrm{\δ}}_y\\ {\mathrm{\δ}}_z\end{array}\right]=g_2^{-1}\left(x_2\right)(\left[\begin{array}{c}{v}_{{\mathrm{\ω}}_{mx}}\\ v_{{\mathrm{\ω}}_{my}}\\ v_{{\mathrm{\ω}}_{mz}}\end{array}\right]-f(x_2\left)\right)---\left(10\right)$

In formula:It is respectively state variable ω_mxc,ω_myc,ω_mzcDesired dynamic form such as formula (8)；

Respectively obtain outer shroud dynamic inversion control device and internal ring dynamic inversion control device by formula (9) and (10), complete the one of guided missile Change and control.