CN114280932B - Carrier pose integrated control method considering dynamic characteristics of servo mechanism - Google Patents
Carrier pose integrated control method considering dynamic characteristics of servo mechanism Download PDFInfo
- Publication number
- CN114280932B CN114280932B CN202111530481.3A CN202111530481A CN114280932B CN 114280932 B CN114280932 B CN 114280932B CN 202111530481 A CN202111530481 A CN 202111530481A CN 114280932 B CN114280932 B CN 114280932B
- Authority
- CN
- China
- Prior art keywords
- servo
- current time
- carrier
- command
- displacement
- 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 47
- 230000007246 mechanism Effects 0.000 title claims abstract description 34
- 239000011159 matrix material Substances 0.000 claims abstract description 66
- 238000006073 displacement reaction Methods 0.000 claims abstract description 64
- 239000013598 vector Substances 0.000 claims abstract description 59
- 238000001914 filtration Methods 0.000 claims abstract description 45
- 230000008878 coupling Effects 0.000 claims abstract description 26
- 238000010168 coupling process Methods 0.000 claims abstract description 26
- 238000005859 coupling reaction Methods 0.000 claims abstract description 26
- 238000004364 calculation method Methods 0.000 claims description 21
- 230000008859 change Effects 0.000 claims description 21
- 239000010720 hydraulic oil Substances 0.000 claims description 10
- 230000008569 process Effects 0.000 claims description 9
- 230000003321 amplification Effects 0.000 claims description 5
- 238000013016 damping Methods 0.000 claims description 5
- 238000003199 nucleic acid amplification method Methods 0.000 claims description 5
- 230000004044 response Effects 0.000 claims description 5
- 238000012937 correction Methods 0.000 claims description 2
- 238000009434 installation Methods 0.000 claims description 2
- 238000013461 design Methods 0.000 abstract description 21
- 238000005516 engineering process Methods 0.000 abstract description 3
- 238000004422 calculation algorithm Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000002457 bidirectional effect Effects 0.000 description 1
- 239000000969 carrier Substances 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 239000007788 liquid Substances 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000005096 rolling process Methods 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
- 238000004804 winding Methods 0.000 description 1
Landscapes
- Feedback Control In General (AREA)
Abstract
The invention discloses a carrier pose integrated control method considering dynamic characteristics of a servo mechanism, which comprises the steps of firstly obtaining task parameters, carrier overall parameters, an execution mechanism configuration matrix, servo system dynamic parameters and current carrier state parameters, obtaining the current servo system state parameters when the track-in requirement is not met, sequentially calculating engine control thrust and command attitude angle, filtering command attitude angle and angular velocity, correcting the matrix when judging that an attitude motion coupling matrix is not singular, continuously calculating current servo actuator command displacement, servo actuator filtering command displacement and velocity, servo actuator hydraulic cylinder command load pressure, servo actuator hydraulic cylinder filtering command load pressure and the like, and finally outputting a control voltage vector and a thrust vector. According to the invention, through the coupling dynamics of the servo-gesture-position of the system carrier, the influence of gesture control time delay on position control is effectively compensated by combining a filtering design technology under a backstepping design framework.
Description
Technical Field
The invention relates to a carrier pose integrated control method considering dynamic characteristics of a servo mechanism, and belongs to the technical field of aircraft dynamics and control.
Background
In order to realize space tasks such as orbit entering, inter-planetary orbit transfer, orbit maneuver and the like, the carrier represented by the upper stage, the space transportation stage and the rocket final stage is often provided with one or two high-thrust liquid engines so as to ensure that enough main power reaches the required position and speed in a limited time, and the posture of the carrier is adjusted through a servo mechanism or an RCS in the whole flying process so as to realize thrust vector control.
In the conventional design mode of the current carrier pose control system, the guidance design (i.e. the position control design) and the pose control design are often independently performed after the respective performance indexes are allocated, and the formed control instructions are deduced through the degree of freedom simulation of the carrier 6 and iterated repeatedly so as to meet the track precision of the carrier motion terminal and the constraint of a servo mechanism (such as amplitude constraint, dynamic performance constraint and the like) in the flight process. Although the gesture control design mode of 'divide and conquer' is easy for subsystem control algorithm design, in principle, the tracking delay of a gesture control subsystem to a guidance instruction can definitely influence the track control precision, and the design mode needs to perform performance index distribution in advance, so that the final design index has conservation.
Disclosure of Invention
The invention aims to overcome the defects, and provides a carrier pose integrated control method considering dynamic characteristics of a servo mechanism, which comprises the steps of firstly obtaining a task parameter, a carrier overall parameter, an execution mechanism configuration matrix, a servo system dynamic parameter and a carrier state parameter at the current moment, judging whether the track entering requirement is met, ending the control process if the track entering requirement is met, otherwise obtaining the servo system state parameter at the current moment, sequentially calculating engine control thrust and command attitude angle, a filtering command attitude angle and angular velocity at the current moment, judging whether a pose motion coupling matrix is singular or not, correcting the matrix if the pose motion coupling matrix is not singular, continuously calculating the servo actuator command displacement at the current moment, the filtering command displacement and velocity of a servo actuator, the servo actuator command load pressure, the filtering command load pressure and change rate of the servo actuator hydraulic cylinder, the servo actuator command spool displacement, the servo actuator spool filtering command displacement and change rate of the servo actuator, inputting a control voltage vector by the servo actuator, and finally outputting the control voltage vector and the thrust vector. According to the invention, through the coupling dynamics of the servo-gesture-position of the system carrier, under the back-step design framework, the influence of gesture control time delay on position control is effectively compensated by combining with the filtering design technology, the carrier position control precision is improved in principle, the overall design allowance can be effectively released from the mode, and the design efficiency can be greatly improved from the design flow.
In order to achieve the above purpose, the present invention provides the following technical solutions:
a carrier pose integrated control method considering dynamic characteristics of a servo mechanism comprises the following steps:
(1) Acquiring a carrier in-orbit task parameter;
(2) Acquiring overall parameters of the carrier, and calculating an execution mechanism configuration matrix according to the overall parameters of the carrier;
(3) Acquiring dynamic parameters of a servo system;
(4) Acquiring the current time t j J=0, 1,2, …;
(5) According to the current time t obtained in the step (4) j Judging whether the carrier meets the track-in requirement or not according to the carrier state parameters and the carrier track-in task parameters obtained in the step (1), and ending the control process if the carrier meets the track-in requirement; otherwise, entering step (6);
(6) Acquiring the current time t j Servo system state parameters of (a);
(7) According to the carrier in-orbit task parameters obtained in the step (1) and the current time t obtained in the step (4) j The current time t is calculated by the carrier state parameter of (2) j Controlling thrust and command attitude angles of an engine;
(8) According to the current time t obtained in the step (7) j The instruction attitude angle of the engine and the carrier in-orbit task parameters obtained in the step (1) calculate the current time t j A filter command attitude angle and a filter command angular velocity of the vehicle;
(9) Judging the current time t j Whether the attitude motion coupling matrix G (theta) of the carrier is singular or not, if so, entering the step (10), otherwise, entering the step (11);
(10) Correcting the attitude motion coupling matrix G (theta), and entering a step (11);
(11) According to the current time t obtained in the step (7) j Controlling thrust of engine and current time t obtained in step (8) j A filtering command attitude angle and a filtering command angular speed of the carrier, an attitude motion coupling matrix G (theta) obtained in the step (9) or the step (10), a mechanism configuration matrix obtained in the step (2), and a current time t obtained in the step (4) j The current time t is calculated by the carrier state parameter of (2) j Is displaced by the servo actuator command;
(12) According to the current time t obtained in the step (11) j The servo actuator command displacement of the step (6) and the current time t obtained in the step (6) j The servo system state parameter of the (1) and the carrier track-in task parameter obtained in the step (1) are calculated to obtain the current time t j A filtering instruction displacement and a filtering instruction speed of the servo actuator;
(13) According to the current time t obtained in the step (12) j Filtering instruction displacement and filtering instruction displacement speed of servo actuator and current time t obtained in step (6) j The state parameters of the servo system and the dynamic parameters of the servo system obtained in the step (3) are calculated to obtain the current time t j A hydraulic cylinder of the servo actuator commands load pressure;
(14) According to the current time t obtained in the step (13) j Instructing load pressure by a hydraulic cylinder of a servo actuator and obtaining the current time t in the step (6) j Is a servo system of (2)Calculating the current time t by the state parameters and the carrier in-orbit task parameters obtained in the step (1) j The method comprises the steps of filtering command load pressure and filtering command load pressure change rate of a hydraulic cylinder of a servo actuator;
(15) According to the current time t obtained in the step (14) j Filtering command load pressure of a hydraulic cylinder of a servo actuator, filtering command load pressure change rate, and current time t obtained in step (6) j Calculating the current time t by the servo system state parameter and the servo system dynamics parameter obtained in the step (3) j The servo actuator instructs the valve core to displace;
(16) According to the current time t obtained in the step (15) j The servo actuator instructs the valve core to displace, and the current time t obtained in the step (6) is obtained j The servo system state parameter of the (1) and the carrier track-in task parameter obtained in the step (1) are calculated to obtain the current time t j Filtering instruction displacement and displacement change rate of a valve core of a servo actuator;
(17) According to the current time t obtained in step (16) j Filtering instruction displacement and change rate of valve core of servo actuator and current time t obtained in step (6) j The state parameters of the servo system and the dynamic parameters of the servo system obtained in the step (3) are calculated to obtain the current time t j Inputting a control voltage vector by the servo actuator;
(18) According to the current time t obtained in step (17) j Inputting a control voltage vector to the servo actuator and obtaining the current time t in the step (7) j The engine control thrust controls the carrier, and returns to step (4).
Further, in the step (1), the carrier-track task parameter includes a desired position r of the track-track task carrier d Desired speed v d Desired track-in accuracy e ss And a control period T;
in the step (2), the overall parameters of the vehicle include the specific impulse I of the main engine of the vehicle sp The radius R of the rocket body of the carrier, the distance D between the servo actuator and the central line of the engine and the distance H between the plane formed by the mounting points on the servo actuator and the mass center of the carrier G Mounting point A of engine A and engine B G And B G Distance servoDistance l of plane formed by mounting points on actuator G ；
In the step (3), the dynamic parameters of the servo system comprise a load mass matrix M and a cross section area A of the hydraulic cylinder p Load stiffness k, damping b, half volume V of hydraulic cylinder t Bulk modulus of elasticity beta of hydraulic oil e Leakage coefficient C of hydraulic cylinder tl Servo valve flow coefficient C d Servo valve area gradient w, hydraulic oil density ρ, hydraulic pump supply pressure p s Servo valve response time constant T sv And servo valve amplification gain k sv ；
In the step (4), the current time t j The carrier status parameter of (c) comprises a carrier mass m (t j ) Moment of inertia of the vehicle J (t j ) Vehicle position vector r (t j ) Vehicle velocity vector v (t j ) Vehicle attitude angle vector θ (t j ) And a carrier attitude angular velocity vector ω (t) j )；
In the step (6), the current time t j The servo system state parameters of (1) include a load moment vector F L (t j ) Displacement variation Δl (t) j ) Displacement change rate of servo actuatorHydraulic cylinder load pressure vector p (t) j ) And servo valve spool displacement vector x v (t j )。
Further, in the step (2), the calculation formula of the actuator configuration matrix H is:
further, in the step (5), the method for judging whether the carrier meets the track-in requirement is as follows:
if meeting r (t) j )-r d ||≤e ss The carrier meets the in-orbit requirement, otherwise, the carrier does not meet the in-orbit requirement.
Further, the method comprises the steps of,in the step (7), the current time t j Engine control thrust force F (t) j ) And the attitude angle θ of the vehicle command c (t j ) The calculation formula is as follows:
wherein,
in xi r Is [0,1]Real number, positive real number k in interval r And k v The position control gain and the speed control gain, respectively.
Further, in the step (8), the current time t j Filtering instruction attitude angle theta of carrier f (t j ) Filtered commanded angular velocityThe calculation formula of (2) is as follows:
wherein τ ψ Andrespectively yaw filter time constant and pitch filter time constant, and psi c (t 0 )＝ψ f (t 0 ),t 0 Is the initial time.
Further, in the step (9), the current time t is determined j The method for judging whether the attitude motion coupling matrix G (theta) of the carrier is singular or not is that the current time t is judged j Whether rank (G (θ)) of attitude motion coupling matrix G (θ) of carrier is smaller than3, if less than 3, G (θ) is singular, and if not less than 3, G (θ) is not singular;
in the step (10), the formula for correcting the gesture motion coupling matrix G (θ) is as follows: g (θ) =g (θ) +εe 3 Wherein ε is a correction coefficient, ε is 0.001 or less, E 3 Is a third-order identity matrix.
Further, in the step (11), the current time t j Is a servo actuator command displacement Deltal c (t j ) The calculation formula of (2) is as follows:
wherein the method comprises the steps of
Wherein H is an actuator configuration matrix,configuring generalized inverse matrix, ζ of matrix H for actuator θ Is [0,1]Real number, positive real number k in interval θ And k ω An attitude control gain and an angular velocity control gain, respectively, < >>The derivative matrix is the inverse of the gesture motion coupling matrix G (θ).
Further, in the step (12), the current time t j Filter command displacement al of servo actuator f (t j ) Filtering command speedThe calculation formula of (2) is as follows:
wherein Deltal c (t j ) For the current time t j Is displaced by the servo actuator command τ l Is a servo filtering time constant, and Deltal f (t 0 )＝0,t 0 Is the initial time.
Further, in the step (13), the current time t j Servo actuator hydraulic cylinder command load pressure p c (t j ) The calculation formula of (2) is as follows:
wherein A is p Is the cross-sectional area of the hydraulic cylinder, K L And K M A servo displacement gain matrix and a servo displacement rate gain matrix respectively, which satisfy
Wherein, the values of the parameters satisfy the following relations:
for the elements in the load mass matrix M +.>j＝1,2,i＝A,B。
Further, in the step (14), the current time t j Filtering instruction load pressure p of hydraulic cylinder of servo actuator f (t j ) Load pressure rate of change of filter commandThe calculation formula of (2) is as follows:
wherein τ p Filter time constant for hydraulic cylinder, and
p f (t 0 )＝p(t 0 ),t 0 is the initial time.
Wherein p (t) 0 ) The hydraulic cylinder load pressure vector is the initial time.
Further, in the step (15), the current time t j Is used for commanding the valve core displacement x of the servo actuator vc (t j ) The calculation formula of (2) is as follows:
wherein k is p Gain is controlled for the hydraulic cylinder pressure;
where, for the servo actuators j, i=a, B, j=1, 2,for spool displacement of servo valve, +.>Is the hydraulic cylinder load pressure.
Further, in the step (16), the current time t j Valve core filtering instruction displacement x of servo actuator vf (t j ) Rate of change of displacementThe calculation formula of (2) is as follows:
wherein τ xv Filter time constant for valve core, and x vf (t 0 )＝x v (t 0 ),x v (t 0 ) The displacement vector of the valve core of the servo actuator is the initial time, t 0 Is the initial time.
Further, in the step (17), the current time t j The servo actuator inputs a control voltage vector u (t j ) The calculation formula of (2) is as follows:
wherein k is u Gain is controlled for spool movement.
Compared with the prior art, the invention has the following beneficial effects:
(1) The invention provides a carrier pose integrated control design method considering servo mechanism characteristics, which comprehensively considers a position control loop, a pose control loop and a servo motion loop, fully utilizes cascading characteristics presented by a system, realizes integrated control instruction design under the framework of backstepping design, and effectively compensates the influence of the pose control loop and servo dynamics on track control precision;
(2) In the control method, a control coupling matrix representation method is provided, the nonlinear dependence of a thrust vector on the Euler angle of the carrier is effectively disclosed in an explicit form, and cascading representation of a carrier 'servo-gesture-position' coupling dynamics system is completed, so that pose integrated design is possible;
(3) In the control method, a filtering virtual control design method is provided, under the back-step control design framework, the complexity of a control algorithm can be effectively reduced, the expansion of the series is avoided, and the resolvability of a control scheme is improved.
Drawings
FIG. 1 is a schematic illustration of the installation of a servo actuator and a main engine of the present invention; fig. 1 (a) is a perspective view, and fig. 1 (b) is a bottom view;
FIG. 2 is a flow chart of a method for integrated control of the attitude of a vehicle taking into account the dynamics of the servo mechanism.
Detailed Description
The features and advantages of the present invention will become more apparent and clear from the following detailed description of the invention.
The word "exemplary" is used herein to mean "serving as an example, embodiment, or illustration. Any embodiment described herein as "exemplary" is not necessarily to be construed as preferred or advantageous over other embodiments. Although various aspects of the embodiments are illustrated in the accompanying drawings, the drawings are not necessarily drawn to scale unless specifically indicated.
(1) Acquiring mission parameters including desired position r of an on-track mission vehicle d And a desired velocity v d Desired track-in accuracy e ss The period T is controlled.
(2) Acquiring overall parameters of a carrier and an execution mechanism configuration matrix, wherein the overall parameters comprise specific impulse I of a main engine of the carrier sp The radius R of the rocket body of the carrier, the distance D between the servo actuator and the central line of the engine and the distance H between the plane formed by the mounting points on the servo actuator and the mass center of the carrier G Mounting point A of engine A and engine B G And B G Distance l from the plane G And calculates an actuator configuration matrix H.
(3) Obtaining dynamic parameters of a servo system, wherein the dynamic parameters comprise a load mass matrix M and a cross section area A of a hydraulic cylinder p Load stiffness k, damping b, half volume V of hydraulic cylinder t Bulk modulus of elasticity beta of hydraulic oil e Leakage coefficient C of hydraulic cylinder tl Servo valve flow coefficient C d Servo valve area gradient w, hydraulic oil density ρ, hydraulic pump supply pressure p s Servo valve response time constant T sv Servo valve amplification gain k sv 。
(4) Acquiring the current time (t) j Moment, j=0, 1,2, …) carrier state parameters, including mass m (t j ) Moment of inertia J (t j ) Position vector r (t j ) Velocity vector v (t j ) Attitude angle vector θ (t j ) And an attitude angular velocity vector ω (t) j )。
(5) Judging whether the track-in requirement is met, and if so, ending the control process; otherwise, go to step (6).
(6) Acquiring the current time (t) j Moment, j=0, 1,2, …) servo system state parameters, including the load moment vector F L (t j ) Displacement variation Δl (t) j ) Rate of changeHydraulic cylinder load pressure vector p (t) j ) Valve core displacement vector x of servo valve v (t j )。
(7) Calculate the current time (t) j Moment, j=0, 1,2, …) engine control thrust and commanded attitude angle.
(8) Calculate the current time (t) j Moment, j=0, 1,2, …).
(9) Judging the current time (t) j At the moment, j=0, 1,2, …) is singular, if the gesture motion coupling matrix G (θ) is singular, the step (10) is entered, otherwise the step (11) is entered.
(10) And correcting the gesture motion coupling matrix.
(11) Calculate the current time (t) j At time j=0, 1,2, …).
(12) Calculate the current time (t) j At time j=0, 1,2, …).
(13) Calculate the current time (t) j Moment, j=0, 1,2, …) of the servo actuator hydraulic cylinder command load pressure p c (t j )。
(14) Calculate the current time (t) j Moment, j=0, 1,2, …) of the servo actuator hydraulic cylinder, and the change rate thereof。
(15) Calculate the current time (t) j Moment j=0, 1,2, …) of the servo actuator command spool displacement x vc (t j )。
(16) Calculate the current time (t) j At time j=0, 1,2, …) the servo actuator spool filters the commanded displacement and rate of change.
(17) Calculate the current time (t) j At the moment j=0, 1,2, …), the servo actuator inputs a control voltage vector u (t) j )。
(18) Output the current time (t) j Control voltage vector u (t) at time j=0, 1,2, … j ) And thrust vector F (t) j ) Control is exercised and step (4) is returned.
Example 1:
the invention can be applied to the problem of high-precision control of the orbit entering tasks of the carriers such as the upper stage, the space transportation stage, the rocket final stage and the like. As shown in FIG. 1, O-X b Y b Z b The main engine is configured in parallel by two machines for the coordinate system of the carrier body, and the bidirectional swinging of the engine is realized through an electrohydraulic servo system. The engine A swings through the servo actuators SA1 and SA2, the engine B swings through the servo actuators SB1 and SB2, the servo actuators SA1, SA2, SB1 and SB2 are all vertically installed, and the distance from the center line of the engine is D. The upper mounting points of the four servo actuators are A12, A22, B12 and B22 respectively, and the lower mounting points are A21, A11, B11 and B21 respectively. The distance between the plane formed by the mounting points on the servo mechanism and the mass center of the carrier is H G While mounting points A of engine A and engine B G And B G Distance from the plane is l G 。
The invention is described in further detail below with reference to the accompanying drawings:
1) Relationship between linear motion of the servo actuator and swinging motion of the engine:
the motion relationship between the engine a and the servo actuators SA1 and SA2 is:
wherein alpha is A And beta A Respectively the engine A winds Z b Axes and Y b The angle of rotation of the shaft,and->The linear displacement changes of the servo actuators SA1 and SA2, respectively.
The motion relationship between the engine B and the servo actuators SB1 and SB2 is:
wherein alpha is B And beta B Respectively the engine B winds Z b Axes and Y b The angle of rotation of the shaft,and->The linear displacement of the servo actuators SB1 and SB2, respectively.
2) Dynamic model of servo actuator
The hydraulic servo dynamics model is characterized as follows:
wherein,
wherein A is p Is the cross section area of the hydraulic cylinder, k is the load rigidity, b is the damping, V t Is half volume of hydraulic cylinder beta e For the bulk modulus of elasticity of hydraulic oil, C tl Is the leakage coefficient of the hydraulic cylinder, C d Is the flow coefficient of the servo valve, w is the area gradient of the servo valve, ρ is the hydraulic oil density, and p s Supply pressure for hydraulic pump, T sv For servo valve response time constant, k sv For servo valve amplification gain, for servo actuator j of engine i (i=a, B, j=1, 2),for load mass +.>For hydraulic cylinder load pressure,/-)>For spool displacement of servo valve, +.>For load moment +.>For inputting a control voltage.
3) Carrier attitude dynamics model
The attitude kinematics and dynamics model of the vehicle body coordinate system relative to the inertial system are characterized as follows:
wherein J is a carrier moment of inertia matrix, and θ and ω are an attitude angle vector and an attitude angular velocity vector of the carrier, respectively, as described below
Wherein, gamma is the rolling angle of the carrier, psi is the yaw of the carrier,for carrier pitch angle omega x ,ω y ,ω z Respectively a carrier winding system X b Axis, Y b Axis and Z b The angular velocity of the shaft, F is the engine thrust, G (θ) is the attitude motion coupling matrix, H is the configuration matrix, described below
Wherein R is the radius of the carrier rocket body.
4) Vehicle position dynamics model
The model of the position dynamics of the vehicle under the inertial frame is characterized as follows:
wherein, vectors r and v are the position vector and velocity vector of the carrier relative to the earth, m is the carrier mass, μ is the gravitational constant, r is the absolute value of vector r, and vector u (θ) can be described as:
5) As shown in fig. 2, a method for controlling the pose of a vehicle in an integrated manner, which takes the dynamic characteristics of a servo mechanism into consideration, comprises the following steps:
(1) Acquiring mission parameters including desired position r of an on-track mission vehicle d And a desired velocity v d Desired track-in accuracy e ss The period T is controlled.
(2) Acquiring overall parameters of a carrier and an execution mechanism configuration matrix, wherein the overall parameters comprise specific impulse I of a main engine of the carrier sp The radius R of the rocket body of the carrier, the distance D between the servo actuator and the central line of the engine and the distance H between the plane formed by the mounting points on the servo actuator and the mass center of the carrier G Mounting point A of engine A and engine B G And B G Distance l from the plane G And calculates an actuator configuration matrix H as follows:
(3) Obtaining dynamic parameters of a servo system, wherein the dynamic parameters comprise a load mass matrix M and a cross section area A of a hydraulic cylinder p Load stiffness k, damping b, half volume V of hydraulic cylinder t Bulk modulus of elasticity beta of hydraulic oil e Leakage coefficient C of hydraulic cylinder tl Servo valve flow coefficient C d Servo valve area gradient w, hydraulic oil density ρ, hydraulic pump supply pressure p s Servo valve response time constant T sv Servo valve amplification gain k sv 。
(4) Acquiring the current time (t) j Moment, j=0, 1,2, …) carrier state parameters, including mass m (t j ) Moment of inertia J (t j ) Position vector r (t j ) Velocity vector v (t j ) Attitude angle vector θ (t j ) And an attitude angular velocity vector ω (t) j )。
(5) Judging whether the carrier meets the requirement of track-in or not according to the following method, namely
||r(t j )-r d ||≤e ss
If yes, the control process is ended; otherwise, go to step (6).
(6) Acquiring the current time (t) j Moment, j=0, 1,2, …) servo system state parameters, including the load moment vector F L (t j ) Displacement variation Δl (t) j ) Rate of changeHydraulic cylinder load pressure vector p (t) j ) Valve core displacement vector x of servo valve v (t j )。
(7) The current time (t) is calculated according to the following formula j Moment, j=0, 1,2, …) engine control thrust, command attitude angle:
wherein,
in xi r Is [0,1]The real number in the interval is 0.5, positive real number k r And k v The position control gain and the speed control gain, respectively, r (t j ) Representing the magnitude of the carrier position.
(8) According to the engine instruction attitude angle theta at the current moment obtained in the step (7) c (t j ) The current time (t) is calculated as follows j Moment, j=0, 1,2, …) the vehicle's filter command attitude angle and angular velocity:
wherein τ ψ Andrespectively yaw filter time constant and pitch filter time constant, and ψ c (t 0 )＝ψ f (t 0 ),/>t 0 Is the initial time.
(9) Judging the current time (t) j At the moment, j=0, 1,2, …) whether the gesture motion coupling matrix G (θ) is singular, that is, whether the rank (G (θ)) of the matrix is less than 3, if less than 3, the matrix is singular, and the process proceeds to step (10), otherwise, the process proceeds to step (11).
(10) And (4) correcting the gesture motion coupling matrix according to the following mode, and entering the step (11).
G(θ)＝G(θ)+εE 3
Where ε is a small positive real number, preferably 0.001, E 3 Is a third-order identity matrix.
(11) According to the engine control thrust force F (t) obtained in the step (7) j ) The current time (t) is calculated as follows j Moment, j=0, 1,2, …) of the servo actuator command displacement:
wherein the method comprises the steps of
In xi θ Is [0,1]The real number in the interval is 0.5, positive real number k θ And k ω The position control gain and the speed control gain,the derivative matrix being the inverse of the matrix G (θ).
(12) According to the current time (t j Moment j=0, 1,2, …) of the servo actuator command displacement Δl c (t j ) The current time (t) is calculated as follows j Moment, j=0, 1,2, …) of the servo actuator:
wherein τ l Is a servo filtering time constant, and Deltal f (t 0 )＝0,t 0 Is the initial time.
(13) Based on the filter command displacement and velocity of the servo actuator at the current time (time, j=0, 1,2, …), the current time (t j Moment, j=0, 1,2, …) of the servo actuator hydraulic cylinder command load pressure p c (t j )：
Wherein A is p Is the cross-sectional area of the hydraulic cylinder, K L And K M A servo displacement gain matrix and a servo displacement rate gain matrix respectively, which satisfy
Wherein, the values of the parameters satisfy the following relations:
j＝1,2,i＝A,B
(14) According to the current time (t j Moment, j=0, 1,2, …) of the servo actuator hydraulic cylinder command load pressure p c (t j ) The current time (t) is calculated as follows j Moment, j=0, 1,2, …) of the servo actuator hydraulic cylinder, the filter command load pressure and the rate of change:
wherein τ p Filter time constant for hydraulic cylinder, and
p f (t 0 )＝p(t 0 ),
(15) According to the current time (t j Moment, j=0, 1,2, …) of the servo actuator hydraulic cylinder f (t j ) Rate of changeThe current time (t) is calculated as follows j Moment j=0, 1,2, …) of the servo actuator command spool displacement x vc (t j )：/>
Wherein k is p Gain is controlled for cylinder pressure.
(16) According to the current time (t j Moment j=0, 1,2, …) of the servo actuator command spool displacement x vc (t j ) The current time (t) is calculated as follows j Moment, j=0, 1,2, …) of the servo actuator spool filter command displacement and rate of change:
wherein τ xv Filter time constant for valve core, and x vf (t 0 )＝x v (t 0 ),x v (t 0 ) The displacement vector of the valve core of the servo actuator is the initial time, t 0 Is the initial time.
(17) According to the filter instruction displacement x of the valve core of the servo actuator vf (t j ) Rate of change of displacementThe following calculation is performedAt the present time (t j At the moment j=0, 1,2, …), the servo actuator inputs a control voltage vector u (t) j )：
Wherein k is u Gain is controlled for spool movement.
(18) Output the current time (t) j Control voltage vector u (t) at time j=0, 1,2, … j ) And thrust vector F (t) j ) Control is exercised and step (4) is returned.
The invention has been described in detail in connection with the specific embodiments and exemplary examples thereof, but such description is not to be construed as limiting the invention. It will be understood by those skilled in the art that various equivalent substitutions, modifications or improvements may be made to the technical solution of the present invention and its embodiments without departing from the spirit and scope of the present invention, and these fall within the scope of the present invention. The scope of the invention is defined by the appended claims.
What is not described in detail in the present specification is a well known technology to those skilled in the art.
Claims (14)
1. The integrated control method for the pose of the carrier taking the dynamic characteristics of the servo mechanism into consideration is characterized by comprising the following steps of:
(1) Acquiring a carrier in-orbit task parameter;
(2) Acquiring overall parameters of the carrier, and calculating an execution mechanism configuration matrix according to the overall parameters of the carrier;
(3) Acquiring dynamic parameters of a servo system;
(4) Acquiring the current time t j J=0, 1,2, …;
(5) According to the current time t obtained in the step (4) j Judging whether the carrier meets the track-in requirement or not according to the carrier state parameters and the carrier track-in task parameters obtained in the step (1), and ending the control process if the carrier meets the track-in requirement; otherwise, entering step (6);
(6) Acquiring the current time t j Servo system state parameters of (a);
(7) According to the carrier in-orbit task parameters obtained in the step (1) and the current time t obtained in the step (4) j The current time t is calculated by the carrier state parameter of (2) j Controlling thrust and command attitude angles of an engine;
(8) According to the current time t obtained in the step (7) j The instruction attitude angle of the engine and the carrier in-orbit task parameters obtained in the step (1) calculate the current time t j A filter command attitude angle and a filter command angular velocity of the vehicle;
(9) Judging the current time t j Whether the attitude motion coupling matrix G (theta) of the carrier is singular or not, if so, entering the step (10), otherwise, entering the step (11);
(10) Correcting the attitude motion coupling matrix G (theta), and entering a step (11);
(11) According to the current time t obtained in the step (7) j Controlling thrust of engine and current time t obtained in step (8) j A filtering command attitude angle and a filtering command angular speed of the carrier, an attitude motion coupling matrix G (theta) obtained in the step (9) or the step (10), a mechanism configuration matrix obtained in the step (2), and a current time t obtained in the step (4) j The current time t is calculated by the carrier state parameter of (2) j Is displaced by the servo actuator command;
(12) According to the current time t obtained in the step (11) j The servo actuator command displacement of the step (6) and the current time t obtained in the step (6) j The servo system state parameter of the (1) and the carrier track-in task parameter obtained in the step (1) are calculated to obtain the current time t j A filtering instruction displacement and a filtering instruction speed of the servo actuator;
(13) According to the current time t obtained in the step (12) j Filtering instruction displacement and filtering instruction displacement speed of servo actuator and current time t obtained in step (6) j The state parameters of the servo system and the dynamic parameters of the servo system obtained in the step (3) are calculated to obtain the current time t j A hydraulic cylinder of the servo actuator commands load pressure;
(14) According to the current time t obtained in the step (13) j Servo actuator liquidThe cylinder instructs the load pressure and the current time t obtained in the step (6) j The servo system state parameter of the (1) and the carrier track-in task parameter obtained in the step (1) are calculated to obtain the current time t j The method comprises the steps of filtering command load pressure and filtering command load pressure change rate of a hydraulic cylinder of a servo actuator;
(15) According to the current time t obtained in the step (14) j Filtering command load pressure of a hydraulic cylinder of a servo actuator, filtering command load pressure change rate, and current time t obtained in step (6) j Calculating the current time t by the servo system state parameter and the servo system dynamics parameter obtained in the step (3) j The servo actuator instructs the valve core to displace;
(16) According to the current time t obtained in the step (15) j The servo actuator instructs the valve core to displace, and the current time t obtained in the step (6) is obtained j The servo system state parameter of the (1) and the carrier track-in task parameter obtained in the step (1) are calculated to obtain the current time t j Filtering instruction displacement and displacement change rate of a valve core of a servo actuator;
(17) According to the current time t obtained in step (16) j Filtering instruction displacement and change rate of valve core of servo actuator and current time t obtained in step (6) j The state parameters of the servo system and the dynamic parameters of the servo system obtained in the step (3) are calculated to obtain the current time t j Inputting a control voltage vector by the servo actuator;
(18) According to the current time t obtained in step (17) j Inputting a control voltage vector to the servo actuator and obtaining the current time t in the step (7) j The engine control thrust controls the carrier, and returns to step (4).
2. The integrated control method for the attitude of a vehicle taking into account the dynamic characteristics of a servo mechanism according to claim 1, wherein,
in the step (1), the carrier on-track task parameter includes a desired position r of the on-track task carrier d Desired speed v d Desired track-in accuracy e ss And a control period T;
in the step (2), the overall parameters of the carrier include the carrierMain engine specific impulse I sp The radius R of the rocket body of the carrier, the distance D between the servo actuator and the central line of the engine and the distance H between the plane formed by the mounting points on the servo actuator and the mass center of the carrier G Mounting point A of engine A and engine B G And B G Distance l from installation point on servo actuator to form plane G ；
In the step (3), the dynamic parameters of the servo system comprise a load mass matrix M and a cross section area A of the hydraulic cylinder p Load stiffness k, damping b, half volume V of hydraulic cylinder t Bulk modulus of elasticity beta of hydraulic oil e Leakage coefficient C of hydraulic cylinder tl Servo valve flow coefficient C d Servo valve area gradient w, hydraulic oil density ρ, hydraulic pump supply pressure p s Servo valve response time constant T sv And servo valve amplification gain k sv ；
In the step (4), the current time t j The carrier status parameter of (c) comprises a carrier mass m (t j ) Moment of inertia of the vehicle J (t j ) Vehicle position vector r (t j ) Vehicle velocity vector v (t j ) Vehicle attitude angle vector θ (t j ) And a carrier attitude angular velocity vector ω (t) j )；
In the step (6), the current time t j The servo system state parameters of (1) include a load moment vector F L (t j ) Displacement variation Δl (t) j ) Displacement change rate of servo actuatorHydraulic cylinder load pressure vector p (t) j ) And servo valve spool displacement vector x v (t j )。
3. The method for integrated control of the vehicle pose taking into account the dynamic characteristics of the servo mechanism according to claim 2, wherein in the step (2), the calculation formula of the actuator configuration matrix H is:
4. the method for integrally controlling the pose of the carrier taking the dynamic characteristics of the servo mechanism into consideration as claimed in claim 2, wherein in the step (5), the method for judging whether the carrier meets the requirement of the track-in is as follows:
if meeting r (t) j )-r d ||≤e ss The carrier meets the in-orbit requirement, otherwise, the carrier does not meet the in-orbit requirement.
5. The method for integrated control of the attitude of a vehicle in consideration of dynamic characteristics of a servo mechanism according to claim 2, wherein in said step (7), the current time t j Engine control thrust force F (t) j ) And the attitude angle θ of the vehicle command c (t j ) The calculation formula is as follows:
wherein,
in xi r Is [0,1]Real number, positive real number k in interval r And k v The position control gain and the speed control gain, respectively.
6. The method for integrated control of vehicle pose taking into account dynamic characteristics of servo mechanism according to claim 5, wherein in said step (8), the current time t j Filtering instruction attitude angle theta of carrier f (t j ) Filtered commanded angular velocityThe calculation formula of (2) is as follows:
wherein τ ψ Andrespectively yaw filter time constant and pitch filter time constant, and psi c (t 0 )＝ψ f (t 0 ),t 0 Is the initial time.
7. The method for integrated control of vehicle pose taking into account dynamic characteristics of servo mechanism according to claim 1 or 2, wherein in said step (9), the current time t is determined j The method for judging whether the attitude motion coupling matrix G (theta) of the carrier is singular or not is that the current time t is judged j Whether the rank (G (theta)) of the attitude motion coupling matrix G (theta) of the carrier is smaller than 3, if smaller than 3, G (theta) is singular, and if not smaller than 3, G (theta) is not singular;
in the step (10), the formula for correcting the gesture motion coupling matrix G (θ) is as follows:
G(θ)＝G(θ)+εE 3 wherein ε is a correction coefficient, ε is 0.001 or less, E 3 Is a third-order identity matrix.
8. The method for integrated control of vehicle pose taking into account dynamic characteristics of servo mechanism according to claim 6, wherein in said step (11), the current time t j Is a servo actuator command displacement Deltal c (t j ) The calculation formula of (2) is as follows:
wherein the method comprises the steps of
Wherein H is an actuator configuration matrix,configuring generalized inverse matrix, ζ of matrix H for actuator θ Is [0,1]Real number, positive real number k in interval θ And k ω An attitude control gain and an angular velocity control gain, respectively, < >>The derivative matrix is the inverse of the gesture motion coupling matrix G (θ).
9. The method for integrated control of the attitude of a vehicle in consideration of the dynamic characteristics of a servo mechanism according to claim 2 or 8, wherein in said step (12), the current time t j Filter command displacement al of servo actuator f (t j ) Filtering command speedThe calculation formula of (2) is as follows:
wherein Deltal c (t j ) For the current time t j Is displaced by the servo actuator command τ l Is a servo filtering time constant, and Deltal f (t 0 )＝0,t 0 Is the initial time.
10. The method for integrated control of vehicle pose taking into account dynamic characteristics of servo mechanism according to claim 9, wherein in said step (13), the current time t j Servo actuator hydraulic cylinder command load pressure p c (t j ) The calculation formula of (2) is as follows:
wherein A is p Is the cross-sectional area of the hydraulic cylinder, K L And K M A servo displacement gain matrix and a servo displacement rate gain matrix respectively, which satisfy
Wherein, the values of the parameters satisfy the following relations:
for the elements in the load mass matrix M +.>
11. The method for integrated control of vehicle pose taking into account dynamic characteristics of servo mechanism according to claim 10, wherein in said step (14), the current time t j Filtering instruction load pressure p of hydraulic cylinder of servo actuator f (t j ) Load pressure rate of change of filter commandThe calculation formula of (2) is as follows:
wherein τ p Filter time constant for hydraulic cylinder, and
p f (t 0 )＝p(t 0 ),t 0 for the initial time period of time, the time period,
wherein p (t) 0 ) The hydraulic cylinder load pressure vector is the initial time.
12. The method for integrated control of vehicle pose taking into account dynamic characteristics of servo mechanism according to claim 11, wherein in said step (15), the current time t j Is used for commanding the valve core displacement x of the servo actuator vc (t j ) The calculation formula of (2) is as follows:
wherein k is p Gain is controlled for the hydraulic cylinder pressure;
where, for the servo actuators j, i=a, B, j=1, 2,for spool displacement of servo valve, +.>Is the hydraulic cylinder load pressure.
13. The method for integrated control of vehicle pose taking into account dynamic characteristics of servo mechanism according to claim 12, wherein in said step (16), the current time t j Valve core filtering instruction displacement x of servo actuator vf (t j ) Rate of change of displacementThe calculation formula of (2) is as follows:
wherein τ xv Filter time constant for valve core, and x vf (t 0 )＝x v (t 0 ),x v (t 0 ) The displacement vector of the valve core of the servo actuator is the initial time, t 0 Is the initial time.
14. The method for integrated control of vehicle pose taking into account dynamic characteristics of servo mechanism according to claim 13, wherein in said step (17), the current time t j The servo actuator inputs a control voltage vector u (t j ) The calculation formula of (2) is as follows:
wherein k is u Gain is controlled for spool movement.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111530481.3A CN114280932B (en) | 2021-12-14 | 2021-12-14 | Carrier pose integrated control method considering dynamic characteristics of servo mechanism |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111530481.3A CN114280932B (en) | 2021-12-14 | 2021-12-14 | Carrier pose integrated control method considering dynamic characteristics of servo mechanism |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114280932A CN114280932A (en) | 2022-04-05 |
| CN114280932B true CN114280932B (en) | 2023-11-10 |
Family
ID=80872170
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111530481.3A Active CN114280932B (en) | 2021-12-14 | 2021-12-14 | Carrier pose integrated control method considering dynamic characteristics of servo mechanism |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114280932B (en) |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105538310A (en) * | 2016-01-11 | 2016-05-04 | 电子科技大学 | Electro-hydraulic servo control method based on fading memory filtering and 2-DOF mechanical arm |
| CN109358507A (en) * | 2018-10-29 | 2019-02-19 | 东北大学 | A kind of visual servo adaptive tracking control method of time-varying performance boundary constraint |
| CN112148026A (en) * | 2020-08-17 | 2020-12-29 | 海洋石油工程股份有限公司 | Thrust distribution method of underwater robot dynamic positioning system |
| CN112405528A (en) * | 2020-10-29 | 2021-02-26 | 中国运载火箭技术研究院 | Space manipulator system explicit dynamics control method considering gravitational influence |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| FR2910417B1 (en) * | 2006-12-21 | 2012-07-06 | Messier Bugatti | ADAPTIVE BRAKING CONTROL METHOD FOR VEHICLE. |
| US10759519B2 (en) * | 2017-10-31 | 2020-09-01 | The Boeing Company | Adaptive feedback control of force fighting in hybrid actuation systems |
-
2021
- 2021-12-14 CN CN202111530481.3A patent/CN114280932B/en active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105538310A (en) * | 2016-01-11 | 2016-05-04 | 电子科技大学 | Electro-hydraulic servo control method based on fading memory filtering and 2-DOF mechanical arm |
| CN109358507A (en) * | 2018-10-29 | 2019-02-19 | 东北大学 | A kind of visual servo adaptive tracking control method of time-varying performance boundary constraint |
| CN112148026A (en) * | 2020-08-17 | 2020-12-29 | 海洋石油工程股份有限公司 | Thrust distribution method of underwater robot dynamic positioning system |
| CN112405528A (en) * | 2020-10-29 | 2021-02-26 | 中国运载火箭技术研究院 | Space manipulator system explicit dynamics control method considering gravitational influence |
Non-Patent Citations (1)
| Title |
|---|
| 先进上面级变轨段姿态解耦控制研究;崔乃刚;张利宾;浦甲伦;;宇航学报(第09期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114280932A (en) | 2022-04-05 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109062237B (en) | Active-disturbance-rejection attitude control method for unmanned tilt-rotor aircraft | |
| Monahemi et al. | Control of wing rock motion using adaptive feedback linearization | |
| Van Nieuwstadt et al. | Real‐time trajectory generation for differentially flat systems | |
| CN102343985B (en) | Satellite time optimal posture maneuvering method with reaction flywheel | |
| Liu et al. | Prescribed performance tracking control of a free-flying flexible-joint space robot with disturbances under input saturation | |
| CN110908278A (en) | Dynamics modeling and stability control method of folding wing aircraft | |
| Seigler et al. | Analysis of transition stability for morphing aircraft | |
| CN111045440B (en) | Hypersonic aircraft nose-down section rapid rolling control method | |
| Cai et al. | Quadrotor waypoint-tracking control under exogenous disturbances based on equivalent-input-disturbance approach | |
| Huang et al. | Prescribed performance fuzzy back-stepping control of a flexible air-breathing hypersonic vehicle subject to input constraints | |
| Ferrier et al. | Active gust load alleviation of high-aspect ratio flexible wing aircraft | |
| Jia et al. | Vibration control of gyroelastic spacecraft using input shaping and angular momentum devices | |
| Jang et al. | Design of launch vehicle flight control systems using ascent vehicle stability analysis tool | |
| CN111142550B (en) | Civil aircraft aided driving control method and system and flight quality evaluation method | |
| Hashemi et al. | Performance optimizing gust load alleviation control of flexible wing aircraft | |
| Prach et al. | Development of a state dependent riccati equation based tracking flight controller for an unmanned aircraft | |
| CN108255193A (en) | A kind of vertical/short field aircraft flight control method | |
| CN113485396B (en) | Spacecraft intersection butt joint final approximation segment relative orbit and attitude tracking control method | |
| CN114637203A (en) | Flight control system for medium-high speed and large-sized maneuvering unmanned aerial vehicle | |
| CN114721266A (en) | Self-adaptive reconstruction control method under structural missing fault condition of airplane control surface | |
| CN113110527B (en) | Cascading control method for finite time path tracking of autonomous underwater vehicle | |
| CN114280932B (en) | Carrier pose integrated control method considering dynamic characteristics of servo mechanism | |
| CN116795126A (en) | Input saturation and output limited deformed aircraft control method | |
| CN116360258A (en) | Hypersonic deformed aircraft anti-interference control method based on fixed time convergence | |
| Xi et al. | L 1 adaptive control of the flying wing UAV with unknown time-varying disturbances |
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 |