A dynamic attitude measurement method for a dual-axis stabilized platform

Abstract

A dynamic attitude measurement method for a dual-axis stabilized platform is firstly performed by using an inertial coordinate system rough alignment method to obtain the initial value of the attitude angle, and then a series of navigation solutions of the attitude angle, attitude angular rate, velocity position are performed. In the navigation solution process, the Kalman filter technology is used to perform combined navigation and combined calibration of the strapdown inertial navigation system, and the misalignment angle, velocity error, and position error are estimated. The quaternion, solution velocity, and solution position of the strapdown inertial navigation system are corrected by using the Kalman filter technology to make the calculated navigation coordinate system coincide with the real navigation coordinate system as much as possible. At the same time, if time permits, the error (gyroscope drift and acceleration bias) of the inertial element is also estimated. The method can enable the dual-axis stabilized platform to maintain a stable accuracy of pitch angle and roll angle ≤10" (RMS) under the condition of a large load of an 80kg gravimeter and a large dynamic condition of the fourth level sea state.

Description

Technical Field

[0001] The invention relates to the field of gravimeter measurement, and in particular to a dynamic posture measurement method of a gravimeter dual-axis stable platform. Background Art

[0002] The dual-axis stabilized platform keeps the frame platform stable in the local geographic horizontal attitude to isolate the impact of horizontal attitude disturbance on the gravimeter. The attitude angle measurement of the previous dual-axis stabilized platform is obtained by complementary filtering and fusion of three accelerometers and two-axis horizontal gyroscope data. The two horizontal axis motors of roll and pitch are stabilized by the gyroscope angular velocity and the inclination angle of the accelerometer for geographic horizontal stabilization.

[0003] In the reference “Marine Gravimeter Stable Platform Control System” (Institute of Geodesy and Geophysics, Chinese Academy of Sciences, Wu Pengfei, Wang Long, Zou Zhou, Wang Yong, Marine Gravimeter Stable Platform Control System, Application (Patent) No.: 201620193744.4, 2016.03.14), a method of controlling directly through accelerometer and gyroscope data fusion is adopted. In addition, an angle sensor is required, which makes the structural processing accuracy high and difficult, and the measurement sensor noise is large, making it difficult to meet the needs of high-precision attitude measurement. Summary of the invention

[0004] The purpose of the present invention is to provide a dynamic attitude measurement method for a dual-axis stabilized platform. Since the attitude angle measurement of the previous dual-axis stabilized platform is obtained by complementary filtering and fusion of accelerometer and gyroscope data, there is not only filtering delay in large dynamics, but also easy to be disturbed by external linear acceleration. This type of stable platform can only achieve angular classification accuracy and is not suitable for high-precision occasions. The present invention proposes an attitude measurement method based on an inertial / satellite navigation (INS / GPS) combination, which avoids the measurement error caused by filtering delay and accelerometer interference through algorithm design, and can still maintain the gravimeter stable platform with a stable accuracy of 10" under large dynamic conditions of 80kg large load and level 4 sea conditions.

[0005] The technical scheme adopted to achieve the above purpose is a dynamic attitude measurement method of a dual-axis stabilized platform, which first adopts an inertial coordinate system coarse alignment method to perform coarse alignment, obtains the initial value of the attitude angle, and then performs a series of navigation solutions of the attitude angle, attitude angular rate, and velocity position. In the navigation solution process, the Kalman filter technology is used to perform combined navigation and combined calibration of the strapdown inertial navigation system, and the misalignment angle, velocity error, and position error are estimated. The quaternion, solution velocity, and solution position of the strapdown inertial navigation system are corrected with the Kalman filter technology to make the calculated navigation coordinate system coincide with the real navigation coordinate system as much as possible.

[0006] The workflow of the attitude solution algorithm module includes the following:

[0007] 1. Coarse alignment; 2. Attitude quaternion update; 3. Attitude angle calculation; 4. Attitude angular rate calculation; 5. Specific force coordinate transformation; 6. Speed ​​update; 7. Position update; 8. Kalman filter calculation; 9. Rotation control command calculation;

[0008] Step 1: Rough alignment

[0009] Rough alignment is to find the approximate azimuth cosine array It is the basis for Kalman precision alignment. The basic idea of ​​the scheme is to use the inertial coordinate system as the reference benchmark, initially fix the two inertial coordinate systems, then update the quaternion of the angular motion of the carrier relative to the inertial coordinate system, and use the transformation relationship between gravity and the linear motion of the carrier between the two inertial coordinate systems to obtain the transformation matrix of the two solidified inertial coordinate systems, and then infer

[0010] First, decompose the azimuth cosine matrix into

[0011]

[0012] The problem turns to solving and Here are the steps:

[0013] 1) Ask

[0014] Will Written

[0015]

[0016] in

[0017]

[0018] λ0 and L0 are the longitude and latitude at the start of alignment, λ t and L t To align the longitude and latitude at time t, substitute equations (3), (4), (5) and (6) into equation (2) to obtain

[0019]

[0020] 2) Ask

[0021] Same as the attitude update algorithm, the attitude quaternion is used to update the differential equation

[0022]

[0023] in is the carrier angular velocity of the gyro sensitive, and the alignment starts at time i b0The a system coincides with the b system.

[0024] 3) Ask

[0025] In the inertial system

[0026]

[0027] Accelerometer output under dynamic base

[0028]

[0029] g b is the projection of gravity on the b system, is the linear motion acceleration of the carrier

[0030] definition have

[0031]

[0032] Under the condition of uniform motion or constant amplitude swing motion of the carrier, the approximate have

[0033]

[0034] in, From step 1), we can find g n =[00-g] T

[0035] In addition, according to the basic equation of velocity

[0036]

[0037] Written

[0038]

[0039] but

[0040]

[0041] Among them, v b is the external reference ground speed, and defines The finishing formula (12) is

[0042]

[0043] in,

[0044] Take any two time points in the alignment process Obtain

[0045]

[0046] The above and Substituting into equation (1) we can obtain the azimuth cosine matrix:

[0047] The largest source of error in alignment is the acceleration of the carrier motion during alignment. For the dual-axis stabilized platform Relative to g b For small quantities, even when moving, only the carrier needs to be not too maneuverable for a short period of time to complete high-precision alignment, and the dynamic applicability is strong;

[0048] Step 2: Attitude quaternion update

[0049] The transformation relationship from n-system to p-system is represented by the rotation quaternion Q, which is composed of a real unit and 3 imaginary units. The hypercomplex number composed of 4 real elements is as follows:

[0050]

[0051] Among them, q0, q1, q2, q3 are four real numbers. is an orthogonal basis of imaginary units;

[0052] If the vector in space is fixed, if the moving coordinate system rotates an angle relative to the reference system, where the moving coordinate system is the p system and the reference coordinate system is the n system, the transformation relationship between the vector described by the quaternion in the two coordinate systems is

[0053]

[0054] In the formula,

[0055]

[0056] Substituting equation (20) into equation (19), we can calculate using quaternion:

[0057]

[0058] And by

[0059]

[0060] Obtain the transformation matrix

[0061]

[0062] The above is obtained using quaternions Transformation matrix: if we know the attitude quaternion Q from n system to p system, we can get the corresponding transformation matrix;

[0063] Assume t k The rotation coordinate system at the moment is p(k), the navigation coordinate system is n(k), t k+1 The carrier coordinate system at the moment is p(k+1), the navigation coordinate system is n(k+1), the rotation quaternion from p(k) to p(k+1) is q(h), and the rotation quaternion from n(k) to b(k) is Q(t k ), the rotation quaternion from n(k+1) to b(k+1) is Q(t k+1 ), the rotation quaternion from n(k) to n(k+1) is p(h), where h = t k+1 -t k is the update cycle, then

[0064]

[0065] According to formula (19), we have

[0066]

[0067] Using the equivalent relationship between equation (19) and equation (22), equation (24) is equivalent to

[0068]

[0069] Comparing equation (25) and equation (26), we get

[0070]

[0071] in

[0072]

[0073] Φ is the equivalent rotation vector from b(k) to b(k+1), and its numerical solution is obtained by the following equivalent rotation vector differential equation

[0074]

[0075] Where Φ = Φ(h) is a posture update period (t k ,t k+1 ] is the equivalent rotation vector from b(k) to b(k+1), ω is the carrier angular velocity vector, and the last two terms reflect the compensation of the rotation non-commutative error;

[0076] Formula (29) is solved by Taylor series expansion method. In a posture update cycle (t k ,t k+1], a polynomial fitting is performed on the carrier angular velocity, a multi-sample sampling is performed on the gyro signal with a sampling period shorter than the update period, the angular increment information of each sampling point is used to solve the coefficient of the fitting polynomial, and the fitted attitude angular velocity is substituted back into the Taylor series expansion of Φ(h) to obtain the numerical solution of the rotation vector;

[0077] If the carrier performs roll and pitch vibrations at the same frequency but different phases, a conic error will be generated on the azimuth axis. When the vibration satisfies a certain phase and frequency relationship, the vibration of any two axes will cause drift on the third axis. The conic error is compensated by the rotating vector four-sample optimization algorithm:

[0078]

[0079] Among them, Δθ1, Δθ2, Δθ3 and Δθ4 are respectively (t k ,t k +h / 4]、(t k +h / 4,t k +h / 2]、(t k +h / 2,t k +3h / 4]、(t k +3h / 4,t k+1 ] Gyro output angle increment within the sampling period;

[0080] In addition, the angle that the n system rotates relative to the i system during the update cycle is

[0081]

[0082] is the projection of the rotation angular rate of the e system relative to the i system in the n system, is the projection of the angular velocity of the n system relative to the e system on the n system, which is calculated by the following two formulas:

[0083]

[0084] The speed, latitude and curvature radius in the formula are all the results of the previous navigation solution update cycle,

[0085]

[0086] In one attitude update cycle, the rotation of the navigation coordinate system changes extremely slowly. is a minimum value and is not suitable as a dividend in a computer. Taylor expansion is performed on equation (34) to obtain a fourth-order approximation:

[0087]

[0088] Substitute equation (28) and equation (35) into equation (27) and perform normalization

[0089] Q(t k+1 )=Q(t k+1 ) / ||Q(t k+1 )|| Formula (36)

[0090] Step 3: Attitude Angle Calculation

[0091] Substituting the above result into equation (23), the attitude transformation matrix of the inertial measurement unit is calculated: The inertial measurement unit of the single-axis rotating laser inertial navigation system rotates around the rotation axis Oz p1 Rotation, assuming the rotation angle position is γ, the attitude transformation matrix from p1 system to b0 system is

[0092]

[0093] Attitude transformation matrix from n system to p1 system for

[0094]

[0095] After the rotation angle transformation, the attitude transformation matrix from the n system to the b0 system is for

[0096]

[0097] The general expression of the attitude transformation matrix of the inertial navigation system is:

[0098]

[0099] Calculate the attitude transformation matrix of the ship carrier

[0100]

[0101] in, It is the attitude zero array obtained after the equipment is calibrated with the true level and true heading on the ship;

[0102] Finally, the horizontal attitude angle and heading angle of the carrier are extracted in real time by the elements of formula (41):

[0103]

[0104]

[0105] The initial value of the quaternion is calculated from the initial attitude angle

[0106]

[0107] Among them, the initial values ​​of the attitude angles H0, P0, and R0 are determined by rough alignment;

[0108] Step 4: Calculation of attitude angular rate

[0109] According to the sampling period h (h = t k -t k-1 ) Take N samples of the smoothed quantity and let

[0110]

[0111] Re-order

[0112] A=(B T B) -1 B T Formula (47)

[0113] definition

[0114] A(2,:)=[A(2,1)A(2,2)...A(2,N)] Formula (48)

[0115] A(2,:) represents the second row of elements in matrix A, and the number of elements is N;

[0116] Current t k Attitude angular rate at time and The calculation is as follows

[0117]

[0118] Where R(t k+i-N )、P(t k+i-N ) and H(t k+i-N ) is t k+i-N Roll angle, pitch angle and heading angle at the moment;

[0119] Step 5: Comparative coordinate transformation

[0120] Accelerometer output specific force f p Through coordinate transformation

[0121]

[0122] The accelerometer is installed on the rotating axis of the rotating mechanism, ignoring the inner lever arm effect, that is,

[0123] Step 6: Speed ​​Update

[0124] Basic equations of inertial navigation

[0125]

[0126] in, is the projection of the geographic coordinate system relative to the earth coordinate system in the geographic coordinate system, called ground speed. Formula (51) can be written in the form of components:

[0127]

[0128] Right now

[0129]

[0130] Among them,

[0131]

[0132] δA is called harmful acceleration;

[0133] but

[0134]

[0135] In a speed update cycle (t k ,t k+1 ]Inside, there is

[0136]

[0137] The accelerometer output pulse is the velocity increment, and the integral term in equation (56) is calculated by the following equation:

[0138]

[0139] Among them, Recorded as Δv p is the velocity increment after the accelerometer output pulse number is scaled, and h is the update period;

[0140] The first term on the right side of formula (57) is the speed compensation caused by specific force;

[0141]

[0142] In the case of dynamic carrier, equation (58) will produce constant errors related to the carrier's linear motion and angular motion. If the carrier simultaneously makes angular motion in the linear motion direction, a rotation effect will be produced. If the carrier makes linear vibration along the longitudinal axis and angular vibration along the transverse axis with the same frequency and phase, a DC quantity will be produced in the azimuth axis direction of the carrier, that is, a constant velocity increment error is introduced. The above motion mode is similar to rowing motion: on the one hand, the paddle shakes periodically around the lateral axis of the boat, and on the other hand, the boat moves intermittently along the longitudinal axis, so it is called the rowing effect. The compensation method for the rotation effect and the rowing effect is to compensate for Δv p Multi-sub-sampling is performed, and the motion trajectory within the update period is fitted using the sample point information. The optimization goal is to minimize the error caused by the algorithm in the extreme case of paddling motion, thereby determining the coefficients of the fitting polynomial.

[0143] The rotation effect compensation term is a primary compensation term. The optimized four-sample algorithm is used for the paddling effect compensation term. The calculation formula is as follows:

[0144] Δv p =Δv+Δv rot +Δv scul Formula (59)

[0145] Among them, the rotation effect compensation

[0146]

[0147] Optimized four-sample paddling effect compensation

[0148]

[0149] Δv=Δv1+Δv2+Δv3+Δv4 Formula (62)

[0150] Δθ=Δθ1+Δθ2+Δθ3+Δθ4 Equation (63)

[0151] Δv1, Δv2, Δv3, and Δv4 are (t k ,t k +h / 4]、(t k +h / 4,t k +h / 2]、(t k +h / 2,t k +3h / 4]、(t k +3h / 4,t k+1 ] The velocity increment of the accelerometer output within the sampling period;

[0152] Step 7: Location Update

[0153] In the pure inertial working mode, the altitude channel of the inertial navigation system is divergent. For surface applications, a high-pass filter is used to obtain the instantaneous vertical velocity and instantaneous displacement;

[0154] The differential equation for longitude and latitude is as follows

[0155]

[0156] In a position update period (t k ,t k+1 ]Inside, there is

[0157]

[0158] Step 8: Kalman filter calculation

[0159] The algorithm is as follows:

[0160] a) Equation of state

[0161] Considering both gyroscope drift and accelerometer bias as random constant processes, we have

[0162] The state variable is selected as 15 dimensions

[0163]

[0164] The state equation of a continuous system is:

[0165]

[0166] From the inertial system error propagation equation, where:

[0167]

[0168]

[0169] Measuring noise for gyro drift and accelerometer bias;

[0170] b) Measurement equation

[0171] The measurement equation with position as the observation quantity is

[0172]

[0173] Where, subscript s represents the output of strapdown inertial navigation system solution, subscript r represents the reference output, and H p =[0 3×6 I 3×3 0 3×6 ], V is the 3D position observation white noise;

[0174] c) Revision

[0175] Kalman filtering estimates the misalignment angle and corrects the attitude array in real time. Then a more accurate attitude matrix is ​​obtained;

[0176] The misalignment angle φ is estimated using Kalman filtering technology E ,φ N ,φ U , the correction angle is:

[0177]

[0178] The quaternion method is used to correct it:

[0179]

[0180] in:

[0181] The correction of speed and position is replaced by the output value of speed and position filter;

[0182] d) Discretization of continuous systems

[0183] The above continuous system error model is established by a) state equation and b) measurement equation. Discretization is required to realize it in the program. Discretization is essentially to calculate the transfer matrix of the discrete system based on the system matrix of the continuous system, and to calculate the noise variance matrix of the discrete system based on the system process variance intensity matrix of the continuous system.

[0184] The state equation model of the continuous system is as follows:

[0185]

[0186] Where X is the n-dimensional state vector of the system, F is the n×n-dimensional system matrix, W is the p-dimensional system process noise, G is the n×p-dimensional noise input matrix, Z is the m-dimensional observation vector of the system, V is the m-dimensional observation noise, and H is the m×n-dimensional observation matrix;

[0187] 1)Φ k,k-1 Calculation

[0188] Using the calculation method of the steady-state system, the state transition matrix Φ k,k-1 The relationship with the system matrix F is as follows:

[0189]

[0190] Among them, (t k-1 ,t k ] is the prediction period, and h = t k -t k-1 , the prediction period h is generally short, and Taylor expansion of equation (83) yields

[0191]

[0192] 2) Calculation

[0193] The covariance matrix of the system noise vector W(t) of the continuous system is Q(t), and the variance matrix of the input noise is:

[0194] Q q =G(t)Q(t)G T (t) Formula (85)

[0195] Input noise variance of Kalman filter The input noise variance Q of the continuous system q There are the following relationships:

[0196]

[0197] Step 9: Calculation of rotation control instructions

[0198] The goal of the stable platform is to isolate the horizontal angular motion of the carrier. For control, the constraint condition is to make

[0199] ∫(R+α)dt=0, ∫(P+β)dt=0, where R is the roll angle, P is the pitch angle, α is the roll angle command, and β is the pitch angle command, that is, the rotation must have the function of tracking the change of the horizontal attitude angle;

[0200] The output command angle is transferred to the control algorithm for calculation and converted into a control quantity or control signal.

[0201] Beneficial Effects

[0202] Compared with the prior art, the present invention has the following advantages.

[0203] The present invention proposes an attitude measurement method based on inertial / satellite navigation (INS / GPS) combination, which avoids the measurement error caused by filtering delay and accelerometer interference through algorithm design. The method enables the dual-axis stabilized platform to maintain a stable accuracy of pitch angle and roll angle ≤10" (RMS) under the load condition of an 80kg gravimeter and the large dynamic condition of level 4 sea state. BRIEF DESCRIPTION OF THE DRAWINGS

[0204] The present invention is further described below in conjunction with the accompanying drawings and embodiments.

[0205] Figure 1 Coordinate system definition;

[0206] Figure 2 The block diagram of attitude calculation principle of dual-axis stabilized platform;

[0207] Figure 3 Rough alignment process based on inertial solidification coordinate system;

[0208] Figure 4 Principle block diagram of attitude solution Kalman filter;

[0209] Figure 5 Vehicle pitch and roll data;

[0210] Figure 6 Vehicle heading data;

[0211] Figure 7 Pitch and Roll data under vehicle maneuvering conditions;

[0212] Figure 8Heading data under vehicle maneuvering conditions;

[0213] Fig. 9 Sea test pitch and roll angle data;

[0214] Fig.10 The appearance of the dual-axis stabilized platform;

[0215] Fig.11 Inertial component hardware diagram. DETAILED DESCRIPTION

[0216] A dynamic attitude measurement method for a dual-axis stabilized platform, firstly, adopts an inertial coordinate system coarse alignment method to perform coarse alignment, obtains the initial value of the attitude angle, and then performs a series of navigation solutions of the attitude angle, attitude angular rate, velocity position, and during the navigation solution process, adopts Kalman filtering technology to perform combined navigation and combined calibration of a strapdown inertial navigation system, estimates the misalignment angle, velocity error, and position error, and uses the estimated misalignment angle, velocity error, and position error to correct the quaternion, solution velocity, and solution position of the strapdown inertial navigation system, so that the calculated navigation coordinate system and the real navigation coordinate system are as consistent as possible;

[0217] The workflow of the attitude solution algorithm module includes the following:

[0218] 1. Coarse alignment; 2. Attitude quaternion update; 3. Attitude angle calculation; 4. Attitude angular rate calculation; 5. Specific force coordinate transformation; 6. Speed ​​update; 7. Position update; 8. Kalman filter calculation; 9. Rotation control command calculation;

[0219] Step 1: Rough alignment

[0220] Rough alignment is to find the approximate azimuth cosine array It is the basis for Kalman precision alignment. The basic idea of ​​the scheme is to use the inertial coordinate system as the reference benchmark, initially fix the two inertial coordinate systems, then update the quaternion of the angular motion of the carrier relative to the inertial coordinate system, and use the transformation relationship between gravity and the linear motion of the carrier between the two inertial coordinate systems to obtain the transformation matrix of the two solidified inertial coordinate systems, and then infer

[0221] First, decompose the azimuth cosine matrix into

[0222]

[0223] The problem turns to solving and Here are the steps:

[0224] 1) Ask

[0225] Will Written

[0226]

[0227] in

[0228]

[0229] λ0 and L0 are the longitude and latitude at the start of alignment, λ t and L t To align the longitude and latitude at time t, substitute equations (89), (90), (91) and (92) into equation (88) to obtain

[0230]

[0231] 2) Ask

[0232] Same as the attitude update algorithm, the attitude quaternion is used to update the differential equation

[0233]

[0234] in is the carrier angular velocity of the gyro sensitive, and the alignment starts at time i b0 The a system coincides with the b system.

[0235] 3) Ask

[0236] In the inertial system

[0237]

[0238] Accelerometer output under dynamic base

[0239]

[0240] g b is the projection of gravity on the b system, is the linear motion acceleration of the carrier

[0241] definition have

[0242]

[0243] Under the condition of uniform motion or constant amplitude swing motion of the carrier, the approximate have

[0244]

[0245] in, From step 1), we can find g n =[00-g]T

[0246] In addition, according to the basic equation of velocity

[0247]

[0248] Written

[0249]

[0250] but

[0251]

[0252] Among them, v b is the external reference ground speed, and is defined Arrangement (98) has

[0253]

[0254] in,

[0255] Take any two time points in the alignment process Obtain

[0256]

[0257] The above and Substituting into equation (87) we can obtain the azimuth cosine matrix:

[0258] The largest source of error in alignment is the acceleration of the carrier motion during alignment. For the dual-axis stabilized platform Relative to g b For small quantities, even when moving, only the carrier needs to be not too maneuverable for a short period of time to complete high-precision alignment, and the dynamic applicability is strong;

[0259] Step 2: Attitude quaternion update

[0260] The transformation relationship from n-system to p-system is represented by the rotation quaternion Q, which is composed of a real unit and 3 imaginary units. The hypercomplex number composed of 4 real elements is as follows:

[0261]

[0262] Among them, q0, q1, q2, q3 are four real numbers. is an orthogonal basis of imaginary units;

[0263] If the vector in space is fixed, if the moving coordinate system rotates an angle relative to the reference system, where the moving coordinate system is the p system and the reference coordinate system is the n system, the transformation relationship between the vector described by the quaternion in the two coordinate systems is

[0264]

[0265] In the formula,

[0266]

[0267] Substituting equation (106) into equation (105), we can calculate using quaternion:

[0268]

[0269] And by

[0270]

[0271] Obtain the transformation matrix

[0272]

[0273] The above is obtained using quaternions Transformation matrix: if we know the attitude quaternion Q from n system to p system, we can get the corresponding transformation matrix;

[0274] Assume t k The rotation coordinate system at the moment is p(k), the navigation coordinate system is n(k), t k+1 The carrier coordinate system at the moment is p(k+1), the navigation coordinate system is n(k+1), the rotation quaternion from p(k) to p(k+1) is q(h), and the rotation quaternion from n(k) to b(k) is Q(t k ), the rotation quaternion from n(k+1) to b(k+1) is Q(t k+1 ), the rotation quaternion from n(k) to n(k+1) is p(h), where h = t k+1 -t k is the update cycle, then

[0275]

[0276] According to formula (105),

[0277]

[0278] Using the equivalent relationship between equation (105) and equation (108), equation (110) is equivalent to

[0279]

[0280] Comparing equation (111) and equation (112), we get

[0281]

[0282] in

[0283]

[0284] Φ is the equivalent rotation vector from b(k) to b(k+1), and its numerical solution is obtained by the following equivalent rotation vector differential equation

[0285]

[0286] Where Φ = Φ(h) is a posture update period (t k ,t k+1 ] is the equivalent rotation vector from b(k) to b(k+1), ω is the carrier angular velocity vector, and the last two terms reflect the compensation of the non-commutative error of rotation;

[0287] Equation (115) is solved using the Taylor series expansion method. In one attitude update cycle (t k ,t k+1 ], a polynomial fitting is performed on the carrier angular velocity, a multi-sample sampling is performed on the gyro signal with a sampling period shorter than the update period, the angular increment information of each sampling point is used to solve the coefficient of the fitting polynomial, and the fitted attitude angular velocity is substituted back into the Taylor series expansion of Φ(h) to obtain the numerical solution of the rotation vector;

[0288] If the carrier performs roll and pitch vibrations at the same frequency but different phases, a conic error will be generated on the azimuth axis. When the vibration satisfies a certain phase and frequency relationship, the vibration of any two axes will cause drift on the third axis. The conic error is compensated by the rotating vector four-sample optimization algorithm:

[0289]

[0290] Among them, Δθ1, Δθ2, Δθ3 and Δθ4 are respectively (t k ,t k +h / 4]、(t k +h / 4,t k +h / 2]、(t k +h / 2,t k +3h / 4]、(t k +3h / 4,t k+1 ] Gyro output angle increment within the sampling period;

[0291] In addition, the angle that the n system rotates relative to the i system during the update cycle is

[0292]

[0293] is the projection of the rotation angular velocity of the e system relative to the i system in the n system, is the projection of the angular velocity of the n system relative to the e system on the n system, which is calculated by the following two formulas:

[0294]

[0295] The speed, latitude and curvature radius in the formula are all the results of the previous navigation solution update cycle,

[0296]

[0297] In one attitude update cycle, the rotation of the navigation coordinate system changes extremely slowly. is a minimum value and is not suitable as a dividend in a computer. Taylor expansion is performed on equation (120) to obtain a fourth-order approximation:

[0298]

[0299] Substitute equation (114) and equation (121) into equation (113) and perform normalization

[0300] Q(t k+1 )=Q(t k+1 ) / ||Q(t k+1 )|| Formula (122)

[0301] Step 3: Attitude Angle Calculation

[0302] Substituting the above result into equation (109), the attitude transformation matrix of the inertial measurement unit is calculated: The inertial measurement unit of the single-axis rotating laser inertial navigation system rotates around the rotation axis Oz p1 Rotation, assuming the rotation angle position is γ, the attitude transformation matrix from p1 system to b0 system is

[0303]

[0304] Attitude transformation matrix from n system to p1 system for

[0305]

[0306] After the rotation angle transformation, the attitude transformation matrix from the n system to the b0 system is for

[0307]

[0308] The general expression of the attitude transformation matrix of the inertial navigation system is:

[0309]

[0310] Calculate the attitude transformation matrix of the ship carrier

[0311]

[0312] in, It is the attitude zero array obtained after the equipment is calibrated with the true level and true heading on the ship;

[0313] Finally, the horizontal attitude angle and heading angle of the carrier are extracted in real time by the elements of formula (127):

[0314]

[0315]

[0316] The initial value of the quaternion is calculated from the initial attitude angle

[0317]

[0318] Among them, the initial values ​​of the attitude angles H0, P0, and R0 are determined by rough alignment;

[0319] Step 4: Calculation of attitude angular rate

[0320] According to the sampling period h (h = t k -t k-1 ) Take N samples of the smoothed quantity and let

[0321]

[0322] Re-order

[0323] A=(B T B) -1 B T Formula (135)

[0324] definition

[0325] A(2,:)=[A(2,1)A(2,2)...A(2,N)] Formula (136)

[0326] A(2,:) represents the second row of elements in matrix A, and the number of elements is N;

[0327] Current t k Attitude angular rate at time and The calculation is as follows

[0328]

[0329] Where R(t k+i-N )、P(t k+i-N ) and H(tk+i-N ) is t k+i-N Roll angle, pitch angle and heading angle at the moment;

[0330] Step 5: Comparative coordinate transformation

[0331] Accelerometer output specific force f p Through coordinate transformation

[0332]

[0333] The accelerometer is installed on the rotating axis of the rotating mechanism, ignoring the inner lever arm effect, that is,

[0334] Step 6: Speed ​​Update

[0335] Basic equations of inertial navigation

[0336]

[0337] in, is the projection of the geographic coordinate system relative to the earth coordinate system in the geographic coordinate system, called ground speed. The formula (139) can be written in the form of components:

[0338]

[0339] Right now

[0340]

[0341] Among them,

[0342]

[0343] δA is called harmful acceleration;

[0344] but

[0345]

[0346] In a speed update cycle (t k ,t k+1 ]Inside, there is

[0347]

[0348] The accelerometer output pulse is the velocity increment, and the integral term in equation (144) is calculated by the following equation:

[0349]

[0350] Among them, Recorded as Δv pis the velocity increment after the accelerometer output pulse number is scaled, and h is the update period;

[0351] The first term on the right side of formula (145) is the speed compensation caused by specific force;

[0352]

[0353] In the case of dynamic carrier, equation (146) will produce constant errors related to the carrier's linear motion and angular motion. If the carrier makes angular motion in the linear motion direction at the same time, a rotation effect will be generated. If the carrier makes linear vibration along the longitudinal axis and angular vibration along the transverse axis with the same frequency and phase, a DC quantity will be generated in the azimuth axis direction of the carrier, that is, a constant velocity increment error is introduced. The above motion mode is similar to rowing motion: on the one hand, the paddle shakes periodically around the lateral axis of the boat, and on the other hand, the boat moves intermittently along the longitudinal axis, so it is called the rowing effect. The compensation method for the rotation effect and the rowing effect is to compensate for Δv p Multi-sub-sampling is performed, and the motion trajectory within the update period is fitted using the sample point information. The optimization goal is to minimize the error caused by the algorithm in the extreme case of paddling motion, thereby determining the coefficients of the fitting polynomial.

[0354] The rotation effect compensation term is a primary compensation term. The optimized four-sample algorithm is used for the paddling effect compensation term. The calculation formula is as follows:

[0355] Δv p =Δv+Δv rot +Δv scul Formula (147)

[0356] Among them, the rotation effect compensation

[0357]

[0358] Optimized four-sample paddling effect compensation

[0359]

[0360] Δv=Δv1+Δv2+Δv3+Δv4 Formula (150)

[0361] Δθ=Δθ1+Δθ2+Δθ3+Δθ4 Formula (151)

[0362] Δv1, Δv2, Δv3, and Δv4 are (t k ,t k +h / 4]、(t k +h / 4,t k +h / 2]、(t k +h / 2,t k +3h / 4]、(tk +3h / 4,t k+1 ] The velocity increment of the accelerometer output within the sampling period;

[0363] Step 7: Location Update

[0364] In the pure inertial working mode, the altitude channel of the inertial navigation system is divergent. For surface applications, a high-pass filter is used to obtain the instantaneous vertical velocity and instantaneous displacement;

[0365] The differential equation for longitude and latitude is as follows

[0366]

[0367] In a position update period (t k ,t k+1 ]Inside, there is

[0368]

[0369] Step 8: Kalman filter calculation

[0370] The algorithm is as follows:

[0371] a) Equation of state

[0372] Considering both gyroscope drift and accelerometer bias as random constant processes, we have

[0373] The state variable is selected as 15 dimensions

[0374]

[0375] The state equation of a continuous system is:

[0376]

[0377] From the inertial system error propagation equation, where:

[0378]

[0379]

[0380]

[0381] Measuring noise for gyro drift and accelerometer bias;

[0382] b) Measurement equation

[0383] The measurement equation with position as the observation quantity is

[0384]

[0385] Where, subscript s represents the output of strapdown inertial navigation system solution, subscript r represents the reference output, and H p =[0 3×6 I 3× 30 3×6 ], V is the 3D position observation white noise;

[0386] c) Revision

[0387] Kalman filtering estimates the misalignment angle and corrects the attitude array in real time. Then a more accurate attitude matrix is ​​obtained;

[0388] The misalignment angle φ is estimated using Kalman filtering technology E ,φ N ,φ U , the correction angle is:

[0389]

[0390] The quaternion method is used to correct it:

[0391]

[0392] in:

[0393] The correction of speed and position is replaced by the output value of speed and position filter;

[0394] d) Discretization of continuous systems

[0395] The above continuous system error model is established by a) state equation and b) measurement equation. Discretization is required to realize it in the program. Discretization is essentially to calculate the transfer matrix of the discrete system based on the system matrix of the continuous system, and to calculate the noise variance matrix of the discrete system based on the system process variance intensity matrix of the continuous system.

[0396] The state equation model of the continuous system is as follows:

[0397]

[0398] Where X is the n-dimensional state vector of the system, F is the n×n-dimensional system matrix, W is the p-dimensional system process noise, G is the n×p-dimensional noise input matrix, Z is the m-dimensional observation vector of the system, V is the m-dimensional observation noise, and H is the m×n-dimensional observation matrix;

[0399] 1)Φ k,k-1 Calculation

[0400] Using the calculation method of the steady-state system, the state transition matrix Φ k,k-1 The relationship with the system matrix F is as follows:

[0401]

[0402] Among them, (t k-1 ,t k ] is the prediction period, and h = t k -t k-1 , the prediction period h is generally short, and Taylor expansion of equation (171) yields

[0403]

[0404] 2) Calculation

[0405] The covariance matrix of the system noise vector W(t) of the continuous system is Q(t), and the variance matrix of the input noise is:

[0406] Q q =G(t)Q(t)G T (t) Formula (173)

[0407] Input noise variance of Kalman filter The input noise variance Q of the continuous system q There are the following relationships:

[0408]

[0409] Step 9: Calculation of rotation control instructions

[0410] The goal of the stable platform is to isolate the horizontal angular motion of the carrier. For control, the constraint condition is to make

[0411] ∫(R+α)dt=0, ∫(P+β)dt=0, where R is the roll angle, P is the pitch angle, α is the roll angle command, and β is the pitch angle command, that is, the rotation must have the function of tracking the change of the horizontal attitude angle;

[0412] The output command angle is transferred to the control algorithm for calculation and converted into a control quantity or control signal.

[0413] Figure 2 It is a principle block diagram of the attitude solution of the dual-axis stabilized platform, covering the entire process of steps 1 to 9. The figure draws the main data flow and the data processing done in each stage. This principle block diagram combined with the algorithm description in the description has the possibility of realizing this method.

[0414] Figure 3 It is a rough alignment process based on the inertial solidification coordinate system, which is a well-known technology for professionals in inertial navigation.

[0415] Figure 4It is a block diagram of the Kalman filter principle for attitude solution. The Kalman filter algorithm used in this process is well known to professional and technical personnel, but its configuration parameters are unique to the present invention;

[0416] Figure 5-9 It is the land vehicle test data, from Figure 5 From the data, we can see that the RMS value of the pitch error under static conditions is 0.0002° (0.72″), and the RMS value of the roll error is 0.002° (7.2″). Figure 7 From the data, we can see that under dynamic conditions, the RMS value of the pitch error is 0.0026° (9.36″), and the RMS value of the roll error is 0.0025° (9″). Fig. 9 and Fig.11 The heading change state can be seen to have obvious differences between static and dynamic.

[0417] Fig. 9 The data curve shows that during the nearly 5 days of continuous working time at sea, the maximum value of the data did not exceed 0.01° (3.6″). The straight part of the curve in the figure is the period when the ship is anchored, and the ups and downs of the curve are the period when the ship is maneuvering. It can be seen that the dual-axis stabilized platform using this control method greatly improves the stability in a large dynamic environment, and effectively ensures the stable operation of the gravimeter in various sports environments.

[0418] Fig.10 This is the appearance structure diagram of the dual-axis stabilized platform, in which the algorithm proposed in this technical solution is implemented in the control box. The code-named components in the figure are: 1. Cold atomic gravimeter; 2. Base; 3. Outer frame; 4. Inertial component; 5. Control box; 6. Display; 7. Locker. The middle frame is not marked in the figure due to view obstruction.

[0419] The algorithm used in this technical solution runs on the acquisition board in the inertial component. The hardware structure of the inertial component is as follows: Fig.11 shown.

[0420] The inertial measurement unit mainly includes three parts: inertial sensor, electronic circuit unit (EU), platform assembly and chassis.

[0421] The inertial sensitive devices include three fiber optic gyroscopes and three quartz accelerometers.

[0422] The electronic circuit unit includes an accelerometer signal conversion board, an acquisition and solution board, and a DC power supply module.

[0423] The accelerometer signal conversion board converts the current signals of the three accelerometers into digital pulse signals, measures the temperature measurement signals output by the accelerometer temperature measurement device, and outputs these signals to the acquisition and solution board;

[0424] The acquisition and solution board generates a 1ms external trigger signal to the fiber optic gyro and accelerometer circuit board, collects gyro pulse signals, temperature signals, accelerometer pulse signals, and temperature signals through the 422 serial port, performs temperature compensation and scale conversion to obtain the carrier angular velocity and specific force, and performs Kalman filter calculation and strapdown algorithm navigation solution; the acquisition and solution board also receives external satellite navigation reference information, outputs the navigation solution results to the user through Ethernet / CAN bus / RS-422 asynchronous serial port / optical fiber interface, and interacts with the user through the display and control device;

[0425] The DCDC DC power supply module converts the +24V DC power converted by the power supply device into various DC power supplies required for the gyroscope, accelerometer, accelerometer loop board, acquisition and solution board, and interface storage board.

[0426] The inertial component is installed at the bottom of the cold atom gravimeter, and is installed together with the cold atom gravimeter on the stable frame in the middle of the dual-axis stable platform, and moves synchronously with the cold atom gravimeter. When the whole set of equipment moves on a moving carrier such as a vehicle or ship, it is necessary to ensure that the cold atom gravimeter remains in a horizontal state, that is, the pitch angle and roll angle of the platform working plane and the horizontal plane meet the requirement of ≤10". The inertial component is an important component for measuring these two key parameters, and the algorithm proposed in this technical solution is an important method for calculating these two parameters.

Claims

1. A method for measuring dynamic posture of a dual-axis stabilized platform, characterized in that: Firstly, the rough alignment method of the inertial coordinate system is used for rough alignment. After the initial value of the attitude angle is obtained, a series of navigation solutions of the attitude angle, attitude angular rate, velocity and position are performed. In the process of navigation solution, the Kalman filter technology is used for combined navigation and combined calibration of the strapdown inertial navigation system. The misalignment angle, velocity error and position error are estimated, and the quaternion, solution velocity and solution position of the strapdown inertial navigation system are corrected with it. The workflow of the attitude solution algorithm module includes the following:

1. Coarse alignment; 2. Attitude quaternion update; 3. Attitude angle calculation; 4. Attitude angular rate calculation; 5. Specific force coordinate transformation; 6. Velocity update; 7. Position update; 8. Kalman filter calculation; 9. Rotation control command calculation; Step 1: Rough alignment Rough alignment is to find the approximate azimuth cosine array It is the basis for Kalman precision alignment. The inertial coordinate system is used as the reference datum. The two inertial coordinate systems are initially fixed. Then the quaternion of the angular motion of the carrier relative to the inertial coordinate system is updated. The transformation relationship between gravity and the linear motion of the carrier between the two inertial coordinate systems is used to obtain the transformation matrix of the two solidified inertial coordinate systems, and then the transformation matrix of the two solidified inertial coordinate systems is inferred. First, decompose the azimuth cosine matrix into The problem turns to solving and Here are the steps: 1) Ask Will Written in λ0 and L0 are the longitude and latitude at the start of alignment, λ t and L t To align the longitude and latitude at time t, substitute equations (3), (4), (5) and (6) into equation (2) to obtain 2) Ask Same as the attitude update algorithm, the attitude quaternion is used to update the differential equation in is the carrier angular velocity of the gyro sensitive, and the alignment starts at time i b0 The a system coincides with the b system. 3) Ask In the inertial system Accelerometer output under dynamic base g b is the projection of gravity on the b system, is the linear motion acceleration of the carrier definition have Under the condition of uniform motion or constant amplitude swing motion of the carrier, the approximate have in, From step 1), we can find g n =[00-g] T In addition, according to the basic equation of velocity Written but Among them, v b is the external reference ground speed, and is defined The finishing formula (12) is in, Take any two time points in the alignment process Obtain Will and Substituting into equation (1) we can obtain the azimuth cosine matrix: Step 2: Attitude quaternion update The transformation relationship from n-system to p-system is represented by the rotation quaternion Q, which is composed of a real unit and 3 imaginary units. The hypercomplex number composed of 4 real elements is as follows: Among them, q0, q1, q2, q3 are four real numbers. is an orthogonal basis of imaginary units; If the vector in space is fixed, if the moving coordinate system rotates an angle relative to the reference system, where the moving coordinate system is the p system and the reference coordinate system is the n system, the transformation relationship between the vector described by the quaternion in the two coordinate systems is In the formula, Substituting equation (20) into equation (19), we can calculate using quaternion: And by Obtain the transformation matrix Assume t k The rotation coordinate system at the moment is p(k), the navigation coordinate system is n(k), t k+1 The carrier coordinate system at the moment is p(k+1), the navigation coordinate system is n(k+1), the rotation quaternion from p(k) to p(k+1) is q(h), and the rotation quaternion from n(k) to b(k) is Q(t k ), the rotation quaternion from n(k+1) to b(k+1) is Q(t k+1 ), the rotation quaternion from n(k) to n(k+1) is p(h), where h = t k+1 -t k is the update cycle, then According to formula (19), we have Using the equivalent relationship between equation (19) and equation (22), equation (24) is equivalent to Comparing equation (25) and equation (26), we get in Φ is the equivalent rotation vector from b(k) to b(k+1), and its numerical solution is obtained by the following equivalent rotation vector differential equation Where Φ = Φ(h) is a posture update period (t k ,t k+1 ] is the equivalent rotation vector from b(k) to b(k+1), ω is the carrier angular velocity vector, and the last two terms reflect the compensation of the non-commutative error of rotation; Formula (29) is solved by Taylor series expansion method. In a posture update cycle (t k ,t k+1 ], a polynomial fitting is performed on the carrier angular velocity, a multi-sample sampling is performed on the gyro signal with a sampling period shorter than the update period, the angular increment information of each sampling point is used to solve the coefficient of the fitting polynomial, and the fitted attitude angular velocity is substituted back into the Taylor series expansion of Φ(h) to obtain the numerical solution of the rotation vector; If the carrier performs roll and pitch vibrations at the same frequency but different phases, a conic error will be generated on the azimuth axis. When the vibration satisfies a certain phase and frequency relationship, the vibration of any two axes will cause drift on the third axis. The conic error is compensated by the rotating vector four-sample optimization algorithm: Among them, Δθ1, Δθ2, Δθ3 and Δθ4 are respectively (t k ,t k +h / 4]、(t k +h / 4,t k +h / 2]、(t k +h / 2,t k +3h / 4]、(t k +3h / 4,t k+1 ] Gyro output angle increment within the sampling period; In addition, the angle that the n system rotates relative to the i system during the update cycle is is the projection of the rotation angular velocity of the e system relative to the i system in the n system, is the projection of the angular velocity of the n system relative to the e system on the n system, which is calculated by the following two formulas: The speed, latitude and curvature radius in the formula are all the results of the previous navigation solution update cycle, In one attitude update cycle, the rotation of the navigation coordinate system changes extremely slowly. is a minimum value and is not suitable as a dividend in a computer. Taylor expansion is performed on equation (34) to obtain a fourth-order approximation: Substitute equation (28) and equation (35) into equation (27) and perform normalization Q(t k+1 ) = Q(t k+1 ) / ||Q(t k+1 )|| Equation (36) Step 3: Attitude Angle Calculation Substitute the result of formula (36) into formula (23) to calculate the attitude transformation matrix of the inertial measurement unit: The inertial measurement unit of the single-axis rotating laser inertial navigation system rotates around the rotation axis Oz p1 Rotation, assuming the rotation angle position is γ, the attitude transformation matrix from p1 system to b0 system is Attitude transformation matrix from n system to p1 system for After the rotation angle transformation, the attitude transformation matrix from the n system to the b0 system is for The general expression of the attitude transformation matrix of the inertial navigation system is: Calculate the attitude transformation matrix of the ship carrier in, It is the attitude zero array obtained after the equipment is calibrated with the true level and true heading on the ship; Finally, the horizontal attitude angle and heading angle of the carrier are extracted in real time by the elements of formula (41): The initial value of the quaternion is calculated from the initial attitude angle Among them, the initial values ​​of the attitude angle H0, P0, and R0 are determined by rough alignment; Step 4: Calculation of attitude angular rate According to the sampling period h (h = t k -t k-1 ) Take N samples of the smoothed quantity and let Re-order A=(B T B) -1 B T Formula (47) definition A(2,:)=[A(2,1)A(2,2)...A(2,N)] Formula (48) A(2,:) represents the second row of elements in matrix A, which has N elements. k Attitude angular rate at time and The calculation is as follows Where R(t k+i-N )、P(t k+i-N ) and H(t k+i-N ) is t k+i-N Roll angle, pitch angle and heading angle at the moment; Step 5: Comparative coordinate transformation Accelerometer output specific force f p Through coordinate transformation The accelerometer is installed on the rotating axis of the rotating mechanism, ignoring the inner lever arm effect, that is, Step 6: Speed ​​Update Basic equations of inertial navigation in, is the projection of the geographic coordinate system relative to the earth coordinate system in the geographic coordinate system, called ground speed. Formula (51) can be written in the form of components: Right now Among them, δA is called harmful acceleration; but In a speed update cycle (t k ,t k+1 ]Inside, there is The accelerometer output pulse is the velocity increment, and the integral term in equation (56) is calculated by equation (57): Among them, Recorded as Δv p is the velocity increment after the accelerometer output pulse number is scaled, and h is the update period; The first term on the right side of formula (57) is the speed compensation caused by specific force; The rotation effect compensation term is a primary compensation term, and the paddling effect compensation term adopts the optimized four-sample algorithm Δv p =Δv+Δv rot +Δv scul expression(59) Among them, the rotation effect compensation Optimized four-sample paddling effect compensation Δv=Δv1+Δv2+Δv3+Δv4 Formula (62) Δθ=Δθ1+Δθ2+Δθ3+Δθ4 Equation (63) Δv1, Δv2, Δv3, and Δv4 are (t k ,t k +h / 4]、(t k +h / 4,t k +h / 2]、(t k +h / 2,t k +3h / 4]、(t k +3h / 4,t k+1 ] The velocity increment of the accelerometer output within the sampling period; Step 7: Location Update In the pure inertial working mode, the altitude channel of the inertial navigation system is divergent. For surface applications, a high-pass filter is used to obtain the instantaneous vertical velocity and instantaneous displacement; The differential equation for longitude and latitude is as follows In a position update period (t k ,t k+1 ]Inside, there is Step 8: Kalman filter calculation The algorithm is as follows: a) Equation of state Considering both gyroscope drift and accelerometer bias as random constant processes, we have The state variable is selected as 15 dimensions The state equation of a continuous system is: From the inertial system error propagation equation, where: Measuring noise for gyro drift and accelerometer bias; b) Measurement equation The measurement equation with position as the observation quantity is Subscript s represents the output of strapdown inertial navigation system solution, subscript r represents the reference output, H p =[0 3×6 I 3×3 0 3×6 ], V is the 3D position observation white noise; c) Revision Kalman filtering estimates the misalignment angle and corrects the attitude array in real time. Then a more accurate attitude matrix is ​​obtained; The misalignment angle φ is estimated using Kalman filtering technology E ,φ N ,φ U , the correction angle is: The quaternion method is used to correct it: in: The correction of speed and position is replaced by the output value of speed and position filter; d) Discretization of continuous systems The continuous system error model is established by a) state equation and b) measurement equation. Discretization is required to realize it in the program. Discretization is essentially to calculate the transfer matrix of the discrete system based on the system matrix of the continuous system, and to calculate the noise variance matrix of the discrete system based on the system process variance intensity matrix of the continuous system. The state equation model of the continuous system is as follows: X is the n-dimensional state vector of the system, F is the n×n-dimensional system matrix, W is the p-dimensional system process noise, G is the n×p-dimensional noise input matrix, Z is the m-dimensional observation vector of the system, V is the m-dimensional observation noise, and H is the m×n-dimensional observation matrix; 1)Φ k,k-1 Calculation Using the calculation method of the steady-state system, the state transition matrix Φ k,k-1 The relationship with the system matrix F is as follows: Among them, (t k-1 ,t k ] is the prediction period, and h = t k -t k-1 , the prediction period h is generally short, and Taylor expansion of equation (83) yields 2) Calculation The covariance matrix of the system noise vector W(t) of the continuous system is Q(t), and the variance matrix of the input noise is: Q q = G(t)Q(t)G T (t) Equation (85) Input noise variance of Kalman filter The input noise variance Q of the continuous system q There are the following relationships: Step 9: Calculation of rotation control instructions The goal of stabilizing the platform is to isolate the horizontal angular motion of the carrier. For control, the constraints are to make ∫(R+α)dt=0, ∫(P+β)dt=0, where R is the roll angle, P is the pitch angle, α is the roll angle command, and β is the pitch angle command, that is, the rotation must have the function of tracking the change of the horizontal attitude angle; The output command angle is transferred to the control algorithm for calculation and converted into a control quantity or control signal.

Images