CN111914411A - Full-attitude four-axis turntable frame angle instruction resolving method - Google Patents

Full-attitude four-axis turntable frame angle instruction resolving method Download PDF

Info

Publication number
CN111914411A
CN111914411A CN202010697584.8A CN202010697584A CN111914411A CN 111914411 A CN111914411 A CN 111914411A CN 202010697584 A CN202010697584 A CN 202010697584A CN 111914411 A CN111914411 A CN 111914411A
Authority
CN
China
Prior art keywords
attitude
frame
angle
aircraft
axis
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.)
Granted
Application number
CN202010697584.8A
Other languages
Chinese (zh)
Other versions
CN111914411B (en
Inventor
陈松林
李宏禄
邢宝祥
杨宝庆
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Hit Hanbo Technology Co ltd
Original Assignee
Hit Hanbo Technology Co ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Hit Hanbo Technology Co ltd filed Critical Hit Hanbo Technology Co ltd
Priority to CN202010697584.8A priority Critical patent/CN111914411B/en
Publication of CN111914411A publication Critical patent/CN111914411A/en
Application granted granted Critical
Publication of CN111914411B publication Critical patent/CN111914411B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F30/00—Computer-aided design [CAD]
    • G06F30/20—Design optimisation, verification or simulation
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F30/00—Computer-aided design [CAD]
    • G06F30/10—Geometric CAD
    • G06F30/15—Vehicle, aircraft or watercraft design
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F2111/00—Details relating to CAD techniques
    • G06F2111/04—Constraint-based CAD
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Abstract

The invention provides a method for calculating frame angle instructions of a full-attitude four-axis turntable, which comprises the steps of establishing a kinematics model of a frame angle of the four-axis turntable and a full-space aircraft attitude angle based on a double Euler method, initializing each frame angular position at a simulation initial moment, designing a turntable kinematics inverse solution algorithm based on a constraint optimization theory, calculating to obtain each frame angular position instruction of the four-axis turntable at the current moment according to the kinematics inverse solution algorithm, and designing a kinematics forward solution algorithm of the four-axis turntable to finally obtain all instructions of the frame angular positions. The method can ensure that the instruction resolving of the four-axis turntable does not generate large-amplitude resolving deviation jump in the full attitude on the premise of meeting the real-time performance, effectively avoiding the singular position of kinematics and the like, and eliminates the phenomenon of large-amplitude error caused by positive solution multi-resolution.

Description

Full-attitude four-axis turntable frame angle instruction resolving method
Technical Field
The invention belongs to the technical field of semi-physical simulation of aircrafts, and particularly relates to a full-attitude four-axis turntable frame angle instruction resolving method.
Background
In aircraft semi-physical simulation and testing, a flight simulation turntable is one of the commonly used hardware devices. Among various simulation turntables, a three-axis simulation turntable is most commonly used, and can accurately simulate three attitude changes of a pitching attitude, a rolling attitude and a yawing attitude of an aircraft in space. However, due to the structural characteristics of the three-axis simulation turntable, the three-axis simulation turntable has a kinematic singular position and cannot adapt to the new requirement of full-attitude flight simulation. In order to avoid singular positions on the premise of ensuring the attitude of an aircraft in a simulation space, a four-axis turntable scheme is provided in engineering, and an additional secondary task is realized by increasing the redundant degree of freedom. However, unlike the three-axis turret, each frame angle corresponds to an attitude angle of the simulated aircraft one by one, and there is a coupling relationship between four frame angles and three attitude angles of the four-axis turret, that is, there may be countless sets of frame angles corresponding to a given set of attitude angles. For the calculation of each frame angle instruction of the four-axis turntable, two types of solutions mainly exist at present, namely a solution based on an adaptive adjustment algorithm and a solution based on an optimization problem containing constraint. In the first category, the solving scheme based on the adaptive adjustment algorithm solves the angular position instructions of the other three frames by actively designing the motion law of the base frame, so as to realize the simulation of the attitude of the aircraft. The scheme can effectively avoid the kinematic singular position of the four-axis turntable, but an ideal base frame motion law is difficult to design. In addition, when the full-attitude simulation of the aircraft is realized by the scheme, the situation that the angle position commands of each frame obtained by solving are discontinuous and the solving result has large-amplitude error jump can occur. The second category of solutions treats the instruction solving problem as a multi-solvability problem and converts it into an optimization problem to solve. According to the scheme, optimization variables, constraint conditions, objective functions, optimization algorithms and the like of the optimization problem are designed according to expected performance indexes of users, but all performance indexes such as resolving instantaneity, accuracy and avoidance of singular positions are difficult to consider simultaneously. In addition, when the scheme is adopted to realize the simulation of the full attitude of the aircraft, the phenomena of instruction discontinuity and large-amplitude error jump can also occur.
Disclosure of Invention
The invention aims to overcome the limitation that the existing scheme cannot simulate in a full-attitude range, and provides a full-attitude four-axis turntable frame angle instruction resolving method.
The invention is realized by the following technical scheme, and provides a full-attitude four-axis turntable frame angle instruction resolving method which specifically comprises the following steps:
the method comprises the following steps: according to the structure of a four-axis turntable and the principle of simulating the attitude change of an aircraft, establishing a kinematics model of a frame angle of the four-axis turntable and an attitude angle of a full-space aircraft based on a double Euler method, wherein the kinematics model comprises an angular position kinematics model and an angular velocity kinematics model;
step two: inputting aircraft attitude at simulation initial moment
Figure BDA0002591872080000021
Gamma (0) is the roll angle at the simulated initial moment,
Figure BDA0002591872080000022
measuring initial position phi of each frame angle of the four-axis turntable to simulate yaw angle at initial moment, and theta (0) is pitch angle at the simulated initial moment0＝[φ10,φ20,φ30,φ40]TInitializing each frame angular position at the simulation initial moment according to the established angular position kinematic relationship between the four-axis turntable frame angle and the aircraft attitude angle to obtain phi (0) [ [ phi ] ]1(0),φ2(0),φ3(0),φ4(0)]T；
Step three: designing a turntable kinematics inverse solution algorithm based on a constraint optimization theory, and selecting an optimization variable in an optimization problem, namely each frame angular position phi [ [ phi ]1,φ2,φ3,φ4]TConstraint conditions
Figure BDA0002591872080000023
m represents the number of constraint conditions, an objective function f (phi) and an optimization algorithm;
step four: inputting the attitude of the aircraft to be simulated at the current moment, namely the nth moment
Figure BDA0002591872080000024
Aircraft attitude simulation solution at the previous moment, namely the (n-1) th moment
Figure BDA0002591872080000025
And each frame angular position phi (n-1) of the four-axis turntable at the previous moment is equal to [ phi ]1(n-1),φ2(n-1),φ3(n-1),φ4(n-1)]TCalculating to obtain an angular position command phi (n) of each frame of the four-axis turntable at the current moment as [ phi (n) ]according to a kinematics inverse solution algorithm1(n),φ2(n),φ3(n),φ4(n)]T；
Step five: designing a kinematics positive solution algorithm of the four-axis turntable, and changing the angle position command phi (n) of each frame into [ phi ] according to the current moment1(n),φ2(n),φ3(n),φ4(n)]TAnd resolving to obtain the attitude simulation solution of the aircraft at the current moment
Figure BDA0002591872080000026
And will be
Figure BDA0002591872080000027
Inputting the motion data into inverse kinematics solution calculation at the next moment;
step six: and repeating the third step to the fifth step in an iterative manner, and resolving all the instructions of the angular position of the frame.
Further, the four-axis turntable is a vertical four-axis turntable.
Further, the first step specifically comprises:
a positive Euler method representation angular position kinematic model for describing the attitude of the aircraft by using 2-3-1 rotation sequence attitude angles is as follows:
RG b＝RG l (1)
an inverse Euler method representation angular position kinematic model for describing the attitude of the aircraft by using 2-1-3 rotation order attitude angles is as follows:
RrG b＝RG l (2)
Figure BDA0002591872080000031
Figure BDA0002591872080000032
Figure BDA0002591872080000033
wherein R isG bRepresenting an attitude matrix, R, describing the attitude change of the aircraft in order of 2-3-1 attitude anglesrG bRepresenting an attitude matrix, R, describing the attitude changes of the aircraft in attitude angles of 2-1-3 rotation orderG lRepresenting an attitude matrix for describing the attitude change of the vertical four-axis turntable load; gamma, the concentration of the gamma-rays,
Figure BDA0002591872080000034
theta respectively represents a rolling angle, a yaw angle and a pitch angle in the normal Eulerian method; gamma rayr，
Figure BDA0002591872080000035
θrRespectively representing a rolling angle, a yaw angle and a pitch angle in the anti-Eulerian method; phi is a1，φ2，φ3，φ4Respectively showing the frame angles of four frames of the vertical four-axis turntable from outside to inside; g represents a ground coordinate system, b represents an aircraft coordinate system, and l represents a load coordinate system;
a positive Euler method representation angular velocity kinematic model for describing the attitude of the aircraft by using a 2-3-1 rotation sequence attitude angle is as follows:
Figure BDA0002591872080000036
an inverse Euler method representation angular velocity kinematics model for describing the attitude of the aircraft by using 2-1-3 rotation sequence attitude angles is as follows:
Figure BDA0002591872080000037
Figure BDA0002591872080000038
Figure BDA0002591872080000039
Figure BDA0002591872080000041
wherein, JG bThe jacobian matrix is shown describing the aircraft as moving in 2-3-1 rotation order attitude angles,
Figure BDA0002591872080000042
respectively representing the rolling angular velocity, the yaw angular velocity and the pitch angular velocity in the positive Euler method; j. the design is a squarerG bThe jacobian matrix is shown describing the aircraft as moving in 2-1-3 rotation order attitude angles,
Figure BDA0002591872080000043
respectively representing the rolling angular velocity, the yaw angular velocity and the pitch angular velocity in the anti-Eulerian method; j. the design is a squareG lA jacobian matrix is shown describing the vertical four-axis turntable load motion,
Figure BDA0002591872080000044
Figure BDA0002591872080000045
respectively representing the frame angular velocities of the four frames of the vertical four-axis turntable from outside to inside; the vector of rotation of the aircraft is denoted ωb＝[ωbx,ωby,ωbz]TWherein ω isbx、ωby、ωbzRespectively projecting the rotation vector in X-axis, Y-axis and Z-axis directions of an aircraft coordinate system; the vector of rotation of the load is ωl＝[ωlx,ωly,ωlz]TWherein ω islx、ωly、ωlzThe projections of the rotation vector in the X-axis direction, the Y-axis direction and the Z-axis direction of the load coordinate system are respectively;
for each angular velocity in the formula (6) and the formula (7), angular position difference is adopted to approximate in engineering, the sampling time interval is set to be delta t, and the attitude angle of the aircraft at the current moment is respectively gamman，
Figure BDA0002591872080000046
θn，γr n，
Figure BDA0002591872080000047
θr nThe attitude angles at the previous time are gamman-1，
Figure BDA0002591872080000048
θn-1，γr n-1，
Figure BDA0002591872080000049
θr n-1Then, there are:
Figure BDA00025918720800000410
let the attitude matrix R in the formulas (1) and (2)G b＝RrG bThe (i, j) th entry in the matrix is denoted as RijThen, the conversion formula for calculating the inverse euler angle from the positive euler angle when the same attitude changes is described as follows:
Figure BDA00025918720800000411
when the positive Euler angle is solved by the inverse Euler angle, the conversion formula of the positive Euler angle is as follows:
Figure BDA0002591872080000051
in formulas (12) and (13), k is 0 or 1;
defining a rounding function Δ (x)1,x2) As shown in equation (14):
Δ(x1,x2)＝min(|x1-x2|,2π-|x1-x2|) (14)
when switching between positive and negative Euler angles in practical application, each Euler angle at the current moment is set as gamman，
Figure BDA0002591872080000052
θn，γr n，
Figure BDA0002591872080000053
θr nThe Euler angles at the previous time are gamman-1，
Figure BDA0002591872080000054
θn-1，γr n-1，
Figure BDA0002591872080000055
θr n-1Introducing a discriminant function dk
Figure BDA0002591872080000056
Respectively calculating d according to the values of k0And d1Get an order dkAnd a group of attitude angles with small values are used as conversion results of positive and negative Euler angles at the current moment.
Further, in the second step, the first step,
aircraft attitude at initial time based on input simulation
Figure BDA0002591872080000057
And measuring the initial position phi of each frame angle of the four-axis rotary table0＝[φ10,φ20,φ30,φ40]TKeeping the angular position phi of the base frame of the four-axis turntable at the initial simulation time1(0)＝φ10At the same time, the other three frame angular positions phi2(0)，φ3(0)，φ4(0) Satisfy the requirement of
Figure BDA00025918720800000511
Wherein R isX(γ (0)) represents a rotation matrix rotating about the X-axis by an angle γ (0), RZ(theta (0)) represents a rotation matrix rotated by an angle theta (0) around the Z axis,
Figure BDA0002591872080000058
indicating rotation about the Y axis
Figure BDA0002591872080000059
A rotation matrix of angles; rY(φ1(0))，RX(φ4(0))，RY(φ3(0))，RZ(φ2(0) The same applies;
let RX(φ4(0))RY(φ3(0))RZ(φ2(0) T) then there are
Figure BDA00025918720800000510
Figure BDA0002591872080000061
In the formula (17), S represents a sine function sin (·), and C represents a cosine function cos (·);
the vertical four-axis turntable is internally provided with a pitching outer frame, a yawing middle frame and a rolling inner frameInitial angular position
Figure BDA0002591872080000062
Figure BDA0002591872080000063
The solving formula is as follows:
Figure BDA0002591872080000064
wherein the content of the first and second substances,
Figure BDA0002591872080000065
k is 1 or-1, two groups of actual attitude angles can be obtained through kinematics positive solution according to two groups of frame angular position instructions obtained through solution, and are respectively recorded as
Figure BDA0002591872080000066
And
Figure BDA0002591872080000067
the actual attitude angles of the aircraft at the current moment are gamma, phi and theta respectively; introducing a discriminant function dk
Figure BDA0002591872080000068
Respectively calculating d according to the values of k1，d-1Get an order dkGroup of small values
Figure BDA0002591872080000069
As the initial angular positions of the three frames inside the four-axis turntable at the current moment.
Further, in the third step,
the optimization variable of the optimization problem is selected as each frame angular position, and when a certain moment t is set, each frame angular position of the four-axis rotary table is phi1(t)，φ2(t)，φ3(t)，φ4(t); for the differential form of the angular position of the frame involved in the optimization scheme, the toolIn the process, the difference mode is adopted for replacement, and the angular positions of all frames of the four-axis rotary table at the previous moment are respectively set as phi1(t-1)，φ2(t-1)，φ3(t-1)，φ4(t-1) the time interval between two moments is Δ t, the differential form of the angular position of the frames at t, i.e. the angular velocity of the frames
Figure BDA00025918720800000610
Can be finished to obtain:
Figure BDA00025918720800000611
the optimization problem objective function is designed as:
Figure BDA0002591872080000071
wherein, w1，w2，w3，w4The weighting coefficients are respectively the angular velocity of each frame and represent the motion capability and the dynamic performance of each frame, and the four weighting coefficients meet w when being taken as values1＞w2＞w3＞w4＞0；w1，w2，w4The value of (a) is designed and set according to the dynamic response difference of the corresponding frame angle, and for the aircrafts with postures which are different and change within a large range, w1＝w2＝w4＝Δt2(ii) a Weighting coefficient w of middle frame3The angular positions of the outer frame and the middle frame are jointly influenced, and the influences of the angular positions of the outer frame and the middle frame on the weighting coefficient of the middle frame are mutually independent; therefore, in the engineering, in order to avoid the singular position of the kinematics of the four-axis turntable, the weighting coefficient w is used3Expressed as a two-dimensional normal distribution probability density function:
Figure BDA0002591872080000072
wherein x and y are respectively mod (| φ)2|,2π)，mod(|φ3L, 2 pi) representing the angular positions of the outer frame and the middle frame of the four-axis turntable; the parameter K describes the height of the probability density function, the parameter D describes the weighting coefficient of the intermediate frame, mu, away from the singular position1And mu2Describing the kinematic singular positions of the four-axis turntable, namely the corresponding angular positions of the outer frame and the middle frame; sigma1And sigma2Describing the change rate and the change amplitude of the frame weighting coefficient in the position close to the singular position;
the constraint conditions are equality constraint, and the equality constraint in engineering is acted by the full-attitude aircraft angular velocity kinematics equation based on the double Euler method, so that
Figure BDA0002591872080000073
Figure BDA0002591872080000074
Figure BDA0002591872080000075
Then, the equality constraint is:
Figure BDA0002591872080000081
wherein the content of the first and second substances,
Figure BDA0002591872080000082
Figure BDA0002591872080000083
Figure BDA0002591872080000084
the optimization algorithm selects a Lagrange multiplier method, and Z represents an integer set.
Further, in step five, the designing of the kinematics forward solution algorithm of the four-axis turntable specifically includes:
according to four frame angular positions of the four-axis rotary table, the terminal load of the rotary table, namely three attitude angles of the equivalent simulated aircraft, satisfy the equation:
Figure BDA0002591872080000085
the solving formula of the load attitude angle is as follows:
Figure BDA0002591872080000086
in formula (29)
Figure BDA0002591872080000087
k takes 1 or-1, corresponding to two groups of results;
defining a rounding function Δ (x)1,x2)＝min(|x1-x2|,2π-|x1-x2| in the kinematics positive solution process, each attitude angle of the load at a certain moment is set as gamma respectivelyn、
Figure BDA0002591872080000091
θnThen, each attitude angle at the previous time is γn-1、
Figure BDA0002591872080000092
θn-1Introducing a discriminant function dk
Figure BDA0002591872080000093
Wherein k is 1 or-1; respectively calculating d according to the values of k1，d-1Get an order dkA small set of attitude angles as the kinematic positive of the current timeAnd (5) solving the result.
The invention has the beneficial effects that:
1. the angular velocity kinematic relationship based on the double-Euler method established by the invention provides a theoretical basis of constraint conditions for the optimization problem converted by the kinematic inverse solution in the step three, and successfully establishes the angular velocity kinematic relationship between the attitude angle of the full-attitude aircraft and each frame angle of the four-axis turntable. The established kinematic relationship between the attitude angle of the full-attitude aircraft and the angular velocity of each frame of the rotary table can ensure that a mathematical model which is full in attitude and can overcome the singularity problem of the Euler equation can be established for any given attitude angle form (such as 2-3-1 rotation sequence) with fixed rotation sequence and four-axis rotary tables (such as vertical or horizontal four-axis rotary tables) with different forms. In engineering, angular velocity is subjected to angular position differentiation to approximate substitution, and the real-time performance of calculation is guaranteed while the full-attitude simulation of the aircraft is eliminated.
2. The initialization algorithm of each frame angular position of the rotary table provided by the method successfully solves the problem that each frame angular position of the four-axis rotary table corresponding to the initial moment of the attitude simulation of the aircraft is not zero. In the initialization process, the base frame is adjusted with a minimum amplitude in consideration of the worst dynamic performance of the base frame. In addition, the initialization algorithm expands the value range of the inverse trigonometric function into the full space, and by designing the accept-and-reject function, the multi-solution problem caused by the expanded value range is rejected, so that the initialized instruction resolving can not generate large-amplitude resolving deviation jump in the full posture.
3. The kinematics forward solution improved algorithm provided by the method successfully solves the problems that the calculation result is greatly different from the theoretical value and large-amplitude jump occurs possibly caused by the traditional kinematics forward solution algorithm when the full-attitude simulation of the aircraft is carried out. The improved algorithm expands the value range of the inverse trigonometric function into the full space, and rejects the multi-solution problem caused by the expansion of the value range by designing a rejection function, thereby eliminating the phenomenon of large-amplitude error caused by the forward multi-solution.
4. The invention designs a method for dynamically changing weighting coefficients of a centering frame in a kinematic inverse solution scheme, and simultaneously considers the angular positions of an outer frame and a middle frame in a singular position configuration of a four-axis turntable. When the outer frame and the middle frame are close to the singular position together, the weighting coefficient of the angular velocity of the middle frame is increased, the angular velocity of the middle frame is reduced along with the increase of the weighting coefficient, and the movement trend of the middle frame close to the singular position is restrained. When only one of the outer or middle frames approaches the singular position, the angular velocity of the middle frame does not change significantly. In engineering, the kinematic singular position of the four-axis turntable can be more simply and effectively avoided.
Drawings
FIG. 1 is a graph of a coordinate system transformation relationship based on 2-3-1 rotation Euler angles in the present invention;
FIG. 2 is a schematic structural diagram of a vertical four-axis turntable;
FIG. 3 is a schematic diagram of kinematic singular positions caused by structural limitations of a vertical four-axis turntable;
FIG. 4 is a graph of a coordinate system transformation relationship based on 2-1-3 rotation Euler angles in the present invention;
FIG. 5 is a schematic diagram illustrating respective range divisions of the dual-Euler method of the present invention;
FIG. 6 is a schematic diagram of a result of inverse kinematics solution in the present invention;
FIG. 7 is a flow chart of a method for calculating an angular position command of a frame of the full-attitude flight simulation turntable according to the invention;
FIG. 8 is a schematic view of an attitude and angular position command of an aircraft in an embodiment;
FIG. 9 is a schematic diagram of a result obtained by a four-axis turntable frame angular position calculation method in the embodiment;
FIG. 10 is a schematic diagram of a simulation solution of the attitude and angular positions of the aircraft obtained from the positive solution of the frame angles of the four-axis turntable in the embodiment;
FIG. 11 is a schematic diagram of a calculated deviation between an expected attitude angle of the aircraft and an attitude angle simulation solution in the embodiment.
Detailed Description
The technical solutions in the embodiments of the present invention will be described clearly and completely with reference to the accompanying drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
With reference to fig. 1 to 11, the invention provides a method for calculating a frame angle instruction of a full-attitude four-axis turntable, which specifically comprises the following steps:
the method comprises the following steps: according to the structure of a four-axis turntable and the principle of simulating the attitude change of an aircraft, establishing a kinematics model of a frame angle of the four-axis turntable and an attitude angle of a full-space aircraft based on a double Euler method, wherein the kinematics model comprises an angular position kinematics model and an angular velocity kinematics model;
step two: inputting aircraft attitude at simulation initial moment
Figure BDA0002591872080000101
Gamma (0) is the roll angle at the simulated initial moment,
Figure BDA0002591872080000102
measuring initial position phi of each frame angle of the four-axis turntable to simulate yaw angle at initial moment, and theta (0) is pitch angle at the simulated initial moment0＝[φ10,φ20,φ30,φ40]TInitializing each frame angular position at the simulation initial moment according to the established angular position kinematic relationship between the four-axis turntable frame angle and the aircraft attitude angle to obtain phi (0) [ [ phi ] ]1(0),φ2(0),φ3(0),φ4(0)]T；
Step three: designing a turntable kinematics inverse solution algorithm based on a constraint optimization theory, and selecting an optimization variable in an optimization problem, namely each frame angular position phi [ [ phi ]1,φ2,φ3,φ4]TConstraint h (Φ) ═ h1(Φ),…,hm(Φ)]TM represents the number of constraint conditions, an objective function f (phi) and an optimization algorithm;
step four: inputting the attitude of the aircraft to be simulated at the current moment, namely the nth moment
Figure BDA0002591872080000111
Aircraft attitude simulation solution at the previous moment, namely the (n-1) th moment
Figure BDA0002591872080000112
And each frame angular position phi (n-1) of the four-axis turntable at the previous moment is equal to [ phi ]1(n-1),φ2(n-1),φ3(n-1),φ4(n-1)]TCalculating to obtain an angular position command phi (n) of each frame of the four-axis turntable at the current moment as [ phi (n) ]according to a kinematics inverse solution algorithm1(n),φ2(n),φ3(n),φ4(n)]T；
Step five: designing a kinematics positive solution algorithm of the four-axis turntable, and changing the angle position command phi (n) of each frame into [ phi ] according to the current moment1(n),φ2(n),φ3(n),φ4(n)]TAnd resolving to obtain the attitude simulation solution of the aircraft at the current moment
Figure BDA0002591872080000113
And will be
Figure BDA0002591872080000114
Inputting the motion data into inverse kinematics solution calculation at the next moment;
step six: and repeating the third step to the fifth step in an iterative manner, and resolving all the instructions of the angular position of the frame.
The four-axis rotary table is a vertical four-axis rotary table. As shown in fig. 2, fig. 2 is a schematic structural diagram of a vertical four-axis turntable applicable to the four-axis turntable, in which yaw, pitch, yaw, and roll attitudes of an aircraft are simulated sequentially from the outside to the inside, and the aircraft rotates around the Y-axis direction, the Z-axis direction, the Y-axis direction, and the X-axis direction when the attitudes change. Wherein, OXBYBZB、OXOYOZO、OXMYMZM、OXIYIZIAre respectively provided withRepresenting a base frame coordinate system, an outer frame coordinate system, an intermediate frame coordinate system, and an inner frame coordinate system. Fig. 3 shows the kinematic singular positions caused by the structural limitation of the vertical four-axis turntable. At the moment, the angular positions of the outer frame and the middle frame of the turntable are phi2K pi, k e Z and phi3Pi/2 + k pi, k e Z. At the moment, the four-axis turntable can only simulate the rolling and yawing or the rolling and pitching postures of the aircraft, and the pitching or yawing postures are lost, namely one degree of freedom is lost.
FIG. 1 is a graph of coordinate system transformation relation based on 2-3-1 rotation Euler angles in the present invention. Set the ground coordinate system of the aircraft in the space as OXGYGZGThe coordinate system of the projectile body is OXbYbZbThe Euler angles in the sequence of 2-3-1 represent the attitude change of the aircraft from the position of the ground coordinate system to the position of the projectile coordinate system, and can be decomposed into successive OY windingsG、OZ’、OX1Three elementary rotations of the axis. FIG. 4 is a graph of coordinate system transformation relation based on 2-1-3 rotation Euler angles in the present invention. Set the ground coordinate system of the aircraft in the space as OXGYGZGThe coordinate system of the projectile body is OXbYbZbThe Euler angles in the sequence of 2-1-3 represent the attitude change of the aircraft from the position of the ground coordinate system to the position of the projectile coordinate system, and can be decomposed into successive OY windingsG、OX’、OZ1Three elementary rotations of the axis. The first step is specifically as follows:
a positive Euler method representation angular position kinematic model for describing the attitude of the aircraft by using 2-3-1 rotation sequence attitude angles is as follows:
RG b＝RG l (1)
an inverse Euler method representation angular position kinematic model for describing the attitude of the aircraft by using 2-1-3 rotation order attitude angles is as follows:
Figure BDA0002591872080000121
Figure BDA0002591872080000122
Figure BDA0002591872080000123
Figure BDA0002591872080000124
wherein R isG bRepresenting an attitude matrix, R, describing the attitude change of the aircraft in order of 2-3-1 attitude anglesrG bRepresenting an attitude matrix, R, describing the attitude changes of the aircraft in attitude angles of 2-1-3 rotation orderG lRepresenting an attitude matrix for describing the attitude change of the vertical four-axis turntable load; gamma, the concentration of the gamma-rays,
Figure BDA0002591872080000125
theta respectively represents a rolling angle, a yaw angle and a pitch angle in the normal Eulerian method; gamma rayr，
Figure BDA0002591872080000126
θrRespectively representing a rolling angle, a yaw angle and a pitch angle in the anti-Eulerian method; phi is a1，φ2，φ3，φ4Respectively showing the frame angles of four frames of the vertical four-axis turntable from outside to inside; g represents a ground coordinate system, b represents an aircraft coordinate system, and l represents a load coordinate system;
a positive Euler method representation angular velocity kinematic model for describing the attitude of the aircraft by using a 2-3-1 rotation sequence attitude angle is as follows:
Figure BDA0002591872080000127
an inverse Euler method representation angular velocity kinematics model for describing the attitude of the aircraft by using 2-1-3 rotation sequence attitude angles is as follows:
Figure BDA0002591872080000128
Figure BDA0002591872080000129
Figure BDA00025918720800001210
Figure BDA0002591872080000131
wherein, JG bThe jacobian matrix is shown describing the aircraft as moving in 2-3-1 rotation order attitude angles,
Figure BDA0002591872080000132
respectively representing the rolling angular velocity, the yaw angular velocity and the pitch angular velocity in the positive Euler method; j. the design is a squarerG bThe jacobian matrix is shown describing the aircraft as moving in 2-1-3 rotation order attitude angles,
Figure BDA0002591872080000133
respectively representing the rolling angular velocity, the yaw angular velocity and the pitch angular velocity in the anti-Eulerian method; j. the design is a squareG lA jacobian matrix is shown describing the vertical four-axis turntable load motion,
Figure BDA0002591872080000134
Figure BDA0002591872080000135
respectively representing the frame angular velocities of the four frames of the vertical four-axis turntable from outside to inside; the vector of rotation of the aircraft is denoted ωb＝[ωbx,ωby,ωbz]TWherein ω isbx、ωby、ωbzRespectively projecting the rotation vector in X-axis, Y-axis and Z-axis directions of an aircraft coordinate system; the vector of rotation of the load is ωl＝[ωlx,ωly,ωlz]TWherein ω islx、ωly、ωlzThe projections of the rotation vector in the X-axis direction, the Y-axis direction and the Z-axis direction of the load coordinate system are respectively;
for each angular velocity in the formula (6) and the formula (7), angular position difference is adopted to approximate in engineering, the sampling time interval is set to be delta t, and the attitude angle of the aircraft at the current moment is respectively gamman，
Figure BDA0002591872080000136
θn，γr n，
Figure BDA0002591872080000137
θr nThe attitude angles at the previous time are gamman-1，
Figure BDA0002591872080000138
θn-1，γr n-1，
Figure BDA0002591872080000139
θr n-1Then, there are:
Figure BDA00025918720800001310
let the attitude matrix R in the formulas (1) and (2)G b＝RrG bThe (i, j) th entry in the matrix is denoted as RijThen, the conversion formula for calculating the inverse euler angle from the positive euler angle when the same attitude changes is described as follows:
Figure BDA00025918720800001311
when the positive Euler angle is solved by the inverse Euler angle, the conversion formula of the positive Euler angle is as follows:
Figure BDA0002591872080000141
in formulas (12) and (13), k is 0 or 1;
defining a rounding function Δ (x)1,x2) As shown in equation (14):
Δ(x1,x2)＝min(|x1-x2|,2π-|x1-x2|) (14)
when switching between positive and negative Euler angles in practical application, each Euler angle at the current moment is set as gamman，
Figure BDA0002591872080000142
θn，γr n，
Figure BDA0002591872080000143
θr nThe Euler angles at the previous time are gamman-1，
Figure BDA0002591872080000144
θn-1，γr n-1，
Figure BDA0002591872080000145
θr n-1Introducing a discriminant function dk
Figure BDA0002591872080000146
Respectively calculating d according to the values of k0And d1Get an order dkAnd a group of attitude angles with small values are used as conversion results of positive and negative Euler angles at the current moment.
FIG. 5 is a schematic diagram of the respective range divisions of the dual-Euler method of the present invention. When the attitude angle of the 2-3-1 input is divided into two Euler angle regions, the second angle in the positive Euler angle, namely the pitching attitude angle position of the aircraft, is taken as a target when
Figure BDA0002591872080000147
Then, as shown by the blank part in fig. 5, the eueulerian method is applied. When in use
Figure BDA0002591872080000148
Then, as shown by the shaded portion in FIG. 5, the inverse Euler method is applied.
In the second step, the first step is carried out,
aircraft attitude at initial time based on input simulation
Figure BDA0002591872080000149
And measuring the initial position phi of each frame angle of the four-axis rotary table0＝[φ10,φ20,φ30,φ40]TKeeping the angular position phi of the base frame of the four-axis turntable at the initial simulation time1(0)＝φ10At the same time, the other three frame angular positions phi2(0)，φ3(0)，φ4(0) Satisfy the requirement of
Figure BDA00025918720800001412
Wherein R isX(γ (0)) represents a rotation matrix rotating about the X-axis by an angle γ (0), RZ(theta (0)) represents a rotation matrix rotated by an angle theta (0) around the Z axis,
Figure BDA00025918720800001410
indicating rotation about the Y axis
Figure BDA00025918720800001411
A rotation matrix of angles; rY(φ1(0))，RX(φ4(0))，RY(φ3(0))，RZ(φ2(0) The same applies;
let RX(φ4(0))RY(φ3(0))RZ(φ2(0) T) then there are
Figure BDA0002591872080000151
In the formula (17), S represents a sine function sin (·), and C represents a cosine function cos (·);
the initial angular positions of the pitching outer frame, the yawing middle frame and the rolling inner frame in the vertical four-axis turntable are provided
Figure BDA0002591872080000152
Figure BDA0002591872080000153
The solving formula is as follows:
Figure BDA0002591872080000154
wherein the content of the first and second substances,
Figure BDA0002591872080000155
k is 1 or-1, corresponding to two groups of results. According to the two groups of frame angular position instructions obtained by calculation, two groups of actual attitude angles can be obtained through kinematic positive solution and are respectively recorded as
Figure BDA0002591872080000156
And
Figure BDA0002591872080000157
Figure BDA0002591872080000158
the actual attitude angles of the aircraft at the current moment are gamma, phi and theta respectively; introducing a discriminant function dk
Figure BDA0002591872080000159
Respectively calculating d according to the values of k1，d-1Get an order dkGroup of small values
Figure BDA00025918720800001510
As the initial angular positions of the three frames inside the four-axis turntable at the current moment.
In the third step, the first step is carried out,
in engineering, kinematicsThe inverse solution problem is converted into an optimization problem to be solved. The optimization variable of the optimization problem is selected as each frame angular position, and when a certain moment t is set, each frame angular position of the four-axis rotary table is phi1(t)，φ2(t)，φ3(t)，φ4(t); for the differential form of the frame angular positions involved in the optimization scheme, the differential form is adopted for replacement in the engineering, and the angular positions of the frames of the four-axis rotary table at the previous moment are respectively set as phi1(t-1)，φ2(t-1)，φ3(t-1)，φ4(t-1) the time interval between two moments is Δ t, the differential form of the angular position of the frames at t, i.e. the angular velocity of the frames
Figure BDA00025918720800001511
Figure BDA00025918720800001512
Can be finished to obtain:
Figure BDA0002591872080000161
the optimization problem objective function is designed as:
Figure BDA0002591872080000162
wherein, w1，w2，w3，w4The weighting coefficients are respectively the angular velocity of each frame and represent the motion capability and the dynamic performance of each frame, and the four weighting coefficients meet w when being taken as values1＞w2＞w3＞w4＞0；w1，w2，w4The value of (a) is designed and set according to the dynamic response difference of the corresponding frame angle, and for the aircraft with the attitude which does not change at the same time and changes within a large range, generally, w is taken1＝w2＝w4＝Δt2(ii) a Weighting coefficient w of middle frame3Is influenced by the outer frame and middle frame angular positions, and the image of the outer frame and middle frame angular positions to the middle frame weighting coefficientThe sounds are independent of each other; therefore, in the engineering, in order to avoid the singular position of the kinematics of the four-axis turntable, the weighting coefficient w is used3Expressed as a two-dimensional normal distribution probability density function:
Figure BDA0002591872080000163
wherein x and y are respectively mod (| φ)2|,2π)，mod(|φ3L, 2 pi) representing the angular positions of the outer frame and the middle frame of the four-axis turntable; the parameter K describes the height of the probability density function, the parameter D describes the weighting coefficient of the intermediate frame, mu, away from the singular position1And mu2Describing the kinematic singular positions of the four-axis turntable, namely the corresponding angular positions of the outer frame and the middle frame; sigma1And sigma2Describing the change rate and the change amplitude of the frame weighting coefficient in the position close to the singular position; in engineering, the value of each parameter is mu1＝π/2，μ2＝π，K＝350，D＝1，σ1＝π/9，σ2＝π/9。
The constraint conditions are equality constraint, and the equality constraint in engineering is acted by the full-attitude aircraft angular velocity kinematics equation based on the double Euler method, so that
Figure BDA0002591872080000164
Figure BDA0002591872080000165
Figure BDA0002591872080000171
Then, the equality constraint is:
Figure BDA0002591872080000172
wherein the content of the first and second substances,
Figure BDA0002591872080000173
Figure BDA0002591872080000174
Figure BDA0002591872080000175
the optimization algorithm selects a Lagrange multiplier method, and Z represents an integer set.
In the fifth step, the kinematics positive solution algorithm of the four-axis turntable is designed, and specifically comprises the following steps:
according to four frame angular positions of the four-axis rotary table, the terminal load of the rotary table, namely three attitude angles of the equivalent simulated aircraft, satisfy the equation:
Figure BDA0002591872080000176
the solving formula of the load attitude angle is as follows:
Figure BDA0002591872080000181
in formula (29)
Figure BDA0002591872080000182
k takes 1 or-1, corresponding to two groups of results;
defining a rounding function Δ (x)1,x2)＝min(|x1-x2|,2π-|x1-x2| in the kinematics positive solution process, each attitude angle of the load at a certain moment is set as gamma respectivelyn、
Figure BDA0002591872080000183
θnThen, each attitude angle at the previous time is γn-1、
Figure BDA0002591872080000184
θn-1Introducing a discriminant function dk
Figure BDA0002591872080000185
Wherein k is 1 or-1; respectively calculating d according to the values of k1，d-1Get an order dkAnd a group of attitude angles with small values are used as a kinematic positive solution result at the current moment.
Examples
In order to verify the effectiveness of the method, a vertical four-axis turntable is taken as a research object, the attitude of the aircraft to be simulated is input into a turntable system in a 2-3-1 rotation sequence, as shown in fig. 8, the sampling frequency is 2000Hz, namely the sampling time interval is delta t which is 0.0005s, and the total flight time of the aircraft is about 360 s. The maximum pitch angle of the attitude input command of the aircraft reaches 90 degrees, and the condition of vertical launching of the aircraft can be simulated. The frame angle position instruction is resolved by a full-attitude four-axis turntable frame angle instruction resolving method, and the effectiveness of the method is verified. The method comprises the following specific steps:
(1) according to the flow shown in fig. 7, the angular positions of the frames of the four-axis turntable are initialized. All frames of the four-axis rotary table are in zero position before simulation begins, and the expected attitude angle of the aircraft is also in zero position at the initial moment of the simulation, so all frames of the four-axis rotary table obtained by initialization are still exactly kept in zero position, namely phi1(0)＝0，φ2(0)＝0，φ3(0)＝0，φ4(0)＝0，Φ(0)＝[0,0,0,0]T。
(2) And inputting the expected aircraft attitude angle at each moment, the aircraft attitude simulation solution at the previous moment and the angular positions of the frames of the four-axis platform at the previous moment into the system, and resolving to obtain the angular position commands of the frames corresponding to the current moment.
(3) And (4) putting the angular position instructions of each frame at the current moment into kinematics positive solution calculation, and resolving to obtain a simulation solution of the attitude of the aircraft at the current moment.
(4) And (3) repeating the steps (2) and (3) according to the aircraft attitude command expected at the next moment, the aircraft attitude simulation solution obtained by calculation in the step (3) and each frame angular position command obtained by calculation in the step (2), and gradually obtaining all frame angular position commands.
According to the full-attitude four-axis turntable frame angle instruction calculation method, the final calculation result of each frame angle position is shown in fig. 9. The aircraft attitude simulation solution obtained through the kinematics positive solution is shown in fig. 10, and the resolving error between the attitude simulation solution and the aircraft expected attitude command is shown in fig. 11, so that the full-attitude four-axis turntable frame angle resolving method provided by the invention has good resolving accuracy, and meanwhile, large-amplitude error jump caused by full-attitude simulation can be avoided.
The method for calculating the frame angle instruction of the full-attitude four-axis turntable is described in detail, a specific example is applied to explain the principle and the implementation mode of the method, and the description of the embodiment is only used for helping to understand the method and the core idea of the method; meanwhile, for a person skilled in the art, according to the idea of the present invention, there may be variations in the specific embodiments and the application scope, and in summary, the content of the present specification should not be construed as a limitation to the present invention.

Claims (6)

1. A full-attitude four-axis turntable frame angle instruction resolving method is characterized by comprising the following steps: the method specifically comprises the following steps:
the method comprises the following steps: according to the structure of a four-axis turntable and the principle of simulating the attitude change of an aircraft, establishing a kinematics model of a frame angle of the four-axis turntable and an attitude angle of a full-space aircraft based on a double Euler method, wherein the kinematics model comprises an angular position kinematics model and an angular velocity kinematics model;
step two: inputting aircraft attitude at simulation initial moment
Figure FDA0002591872070000011
Gamma (0) is the roll angle at the simulated initial moment,
Figure FDA0002591872070000012
measuring initial position phi of each frame angle of the four-axis turntable to simulate yaw angle at initial moment, and theta (0) is pitch angle at the simulated initial moment0＝[φ10,φ20,φ30,φ40]TInitializing each frame angular position at the simulation initial moment according to the established angular position kinematic relationship between the four-axis turntable frame angle and the aircraft attitude angle to obtain phi (0) [ [ phi ] ]1(0),φ2(0),φ3(0),φ4(0)]T；
Step three: designing a turntable kinematics inverse solution algorithm based on a constraint optimization theory, and selecting an optimization variable in an optimization problem, namely each frame angular position phi [ [ phi ]1,φ2,φ3,φ4]TConstraint h (Φ) ═ h1(Φ),…,hm(Φ)]TM represents the number of constraint conditions, an objective function f (phi) and an optimization algorithm;
step four: inputting the attitude of the aircraft to be simulated at the current moment, namely the nth moment
Figure FDA0002591872070000013
Aircraft attitude simulation solution at the previous moment, namely the (n-1) th moment
Figure FDA0002591872070000014
And each frame angular position phi (n-1) of the four-axis turntable at the previous moment is equal to [ phi ]1(n-1),φ2(n-1),φ3(n-1),φ4(n-1)]TCalculating to obtain an angular position command phi (n) of each frame of the four-axis turntable at the current moment as [ phi (n) ]according to a kinematics inverse solution algorithm1(n),φ2(n),φ3(n),φ4(n)]T；
Step five: designing a kinematics positive solution algorithm of the four-axis turntable, and changing the angle position command phi (n) of each frame into [ phi ] according to the current moment1(n),φ2(n),φ3(n),φ4(n)]TAnd resolving to obtain the attitude simulation solution of the aircraft at the current moment
Figure FDA0002591872070000015
And will be
Figure FDA0002591872070000016
Inputting the motion data into inverse kinematics solution calculation at the next moment;
step six: and repeating the third step to the fifth step in an iterative manner, and resolving all the instructions of the angular position of the frame.
2. The method of claim 1, wherein: the four-axis rotary table is a vertical four-axis rotary table.
3. The method of claim 2, wherein: the first step is specifically as follows:
a positive Euler method representation angular position kinematic model for describing the attitude of the aircraft by using 2-3-1 rotation sequence attitude angles is as follows:
RG b＝RG l (1)
an inverse Euler method representation angular position kinematic model for describing the attitude of the aircraft by using 2-1-3 rotation order attitude angles is as follows:
RrG b＝RG l (2)
Figure FDA0002591872070000021
Figure FDA0002591872070000022
Figure FDA0002591872070000023
wherein R isG bRepresenting an attitude matrix, R, describing the attitude change of the aircraft in order of 2-3-1 attitude anglesrG bRepresenting an attitude matrix, R, describing the attitude changes of the aircraft in attitude angles of 2-1-3 rotation orderG lRepresenting an attitude matrix for describing the attitude change of the vertical four-axis turntable load; gamma, the concentration of the gamma-rays,
Figure FDA0002591872070000024
theta respectively represents a rolling angle, a yaw angle and a pitch angle in the normal Eulerian method; gamma rayr，
Figure FDA0002591872070000025
θrRespectively representing a rolling angle, a yaw angle and a pitch angle in the anti-Eulerian method; phi is a1，φ2，φ3，φ4Respectively showing the frame angles of four frames of the vertical four-axis turntable from outside to inside; g represents a ground coordinate system, b represents an aircraft coordinate system, and l represents a load coordinate system;
a positive Euler method representation angular velocity kinematic model for describing the attitude of the aircraft by using a 2-3-1 rotation sequence attitude angle is as follows:
Figure FDA0002591872070000026
an inverse Euler method representation angular velocity kinematics model for describing the attitude of the aircraft by using 2-1-3 rotation sequence attitude angles is as follows:
Figure FDA0002591872070000027
Figure FDA0002591872070000028
Figure FDA0002591872070000029
Figure FDA0002591872070000031
wherein, JG bThe jacobian matrix is shown describing the aircraft as moving in 2-3-1 rotation order attitude angles,
Figure FDA0002591872070000032
respectively representing the rolling angular velocity, the yaw angular velocity and the pitch angular velocity in the positive Euler method; j. the design is a squarerG bThe jacobian matrix is shown describing the aircraft as moving in 2-1-3 rotation order attitude angles,
Figure FDA0002591872070000033
respectively representing the rolling angular velocity, the yaw angular velocity and the pitch angular velocity in the anti-Eulerian method; j. the design is a squareG lA jacobian matrix is shown describing the vertical four-axis turntable load motion,
Figure FDA0002591872070000034
Figure FDA0002591872070000035
respectively representing the frame angular velocities of the four frames of the vertical four-axis turntable from outside to inside; the vector of rotation of the aircraft is denoted ωb＝[ωbx,ωby,ωbz]TWherein ω isbx、ωby、ωbzRespectively projecting the rotation vector in X-axis, Y-axis and Z-axis directions of an aircraft coordinate system; the vector of rotation of the load is ωl＝[ωlx,ωly,ωlz]TWherein ω islx、ωly、ωlzThe projections of the rotation vector in the X-axis direction, the Y-axis direction and the Z-axis direction of the load coordinate system are respectively;
for each angular velocity in the formula (6) and the formula (7), angular position difference is adopted to approximate in engineering, the sampling time interval is set to be delta t, and the attitude angle of the aircraft at the current moment is respectively set to be delta tγn，
Figure FDA0002591872070000036
θn，γr n，
Figure FDA0002591872070000037
θr nThe attitude angles at the previous time are gamman-1，
Figure FDA0002591872070000038
θn-1，γr n-1，
Figure FDA0002591872070000039
θr n-1Then, there are:
Figure FDA00025918720700000310
let the attitude matrix R in the formulas (1) and (2)G b＝RrG bThe (i, j) th entry in the matrix is denoted as RijThen, the conversion formula for calculating the inverse euler angle from the positive euler angle when the same attitude changes is described as follows:
Figure FDA00025918720700000311
when the positive Euler angle is solved by the inverse Euler angle, the conversion formula of the positive Euler angle is as follows:
Figure FDA0002591872070000041
in formulas (12) and (13), k is 0 or 1;
defining a rounding function Δ (x)1,x2) As shown in equation (14):
Δ(x1,x2)＝min(|x1-x2|,2π-|x1-x2|) (14)
when switching between positive and negative Euler angles in practical application, each Euler angle at the current moment is set as gamman，
Figure FDA0002591872070000042
θn，γr n，
Figure FDA0002591872070000043
θr nThe Euler angles at the previous time are gamman-1，
Figure FDA0002591872070000044
θn-1，γr n-1，
Figure FDA0002591872070000045
θr n-1Introducing a discriminant function dk
Figure FDA0002591872070000046
Respectively calculating d according to the values of k0And d1Get an order dkAnd a group of attitude angles with small values are used as conversion results of positive and negative Euler angles at the current moment.
4. The method of claim 3, wherein: in the second step, the first step is carried out,
aircraft attitude at initial time based on input simulation
Figure FDA0002591872070000047
And measuring the initial position phi of each frame angle of the four-axis rotary table0＝[φ10,φ20,φ30,φ40]TKeeping the angular position phi of the base frame of the four-axis turntable at the initial simulation time1(0)＝φ10Is not changedThe other three frame angular positions phi2(0)，φ3(0)，φ4(0) Satisfy the requirement of
Figure FDA0002591872070000048
Wherein R isX(γ (0)) represents a rotation matrix rotating about the X-axis by an angle γ (0), RZ(theta (0)) represents a rotation matrix rotated by an angle theta (0) around the Z axis,
Figure FDA0002591872070000049
indicating rotation about the Y axis
Figure FDA00025918720700000410
A rotation matrix of angles; rY(φ1(0))，RX(φ4(0))，RY(φ3(0))，RZ(φ2(0) The same applies;
let RX(φ4(0))RY(φ3(0))RZ(φ2(0) T) then there are
Figure FDA00025918720700000411
Figure FDA0002591872070000051
In the formula (17), S represents a sine function sin (·), and C represents a cosine function cos (·);
the initial angular positions of the pitching outer frame, the yawing middle frame and the rolling inner frame in the vertical four-axis turntable are provided
Figure FDA0002591872070000052
Figure FDA0002591872070000053
Solving methodThe formula is as follows:
Figure FDA0002591872070000054
wherein the content of the first and second substances,
Figure FDA0002591872070000055
k is 1 or-1, two groups of actual attitude angles can be obtained through kinematics positive solution according to two groups of frame angular position instructions obtained through solution, and are respectively recorded as
Figure FDA0002591872070000056
And
Figure FDA0002591872070000057
the actual attitude angles of the aircraft at the current moment are gamma, phi and theta respectively; introducing a discriminant function dk
Figure FDA0002591872070000058
Respectively calculating d according to the values of k1，d-1Get an order dkGroup of small values
Figure FDA0002591872070000059
As the initial angular positions of the three frames inside the four-axis turntable at the current moment.
5. The method of claim 4, wherein: in the third step, the first step is carried out,
the optimization variable of the optimization problem is selected as each frame angular position, and when a certain moment t is set, each frame angular position of the four-axis rotary table is phi1(t)，φ2(t)，φ3(t)，φ4(t); for the differential form of the frame angular positions involved in the optimization scheme, the differential form is adopted for replacement in the engineering, and the angular positions of the frames of the four-axis rotary table at the previous moment are respectively set as phi1(t-1)，φ2(t-1)，φ3(t-1)，φ4(t-1) the time interval between two moments is Δ t, the differential form of the angular position of the frames at t, i.e. the angular velocity of the frames
Figure FDA00025918720700000510
Can be finished to obtain:
Figure FDA00025918720700000511
the optimization problem objective function is designed as:
Figure FDA0002591872070000061
wherein, w1，w2，w3，w4The weighting coefficients are respectively the angular velocity of each frame and represent the motion capability and the dynamic performance of each frame, and the four weighting coefficients meet w when being taken as values1＞w2＞w3＞w4＞0；w1，w2，w4The value of (a) is designed and set according to the dynamic response difference of the corresponding frame angle, and for the aircrafts with postures which are different and change within a large range, w1＝w2＝w4＝Δt2(ii) a Weighting coefficient w of middle frame3The angular positions of the outer frame and the middle frame are jointly influenced, and the influences of the angular positions of the outer frame and the middle frame on the weighting coefficient of the middle frame are mutually independent; therefore, in the engineering, in order to avoid the singular position of the kinematics of the four-axis turntable, the weighting coefficient w is used3Expressed as a two-dimensional normal distribution probability density function:
Figure FDA0002591872070000062
wherein x and y are respectively mod (| φ)2|,2π)，mod(|φ3L, 2 pi), representing the outer frame and the middle frame of the four-axis turntableThe angular position of the frame; the parameter K describes the height of the probability density function, the parameter D describes the weighting coefficient of the intermediate frame, mu, away from the singular position1And mu2Describing the kinematic singular positions of the four-axis turntable, namely the corresponding angular positions of the outer frame and the middle frame; sigma1And sigma2Describing the change rate and the change amplitude of the frame weighting coefficient in the position close to the singular position;
the constraint conditions are equality constraint, and the equality constraint in engineering is acted by the full-attitude aircraft angular velocity kinematics equation based on the double Euler method, so that
Figure FDA0002591872070000063
Figure FDA0002591872070000064
Figure FDA0002591872070000065
Then, the equality constraint is:
Figure FDA0002591872070000071
wherein the content of the first and second substances,
Figure FDA0002591872070000072
the optimization algorithm selects a Lagrange multiplier method, and Z represents an integer set.
6. The method of claim 5, wherein: in the fifth step, the kinematics positive solution algorithm of the four-axis turntable is designed, and specifically comprises the following steps:
according to four frame angular positions of the four-axis rotary table, the terminal load of the rotary table, namely three attitude angles of the equivalent simulated aircraft, satisfy the equation:
Figure FDA0002591872070000073
the solving formula of the load attitude angle is as follows:
Figure FDA0002591872070000074
in formula (29)
Figure FDA0002591872070000075
k takes 1 or-1, corresponding to two groups of results;
defining a rounding function Δ (x)1,x2)＝min(|x1-x2|,2π-|x1-x2| in the kinematics positive solution process, each attitude angle of the load at a certain moment is set as gamma respectivelyn、
Figure FDA0002591872070000081
θnThen, each attitude angle at the previous time is γn-1、
Figure FDA0002591872070000082
θn-1Introducing a discriminant function dk
Figure FDA0002591872070000083
Wherein k is 1 or-1; respectively calculating d according to the values of k1，d-1Get an order dkAnd a group of attitude angles with small values are used as a kinematic positive solution result at the current moment.
CN202010697584.8A 2020-07-20 2020-07-20 Full-attitude four-axis turntable frame angle instruction resolving method Active CN111914411B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010697584.8A CN111914411B (en) 2020-07-20 2020-07-20 Full-attitude four-axis turntable frame angle instruction resolving method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010697584.8A CN111914411B (en) 2020-07-20 2020-07-20 Full-attitude four-axis turntable frame angle instruction resolving method

Publications (2)

Publication Number Publication Date
CN111914411A true CN111914411A (en) 2020-11-10
CN111914411B CN111914411B (en) 2022-11-11

Family

ID=73280651

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010697584.8A Active CN111914411B (en) 2020-07-20 2020-07-20 Full-attitude four-axis turntable frame angle instruction resolving method

Country Status (1)

Country Link
CN (1) CN111914411B (en)

Cited By (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112484720A (en) * 2020-11-17 2021-03-12 天津津航计算技术研究所 double-Euler full-attitude calculation method based on strapdown inertial navigation
CN113934155A (en) * 2021-09-17 2022-01-14 北京星途探索科技有限公司 Semi-physical simulation method for verifying 3-2-1 rotation flight motion model by using vertical three-axis turntable
CN114184210A (en) * 2021-12-03 2022-03-15 江西洪都航空工业集团有限责任公司 Simulation test method, device and system based on horizontal turntable
CN114355787A (en) * 2021-09-17 2022-04-15 北京星途探索科技有限公司 Shaft-lacking turntable semi-physical simulation verification technology based on certain type supersonic cruise target
CN114518709A (en) * 2022-01-26 2022-05-20 哈尔滨工业大学 Full-attitude four-axis turntable frame angle instruction resolving method, equipment and medium
CN114636357A (en) * 2022-03-31 2022-06-17 北京中科宇航技术有限公司 Aiming test design method for vertical turntable shaking state
CN116090097A (en) * 2022-12-30 2023-05-09 北京机电工程研究所 Near-water surface fluid-solid coupling finite element efficient calculation method based on equivalent water collision design
CN114518709B (en) * 2022-01-26 2024-05-17 哈尔滨工业大学 Method, equipment and medium for resolving frame angle instruction of full-attitude four-axis turntable

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20070288101A1 (en) * 2006-06-08 2007-12-13 Liu Hugh H T Method, system and computer program for generic synchronized motion control for multiple dynamic systems
JP2016198873A (en) * 2015-04-14 2016-12-01 トヨタ自動車株式会社 Optimum control device, optimum control method, and optimum control program
US20160355279A1 (en) * 2015-06-02 2016-12-08 The Charles Stark Draper Laboratory, Inc. Rapid slew and settle systems for small satellites
CN107247157A (en) * 2017-05-10 2017-10-13 哈尔滨工程大学 Change the acquisition methods of Eulerian angles in a kind of quaternary number full-shape domain towards big attitude maneuver
CN109634293A (en) * 2018-12-05 2019-04-16 浙江大学 A kind of fixed-wing unmanned plane roller flowing control method
US20190111562A1 (en) * 2017-10-18 2019-04-18 Foshan Huashu Robotics Co., Ltd. Numerical method for obtaining the inverse kinematics of six-degree-of-freedom serial robot with an offset wrist
US20200055192A1 (en) * 2018-08-16 2020-02-20 Hehua Ju Axis-Invariant based Multi-axis robot inverse kinematics modeling and solving method

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20070288101A1 (en) * 2006-06-08 2007-12-13 Liu Hugh H T Method, system and computer program for generic synchronized motion control for multiple dynamic systems
JP2016198873A (en) * 2015-04-14 2016-12-01 トヨタ自動車株式会社 Optimum control device, optimum control method, and optimum control program
US20160355279A1 (en) * 2015-06-02 2016-12-08 The Charles Stark Draper Laboratory, Inc. Rapid slew and settle systems for small satellites
CN107247157A (en) * 2017-05-10 2017-10-13 哈尔滨工程大学 Change the acquisition methods of Eulerian angles in a kind of quaternary number full-shape domain towards big attitude maneuver
US20190111562A1 (en) * 2017-10-18 2019-04-18 Foshan Huashu Robotics Co., Ltd. Numerical method for obtaining the inverse kinematics of six-degree-of-freedom serial robot with an offset wrist
US20200055192A1 (en) * 2018-08-16 2020-02-20 Hehua Ju Axis-Invariant based Multi-axis robot inverse kinematics modeling and solving method
CN109634293A (en) * 2018-12-05 2019-04-16 浙江大学 A kind of fixed-wing unmanned plane roller flowing control method

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
BAOXIANG XING: "Multi-DOF Motion control system design and realization based on etherCAT", 《IEEE》 *
KOON KIAT TEU: "Using dual euler angles for the analysis of arm movement during the badminton smash", 《SPORTS ENGINEERING》 *
徐勤贝: "基于约束最优化理论的四轴转台框架角解算方法研究", 《中国优秀硕士学位论文全文数据库》 *
杨宝庆: "飞行器半实物仿真装备研究进展与展望", 《宇航学报》 *
邢宝祥: "基于位置域迭代学习的激光导引头测试系统时变周期干扰抑制", 《红外与激光工程》 *

Cited By (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112484720A (en) * 2020-11-17 2021-03-12 天津津航计算技术研究所 double-Euler full-attitude calculation method based on strapdown inertial navigation
CN113934155A (en) * 2021-09-17 2022-01-14 北京星途探索科技有限公司 Semi-physical simulation method for verifying 3-2-1 rotation flight motion model by using vertical three-axis turntable
CN114355787A (en) * 2021-09-17 2022-04-15 北京星途探索科技有限公司 Shaft-lacking turntable semi-physical simulation verification technology based on certain type supersonic cruise target
CN114184210A (en) * 2021-12-03 2022-03-15 江西洪都航空工业集团有限责任公司 Simulation test method, device and system based on horizontal turntable
CN114518709A (en) * 2022-01-26 2022-05-20 哈尔滨工业大学 Full-attitude four-axis turntable frame angle instruction resolving method, equipment and medium
CN114518709B (en) * 2022-01-26 2024-05-17 哈尔滨工业大学 Method, equipment and medium for resolving frame angle instruction of full-attitude four-axis turntable
CN114636357A (en) * 2022-03-31 2022-06-17 北京中科宇航技术有限公司 Aiming test design method for vertical turntable shaking state
CN114636357B (en) * 2022-03-31 2023-11-10 北京中科宇航技术有限公司 Aiming test design method aiming at shaking state of vertical turntable
CN116090097A (en) * 2022-12-30 2023-05-09 北京机电工程研究所 Near-water surface fluid-solid coupling finite element efficient calculation method based on equivalent water collision design

Also Published As

Publication number Publication date
CN111914411B (en) 2022-11-11

Similar Documents

Publication Publication Date Title
CN111914411B (en) Full-attitude four-axis turntable frame angle instruction resolving method
CN107479567B (en) The unknown quadrotor drone attitude controller of dynamic characteristic and method
Kang et al. Deep convolutional identifier for dynamic modeling and adaptive control of unmanned helicopter
CN108897338B (en) Circular orbit spacecraft formation reconfiguration anti-collision path planning method based on PIO
CN108427428B (en) Self-adaptive sliding mode variable structure spacecraft attitude control method based on improved iterative algorithm
CN113296507B (en) Multi-power positioning ship cooperative formation control method based on space-time decoupling
CN111695193B (en) Modeling method and system of globally relevant three-dimensional aerodynamic mathematical model
CN112327926B (en) Self-adaptive sliding mode control method for unmanned aerial vehicle formation
CN114911265A (en) Four-rotor unmanned aerial vehicle formation cooperative maneuvering control method
CN110333733A (en) A kind of the tandem variable universe fuzzy attitude control system and method for quadrotor
CN115366109A (en) Composite layered anti-interference method for rotor flight mechanical arm
CN111506095A (en) Method for tracking and controlling relative pose of saturation fixed time between double rigid body feature points
CN110282121A (en) Coordinate system conversion method, its device, aircraft course control method and aircraft
Huang et al. Adaptive backstepping control for autonomous shipboard landing of a quadrotor with input saturation
CN113253617A (en) Online self-adaptive control method for quad-rotor unmanned aerial vehicle
CN111880552B (en) Multi-rotor unmanned aerial vehicle trajectory tracking composite control method
Ali et al. Adaptive backstepping sliding mode control of coaxial octorotor unmanned aerial vehicle
CN112683261A (en) Unmanned aerial vehicle robustness navigation method based on speed prediction
CN114671050A (en) Spacecraft tracking control method based on integrated linear operator and anti-saturation technology
CN110480641B (en) Recursive distributed rapid convergence robust control method for mechanical arm
CN113885549A (en) Four-rotor attitude trajectory control method based on dimension cutting PPO algorithm
CN113792473A (en) Modeling and using method of unmanned aerial vehicle dynamic network prediction model and related equipment
CN114518709B (en) Method, equipment and medium for resolving frame angle instruction of full-attitude four-axis turntable
CN111872943B (en) Robot arc track planning method based on sine curve
CN115576334B (en) Under-actuated underwater vehicle formation control method and system

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