Calibration and compensation method for gain change value of servo loop of three-axis stabilized platform

Abstract

The invention discloses a three-axis stabilized platform servo loop gain change value calibration and compensation method. Firstly, a table body shaft, an inner frame shaft and an outer frame shaft are arranged at 0-degree, 0-degree and 0-degree positions, a servo loop is closed, an external test device inputs sine waves, an output signal of the three-axis stabilized platform is collected, amplitudes of the input signal and the output signal at different frequencies are calculated, and an open-loop amplitude-frequency characteristic curve of the current position of the three-axis stabilized platform is obtained. Secondly, rotating the inner frame shaft by 10 degrees every time, and calculating gain change values of the servo loop of the inner frame shaft at different angles; and finally, fitting a function between the angle of the inner frame shaft and a gain change value through the calibration data, and compensating the function into the servo loop gain of the outer frame shaft. The method for calibrating and compensating the gain change value of the servo loop of the three-axis stabilized platform under the condition that the rotational inertia is uncertain is given for the first time, and the stability of the servo loop when a platform frame system is located at any position of an inner frame is guaranteed.

Description

Technical Field

[0001] The invention relates to a servo loop gain change value calibration and compensation method for a three-axis stable platform, belonging to the field of inertial navigation. Background Art

[0002] As the demand for maneuverable and variable-orbit flight increases, the inertial navigation system is required to achieve precise navigation and guidance under large maneuvers and high dynamic conditions. When the vehicle is to perform large maneuverable and variable-orbit flight, the inner frame axis of the three-axis stabilized platform will have a large attitude angle. Under such conditions, the platform servo loop is required to maintain sufficient stability margin and reliability.

[0003] Since the three-axis stabilized platform adopts a frame structure, when the position of the frame structure changes, it will cause the change of the moment of inertia of the frame structure, which will cause the gain of the platform servo loop to change, and then affect the amplitude stability margin of the servo loop. However, the test value of the moment of inertia has limitations. For a single-axis frame, both the torsion method and the single-line method can only test the moment of inertia in the direction of the rotation axis, and cannot test the moment of inertia in the direction without the rotation axis. The simulation value is also not accurate. When the mass distribution on the platform frame structure is uneven or the frame itself is irregular, the simulation value has a large deviation.

[0004] At present, in order to reduce the gain change caused by the change of moment of inertia, the three-axis stabilized platform mainly works within the range of ±45° of the inner frame, and the change of moment of inertia caused by the small angle of the inner frame is within the controllable range. The compensation based on the mechanical model depends on the accuracy of the model and the accuracy of the moment of inertia, and there is a problem of inaccurate compensation. Therefore, when the vehicle has a large maneuvering posture, the existing measures will cause the stability margin of the servo loop to change due to the change of moment of inertia, and there is a problem of unstable open-loop frequency characteristics when the inner frame works at different angles. Summary of the invention

[0005] The technical problem solved by the present invention is: to overcome the shortcomings of the prior art and provide a method for calibrating and compensating the gain change value of the servo loop of a three-axis stable platform. When the inner frame axis works at a large angle, the method can effectively improve the stability and reliability of the carrier under large maneuvering conditions.

[0006] The technical solution of the present invention is:

[0007] A method for calibrating and compensating a gain change value of a servo loop of a three-axis stable platform, the steps are as follows:

[0008] (1) Place the three-axis stabilized platform body axis, inner frame axis, and outer frame axis at 0°, 0°, and 0° respectively;

[0009] (2) The servo loop is closed, and the external test equipment inputs a sine wave to the three-axis stabilized platform, and the output signal of the three-axis stabilized platform is collected at the same time. The frequency response of the sine wave is identified by the least square method, and the amplitudes of the input signal and the output signal at different frequencies are calculated to obtain the open-loop amplitude-frequency characteristic curve of the three-axis stabilized platform at the current position;

[0010] (3) Each time the inner frame axis increases by 10°, the other two axes are 0°, and the above step (2) is repeated until the inner frame axis rotates to 70°;

[0011] (4) The gain change value of the servo loop of the inner frame axis at different angles is calculated through the open-loop amplitude-frequency characteristic curve of the inner frame axis at 0° to 70°.

[0012] (5) A second-order polynomial data fitting method is used to obtain a fitting function between the inner frame axis angle and the gain change value, and this function is compensated into the servo loop.

[0013] Preferably, in step (2), the open-loop amplitude-frequency characteristic curve is obtained by the following method:

[0014] The external test equipment generates a sinusoidal input signal Uin, which is output to the servo loop through the digital serial port. The servo loop adds the sinusoidal input signal Uin and the control output signal Uctl to obtain an output signal Uout=Uctl+Uin, which is sent to the external test equipment through the digital serial port. The external test equipment uses the least squares identification method to calculate the open-loop amplitude-frequency characteristic curve of Uout / Uin.

[0015] Preferably, in step (4), the gain change value of the servo loop at different angles of the inner frame axis is obtained by the following method:

[0016] When the position of the inner frame axis of the three-axis stabilized platform changes, causing the gain value of the servo loop to change, the open-loop cutoff frequency will move linearly; taking the open-loop amplitude-frequency characteristic curve when the inner frame axis is at 0° as the reference, the cutoff frequency at this time is ω c , when the inner frame axis is 0°, the frequency is ω c When the amplitude is 0dB; when the inner frame axis rotates to 10°, the frequency is ω c When the inner frame rotates to 20°, the corresponding amplitude is h1dB; when the inner frame rotates to 20°, the frequency is ω c The corresponding amplitude is h2 dB; the following data are obtained by testing in sequence:

[0017] Inner frame axis angle 0° 10° 20° 30° 40° 50° 60° 70° <![CDATA[ω c Amplitude at time]]> <![CDATA[h0]]> <![CDATA[h1]]> <![CDATA[h2]]> <![CDATA[h3]]> <![CDATA[h4]]> <![CDATA[h5]]> <![CDATA[h6]]> <![CDATA[h7]]>

[0018] The gain compensation value K is calculated by the following formula i :

[0019] Ki =10 (-hi / 20)

[0020] Right now:

[0021] Inner frame axis angle 0° 10° 20° 30° 40° 50° 60° 70° Gain compensation value <![CDATA[K0]]> <![CDATA[K1]]> <![CDATA[K2]]> <![CDATA[K3]]> <![CDATA[K4]]> <![CDATA[K5]]> <![CDATA[K6]]> <![CDATA[K7]]> .

[0022] Preferably, in step (5), the fitting function between the inner frame axis angle and the gain change value is obtained by the following method:

[0023] Inner frame axis angle β yk As the independent variable, the gain change value K i As the dependent variable, a second-order polynomial fitting function K is used i =p0×β yk 2 (i)+p1×β yk (i)+p2, K i and the corresponding β yk (i) Substitute into the fitting function, calculate the values ​​of p0, p1, and p2, and finally obtain the fitting function between the inner frame axis angle and the gain change value.

[0024] Preferably, the inner frame axis angle β yk is the angle of rotation of the outer frame around the inner frame axis, and the outer frame axis angle β xk is the angle of rotation of the base around the outer frame axis, and the table axis angle β zk It is the rotation angle of the inner frame around the table axis.

[0025] Preferably, the inner frame axis angle β yk , outer frame axis angle β xk 、Table body axis angle β zk The measurement method is as follows:

[0026] An angle sensor is installed on the outer frame axis to measure the angle β of the base rotating around the outer frame axis. xk ; An angle sensor is installed on the inner frame axis to measure the angle β of the outer frame rotating around the inner frame axis yk ; Install the sensor on the table axis to measure the angle β of the inner frame rotating around the table axis zk .

[0027] The advantages of the present invention compared with the prior art are as follows:

[0028] (1) The present invention provides a method for calibrating and compensating the gain change value of the servo loop of a three-axis stabilized platform, which can compensate for the gain change of the inner frame axis at any angle, especially when the inner frame is at a large angle, thereby increasing the stability and reliability of the large maneuvering flight of the carrier.

[0029] (2) The present invention provides a method for calibrating and compensating the gain change value of a three-axis stable platform servo loop. Compared with other gain compensation methods, it can more accurately compensate for the gain change value caused by an unknown mechanism. BRIEF DESCRIPTION OF THE DRAWINGS

[0030] Figure 1 A flow chart of a method for calibrating and compensating a gain change value of a servo loop of a three-axis stable platform of the present invention;

[0031] Figure 2 A schematic diagram of a curve showing a change in the inner frame angle and the outer frame servo loop gain obtained by the test calibration of the present invention;

[0032] Figure 3 It is the amplitude-frequency characteristic curve of the uncompensated servo loop;

[0033] Figure 4 This is the amplitude-frequency characteristic curve of the servo loop after calibration and compensation of the present invention. DETAILED DESCRIPTION

[0034] The present invention is further described in detail below with reference to the accompanying drawings and specific embodiments:

[0035] The method for calibrating and compensating the gain change value of the servo loop of a three-axis stable platform under the condition of uncertain moment of inertia of the present invention calculates the gain change value of the servo loop of the inner frame axis at different angles by calibrating the amplitude-frequency characteristic of the platform servo loop. Through the calibration data of the test, a mathematical function between the inner frame axis angle and the gain change value of the outer frame servo loop is fitted, and this function is compensated to the gain of the outer frame servo loop to compensate for the servo loop gain value under uncertain moment of inertia, so as to improve the stability and reliability of the platform servo loop.

[0036] The experimental process flow chart of the method of the present invention is as follows Figure 1 As shown, the specific steps are as follows:

[0037] 1. Place the platform axis, inner frame axis and outer frame axis of the three-axis stabilized platform at 0°, 0°, 0°.

[0038] 2. Close the servo loop, the external test equipment inputs a sine wave to the three-axis stabilized platform, and at the same time collects the output signal of the three-axis stabilized platform, and uses the least squares method to identify the frequency response of the sine wave, calculate the amplitude of the input signal and the output signal at different frequencies, and obtain the open-loop amplitude-frequency characteristic curve of the three-axis stabilized platform at the current position.

[0039] The external test equipment generates a sinusoidal input signal Uin, which is output to the servo loop through the digital serial port. The servo loop adds the sinusoidal input signal Uin and the control output signal Uctl to obtain an output signal Uout=Uctl+Uin, which is sent to the external test equipment through the digital serial port. The external test equipment uses the least squares identification method to calculate the open-loop amplitude-frequency characteristic curve of Uout / Uin.

[0040] 3. Each time the inner frame axis increases by 10°, and the other two axes are 0°, repeat the above step 2 until the inner frame axis rotates to 70°.

[0041] Inner frame axis angle β yk , outer frame axis angle β xk 、Table body axis angle β zk The measurement method is as follows:

[0042] An angle sensor is installed on the outer frame axis to measure the angle β of the base rotating around the outer frame axis. xk ; An angle sensor is installed on the inner frame axis to measure the angle β of the outer frame rotating around the inner frame axis yk ; Install the sensor on the table axis to measure the angle β of the inner frame rotating around the table axis zk .

[0043] 4. The gain change value of the servo loop of the inner frame axis at different angles is calculated through the open-loop amplitude-frequency characteristic curve of the inner frame axis at 0° to 70°.

[0044] When the position of the inner frame axis of the three-axis stabilized platform changes, causing the gain value of the servo loop to change, the open-loop cutoff frequency will move linearly; taking the open-loop amplitude-frequency characteristic curve when the inner frame axis is at 0° as the reference, the cutoff frequency at this time is ω c , when the inner frame axis is 0°, the frequency is ω c When the amplitude is 0dB; when the inner frame axis rotates to 10°, the frequency is ω c When the inner frame rotates to 20°, the corresponding amplitude is h1dB; when the inner frame rotates to 20°, the frequency is ω c The corresponding amplitude is h2 dB; the following data are obtained by testing in sequence:

[0045] Inner frame axis angle 0° 10° 20° 30° 40° 50° 60° 70° <![CDATA[ω c Amplitude at time]]> <![CDATA[h0]]> <![CDATA[h1]]> <![CDATA[h2]]> <![CDATA[h3]]> <![CDATA[h4]]> <![CDATA[h5]]> <![CDATA[h6]]> <![CDATA[h7]]>

[0046] The gain compensation value K is calculated by the following formula i :

[0047] K i =10 (-hi / 20)

[0048] Right now:

[0049]

[0050] 5. Use the second-order polynomial data fitting method to obtain the fitting function between the inner frame shaft angle and the outer frame servo loop gain change value, and compensate the calibrated function into the servo loop.

[0051] Inner frame axis angle β yk As the independent variable, the gain change value K is taken as the dependent variable, and the second-order polynomial fitting function K is used. i =p0×β yk 2 (i)+p1×β yk (i)+p2, K i and the corresponding β yk (i) Substitute into the fitting function, calculate the values ​​of p0, p1, and p2, and finally obtain the fitting function between the inner frame axis angle and the gain change value.

[0052] Figure 2 This is a schematic diagram of the curve of the inner frame axis angle and the outer frame servo loop gain change value obtained by the test calibration of the present invention. Taking the open-loop amplitude-frequency characteristic curve when the inner frame axis is 0° as the reference, when the inner frame axis is 0°, the cutoff frequency at this time is ω c , the amplitude is 0dB, and the corresponding normalized gain value K0=1; when the inner frame axis rotates to 10°, the cutoff frequency ω c The corresponding amplitude is h1dB, and the normalized gain value is K1 when the inner frame axis is at 0°. When the inner frame axis rotates to 20°, its cutoff frequency ω c The corresponding amplitude is h2 dB, and the normalized gain value is K2 when the inner frame axis is at 0°. yk (i), K i ), and obtain the schematic diagram of the curve of the inner frame axis angle and gain change value based on the calibration data.

[0053] Figure 3 This is the amplitude-frequency characteristic curve of the uncompensated servo loop. The three-axis frame-type stable platform is not a spherical design, and the moment of inertia of the frame structure will change when it rotates around different axes. When the inner frame axis changes from 0° to 70°, the cutoff frequency of the outer frame axis moves from around 25Hz to around 50Hz, and the corresponding amplitude margin is also reduced, affecting the stability of the control system.

[0054] Figure 4This is the open-loop amplitude-frequency characteristic curve of the servo loop after calibration and compensation of the present invention. Using the calibration method, the gain change of the outer frame control loop when the inner frame axis is at different angles can be accurately tested, without relying on known parameters such as moment of inertia. The calibration result is used for data fitting to obtain a precise fitting function between the inner frame axis angle and the outer frame servo loop gain change value. When the inner frame axis angle changes from 0° to 70°, the cutoff frequency of the outer frame axis is always around 25Hz, which accurately compensates for the change in the amplitude-frequency characteristic of the outer frame servo loop caused by the change in moment of inertia, thereby improving the stability and reliability of the control system.

[0055] The above test results can verify the correctness and rationality of the three-axis stable platform servo loop gain change value calibration and compensation method of the present invention.

[0056] The above is only a specific implementation of the present invention, but the protection scope of the present invention is not limited thereto. Any changes or substitutions that can be easily conceived by any technician familiar with the technical field within the technical scope disclosed by the present invention should be covered within the protection scope of the present invention.

[0057] The contents not described in detail in the specification of the present invention belong to the common knowledge of the professionals in this field.

Claims

1. A method for calibrating and compensating the gain variation of a servo loop of a three-axis stable platform, characterized in that: Here are the steps: (1) Place the three-axis stabilized platform body axis, inner frame axis, and outer frame axis at 0°, 0°, and 0° respectively; (2) The servo loop is closed, and the external test equipment inputs a sine wave to the three-axis stabilized platform, and the output signal of the three-axis stabilized platform is collected at the same time. The frequency response of the sine wave is identified by the least square method, and the amplitudes of the input signal and the output signal at different frequencies are calculated to obtain the open-loop amplitude-frequency characteristic curve of the three-axis stabilized platform at the current position; (3) Each time the inner frame axis increases by 10°, the other two axes are 0°, and the above step (2) is repeated until the inner frame axis rotates to 70°; (4) The gain change value of the servo loop of the inner frame axis at different angles is calculated through the open-loop amplitude-frequency characteristic curve of the inner frame axis at 0° to 70°. (5) A second-order polynomial data fitting method is used to obtain a fitting function between the inner frame axis angle and the gain change value, and this function is compensated into the servo loop.

2. A method for calibrating and compensating a servo loop gain change value of a three-axis stabilized platform according to claim 1, characterized in that: In step (2), the open-loop amplitude-frequency characteristic curve is obtained by the following method: The external test equipment generates a sinusoidal input signal Uin, which is output to the servo loop through the digital serial port. The servo loop adds the sinusoidal input signal Uin and the control output signal Uctl to obtain an output signal Uout=Uctl+Uin, which is sent to the external test equipment through the digital serial port. The external test equipment uses the least squares identification method to calculate the open-loop amplitude-frequency characteristic curve of Uout / Uin.

3. A method for calibrating and compensating a servo loop gain variation value of a three-axis stabilized platform according to claim 1, characterized in that: In step (4), the gain change value of the servo loop at different angles of the inner frame axis is obtained by the following method: When the position of the inner frame axis of the three-axis stabilized platform changes, causing the gain value of the servo loop to change, the open-loop cutoff frequency will move linearly; taking the open-loop amplitude-frequency characteristic curve when the inner frame axis is at 0° as the reference, the cutoff frequency at this time is ω c , when the inner frame axis is 0°, the frequency is ω c When the amplitude is 0dB; when the inner frame axis rotates to 10°, the frequency is ω c When the inner frame rotates to 20°, the corresponding amplitude is h1dB; when the inner frame rotates to 20°, the frequency is ω c The corresponding amplitude is h2 dB; the following data are obtained by testing in sequence: The gain compensation value K is calculated by the following formula i : K i ＝10 (-hi / 20) Right now: 。 4. The method for calibrating and compensating the gain variation value of the servo loop of a three-axis stabilized platform according to claim 1, characterized in that: In step (5), the fitting function between the inner frame axis angle and the gain change value is obtained by the following method: Inner frame axis angle β yk As the independent variable, the gain change value K i As the dependent variable, a second-order polynomial fitting function K is used i =p0×β yk 2 (i)+p1×β yk (i)+p2, K i and the corresponding β yk (i) Substitute into the fitting function, calculate the values ​​of p0, p1, and p2, and finally obtain the fitting function between the inner frame axis angle and the gain change value.

5. A method for calibrating and compensating a servo loop gain variation value of a three-axis stabilized platform according to claim 4, characterized in that: The inner frame axis angle β yk is the angle of rotation of the outer frame around the inner frame axis, and the outer frame axis angle β xk is the angle of rotation of the base around the outer frame axis, and the table axis angle β zk It is the rotation angle of the inner frame around the table axis.

6. A method for calibrating and compensating a servo loop gain variation value of a three-axis stabilized platform according to claim 5, characterized in that: Inner frame axis angle β yk , outer frame axis angle β xk 、Table body axis angle β zk The measurement method is as follows: An angle sensor is installed on the outer frame axis to measure the angle β of the base rotating around the outer frame axis. xk ; An angle sensor is installed on the inner frame axis to measure the angle β of the outer frame rotating around the inner frame axis yk ; Install the sensor on the table axis to measure the angle β of the inner frame rotating around the table axis zk .

Images