Servo loop compliant continuous variable structure angular velocity correction and determination method

Abstract

A servo loop compliance continuous variable structure angular velocity correction and determination method, comprising: obtaining the angular velocity of a table body on X p axis, Y p axis and Z p axis according to the angular velocity output by a gyroscope mounted on the table body and acquiring the angle and angular velocity of the internal relative rotation of a four-axis inertial stabilization platform system; calculating the resultant rotation angular velocity ω p of the table body on Z z axis, the resultant rotation angular velocity ω p1 of an inner frame on Y y axis, the resultant rotation angular velocity ω p2 of a middle frame on X x axis and the resultant rotation angular velocity ω p3 of an outer frame on Y yk′ axis according to the angular velocity component and the angle of the internal relative rotation respectively. The present application can avoid the occurrence of singular values in the servo loop decoupling process, ensure the stabilization of the platform table body relative to the inertial space and has the advantages of full attitude and high precision.

Description

TECHNICAL FIELD

[0001] The application relates to a servo loop soft continuous variable structure angular velocity correction and determination method, and belongs to the technical field of inertial measurement. BACKGROUND

[0002] Since a three-axis inertial platform system has a frame locking phenomenon, it is difficult to meet the requirements of large maneuvering motion of a carrier, thus a four-axis inertial platform system is generated. The four-axis inertial platform system increases an outer frame on the basis of a platform body, an inner frame and a middle frame relative to the three-axis inertial platform system, and the outer frame is between the middle frame and a base.

[0003] In order to stabilize the platform body relative to an inertial space, a traditional solution is as follows: a servo loop adopts different decouplers according to intervals of an inner frame angle β yk and a middle frame angle β xk , and reference is made to the document "Variable Structure Partition Control of Four-Axis Gyro Stabilized Platform", Journal of Tsinghua University Vol.50, No.7, 2010. The reason for adopting the partition control is that when three gyroscope outputs and any one frame angle are combined to control four shaft motor, there are regions in which the motor driving torque is infinite, and the control may be invalid.

[0004] When the carrier moves, the inner frame angle and the middle frame angle are in different intervals, and when the partition control is adopted, switching between different control signals exists, even if a hysteresis relay characteristic switching condition is adopted, but the switching time will cause instantaneous shaking of the platform body, and the platform body shakes relative to the space. Therefore, under the premise of stabilizing the platform body relative to the inertial space, the interval division should be reduced as much as possible; meanwhile, the frame locking problem should also be considered. SUMMARY

[0005] The application solves the technical problem of solving the singular point angular velocity correction problem of the servo loop of the four-axis inertial stabilized platform.

[0006] The application is achieved by the following technical scheme:

[0007] A servo loop soft continuous variable structure angular velocity correction method of a four-axis inertial platform system, the four-axis inertial platform system comprising a base, a platform body, an outer frame, a middle frame and an inner frame, and the angular velocity correction method comprising the following steps:

[0008] measuring an angle β p2 of rotation of the outer frame around an X xk axis of the middle frame body coordinate system, an angle β p1 of rotation of the middle frame around a Y yk axis of the inner frame body coordinate system;

[0009] Given the value of the compliant variable structure parameter k, and the corresponding angle values of a plurality of singular points;

[0010] According to the k and the corresponding angle values of a plurality of singular points, the angular velocity correction value of the compliant variable structure is calculated:

[0011]

[0012] N is the number of singular points, i is the serial number of the singular point, β xki , β yki is the angle value of the i-th singular point.

[0013] In an embodiment of the present application, there are 9 singular points, which are (β yk1 , β xk1 ) = (90°, 0°), (β yk2 , β xk2 ) = (90°, 180°), (β yk3 , β xk3 ) = (90°, -180°), (β yk4 , β xk4 ) = (270°, 0°), (β yk5 , β xk5 ) = (270°, 180°), (β yk6 , β xk6 ) = (270°, -180°), (β yk7 , β xk7 ) = (-90°, 180°), (β yk8 , β xk8 ) = (-90°, 0°), (β yk9 , β xk9 ) = (-90°, -180°).

[0014] An angular velocity determination method based on the angular velocity correction method, comprising:

[0015] According to the angular velocity output by the gyroscope installed on the table body, the angular velocity components of the table body on the X p axis, Y p axis and Z p axis are obtained

[0016] The angle β p of the inner frame rotating around the Z zk axis of the table body coordinate system is measured;

[0017] According to the measured β xk , β yk , β zk , and and the angular velocity correction value Δω, the rotating angular velocities of the platform body, the inner frame, the middle frame and the outer frame are calculated, and the specific calculation formula is as follows:

[0018]

[0019] ω z is the synthetic rotating angular velocity of the platform body Z p axis; ω y is the synthetic rotating angular velocity of the inner frame Y p1 axis; ω x is the synthetic rotating angular velocity of the middle frame X p2 axis; ω yk′ is the synthetic rotating angular velocity of the outer frame Y p3 axis.

[0020] In an embodiment of the present application, the relative rotating angles inside the four-axis inertial stabilization platform system are measured by the following method:

[0021] An angle sensor is installed on the X p2 axis of the outer frame to measure the angle β p2 of the outer frame rotating around the X xk axis of the middle frame body coordinate system; an angle sensor is installed on the Y p1 axis of the inner frame to measure the angle β p1 of the middle frame rotating around the Y yk axis of the inner frame body coordinate system; a sensor is installed on the Z p axis of the platform body to measure the angle β p of the inner frame rotating around the Z zk axis of the platform body coordinate system.

[0022] In an embodiment of the present application, the value range of β yk is -90-270°; the value ranges of the rotating angles β zk , β xk , β yk′ are all -180-180°.

[0023] In an embodiment of the present application, the corresponding body coordinate systems of the base, the outer frame, the middle frame, the inner frame and the platform body are respectively the base body coordinate system X1Y1Z1, the outer frame coordinate system X p3 Y p3 Z p3 , the middle frame body coordinate system X p2 Y p2 Z p2 , the inner frame body coordinate system X p1 Y p1 Z p1 and the platform body coordinate system X p Y p Z.p The origins of the five coordinate systems coincide, and: the Z axis of the table body coordinate system coincides with the Z axis of the inner frame body coordinate system p The Z axis of the inner frame body coordinate system coincides with the Z axis of the middle frame body coordinate system p1 The Y axis of the middle frame body coordinate system coincides with the Y axis of the outer frame body coordinate system p2 The Y axis of the outer frame body coordinate system coincides with the Y axis of the base body coordinate system p1 The X axis of the base body coordinate system coincides with the X axis of the inner frame body coordinate system p3 The X axis of the inner frame body coordinate system coincides with the X axis of the middle frame body coordinate system p2 The Y axis of the outer frame body coordinate system coincides with the Y axis of the base body coordinate system p3 The Y axis of the base body coordinate system coincides with the Y axis of the carrier; when the stable platform system internally rotates relative to the carrier, the base rotates around the Y axis of the outer frame body coordinate system p3 The X axis of the outer frame body coordinate system rotates around the X axis of the middle frame body coordinate system p2 The Y axis of the middle frame body coordinate system rotates around the Y axis of the inner frame body coordinate system p1 The Z axis of the inner frame body coordinate system rotates around the Z axis of the table body coordinate system p The Z axis of the table body coordinate system rotates around the Z axis of the inner frame body coordinate system.

[0024] In an embodiment of the present application, the angular velocity determination method is used to improve the precision of inertial navigation.

[0025] In an embodiment of the present application, the angular velocity determination method is used to reduce the dynamic error of the table stability.

[0026] In an embodiment of the present application, the angular velocity determination method is used for the inertial platform table to adapt to the full attitude stability relative to the inertial space.

[0027] A computer readable storage medium has computer program instructions stored thereon, which, when loaded and run by a processor, cause the processor to execute the above angular velocity determination method or angular velocity correction method.

[0028] Compared with the prior art, the present application has the following beneficial effects:

[0029] (1) The method of the present application overcomes the influence of the transition process on the table stability in the interval switching process of the partition discrete variable structure control, eliminates the dynamic error caused by the frequent shaking caused by the interval switching, and thus improves the precision of inertial navigation.

[0030] (2) The method of the present application overcomes the "frame locking" through the soft obstacle avoidance control mode near the singular point, and thus realizes the full attitude adaptability of the inertial platform table relative to the inertial space stability.

[0031] (3) The control strategy of the method of the present application is simple and easy to implement in engineering. BRIEF DESCRIPTION OF DRAWINGS

[0032] Figure 1is a schematic diagram of a four-axis platform structure when the four frame angles are zero;

[0033] Figure 2 is a schematic diagram of the relationship between the five body coordinate systems in a four-axis inertially stabilized platform system;

[0034] Figure 3 is a step flow chart of a compliant continuous variable structure control method for a four-axis inertially stabilized platform servo loop proposed by the present application;

[0035] Figure 4 is a singular point of the four-axis platform of the present application;

[0036] Figure 5 is a region divided according to the inner frame angle and the middle frame angle in variable structure partition control;

[0037] Figure 6 is a plane graph of the inner frame angle and the middle frame angle when adopting partition discrete variable structure control;

[0038] Figure 7 is a change process of the frame angle with time when adopting partition discrete variable structure control;

[0039] Figure 8 is a change process of the angular velocity of the platform relative to the inertial space with time when adopting partition discrete variable structure control;

[0040] Figure 9 is a change process of the angle of the platform relative to the inertial space with time when adopting partition discrete variable structure control;

[0041] Figure 10 is a plane graph of the inner frame angle and the middle frame angle when adopting compliant continuous variable structure control;

[0042] Figure 11 is a change process of the frame angle with time when adopting compliant continuous variable structure control;

[0043] Figure 12 is a change process of the angular velocity of the platform relative to the inertial space with time when adopting compliant continuous variable structure control;

[0044] Figure 13 is a change process of the angle of the platform relative to the inertial space with time when adopting compliant continuous variable structure control;

[0045] Figure 14 is a change process of the modified angular velocity with time when adopting compliant continuous variable structure control. DETAILED DESCRIPTION

[0046] In order to make the purpose, technical solutions and advantages of the present application clearer, the embodiments of the present application will be further described in detail below with reference to the drawings.

[0047] The present invention provides a compliant continuous variable structure control method for a four-axis inertial stabilization platform servo loop, implemented based on a four-axis inertial stabilization platform system. This four-axis stabilization platform system includes a base, outer frame, middle frame, inner frame, and platform body, as shown below. Figure 1 As shown. The corresponding body coordinate systems are the base body coordinate system X1Y1Z1 and the outer frame coordinate system X... p3 Y p3 Z p3 , Medium frame body coordinate system X p2 Y p2 Z p2 The inner frame body coordinate system X p1 Y p1 Z p1 and the X coordinate system of the platform body p Y p Z p .

[0048] like Figure 2 The diagram shows the relationship between the five coordinate systems. The origins of these five coordinate systems coincide, and they have the following relative constraints: the Z-axis of the platform body coordinate system... p Z-axis and the coordinate system of the inner frame p1 The axes coincide, and the Y-axis of the body coordinate system of the middle frame is... p2 Y-axis and the coordinate system of the inner frame p1 The axes coincide, and the X coordinate of the outer frame body coordinate system is... p3 The X-axis of the axis and the body coordinate system of the middle frame p2 The X1 axis of the base body coordinate system coincides with the Y-axis of the follower frame body coordinate system. The base is fixed to the carrier. When the stable platform system undergoes internal relative rotation under the drive of the carrier: the base rotates around the Y-axis of the outer frame body coordinate system... p3 The axis rotates by an angle β. yk′ The X coordinate system of the outer frame around the middle frame body. p2 The axis rotates by an angle β. xk ; Y-axis of the middle frame around the inner frame body coordinate system p1 The axis rotates by an angle β. yk The Z-axis of the inner frame around the body coordinate system of the platform p The axis rotates by an angle β. zk .

[0049] like Figure 3 The processing flowchart shown illustrates the following steps in implementing the compliant continuous variable structure control method for the servo loop of the four-axis inertial platform system of this invention:

[0050] (3) Given the value of the compliant variable structure parameter k (e.g., 4×10) -6 Or 2×10-6 and 9 singular points (β yk1 , β xk1 ) = (90°, 0°), (β yk2 , β xk2 ) = (90°, 180°), (β yk3 , β xk3 ) = (90°, -180°), (β yk4 , β xk4 ) = (270°, 0°), (β yk5 , β xk5 ) = (270°, 180°), (β yk6 , β xk6 ) = (270°, -180°), (β yk7 , β xk7 ) = (-90°, 180°), (β yk8 , β xk8 ) = (-90°, 0°), (β yk9 , β xk9 ) = (-90°, -180°), according to and calculating the angular velocity correction value of the compliant variable structure

[0051]

[0052] (4) According to the measured β xk , β yk , β zk , and and the angular velocity correction value Δω, the rotation angular velocity of the platform body, the inner frame, the middle frame and the outer frame is calculated, and the specific calculation formula is as follows:

[0053]

[0054] Wherein, ω z is the combined rotation angular velocity of the platform body Z p axis; ω y is the combined rotation angular velocity of the inner frame Y p1 axis; ω x is the combined rotation angular velocity of the middle frame X p2 axis; ω yk′ is the combined rotation angular velocity of the outer frame Y p3 axis.

[0055] The four-axis inertially stabilized platform servo loop compliant continuous variable structure control method described above, in step (2), the relative rotation angle inside the four-axis inertially stabilized platform system is measured by the following method:

[0056] In the X p2An angle sensor is installed on the X axis to measure the angle of the outer frame rotating around the X axis of the middle frame body coordinate system p2 The angle of the X axis rotating is β xk The angle of the Y axis rotating is β p1 An angle sensor is installed on the Y axis to measure the angle of the middle frame rotating around the Y axis of the inner frame body coordinate system p1 The angle of the Y axis rotating is β yk The angle of the Z axis rotating is β p A sensor is installed on the Z axis to measure the angle of the inner frame rotating around the Z axis of the table body coordinate system p The angle of the Z axis rotating is β zk .

[0057] In the step (2), the value range of the rotating angle β yk is -90-270°; the value range of the rotating angle β zk , β xk , β yk′ is -180-180°.

[0058] Figure 4 The dot in the figure represents the singular point of the four-axis platform representing "frame locking". (β yk , β xk )=(90°, 0°), (90°, 180°), (90°, -180°), (270°, 0°), (270°, 180°), (270°, -180°), (-90°, 180°), (-90°, 0°), (-90°, -180°).

[0059] The four-axis inertial platform servo loop compliant continuous variable structure control method provided by the present application is illustrated by the schematic diagram as shown in Figure 5 . Figure 5 The document "Variable Structure Partition Control of Four-Axis Gyro Stabilized Platform, Tsinghua University Journal Vol. 50, No. 7, 2010" introduces a partition method of using different decouplers according to the interval of the inner frame angle β yk and the middle frame angle β xk , which has 16 intervals and 4 controllers. The compliant continuous variable structure control of the present application is based on Figure 4 , which has only one interval and one controller with angular velocity correction function.

[0060] Embodiment:

[0061] Let β yk′ , β xk , β yk , β zkThe initial values ​​are 0°, 60°, 90°, and 0° respectively. When using variable structure zoning control, the initial position is in region 4. Let the value of the compliant variable structure parameter k be 2 × 10⁻⁶. -6 angular velocity ω of the platform base x1 =-50° / s, ω y1 =0, ω z1 =0, the result is as follows Figure 6 , Figure 7 , Figure 8 and Figure 9 As shown. Among them, Figure 6 The diagram shows the angular phase plane of the inner frame and middle frame when using partitioned discrete variable structure control. Figure 7 This describes the change of frame angle over time when using partitioned discrete variable structure control. Figure 8 This describes the change of the angular velocity of the platform relative to the inertial space over time when using partitioned discrete variable structure control. Figure 9 This illustrates the change in the angle of the platform relative to the inertial space over time when using partitioned discrete variable structure control. It can be seen that at the moment of transition from region 4 to region 1, the platform sways relative to the inertial space, with an instantaneous angular velocity ω... xp and ω yp Approximately 900° / s, angle θ x More than 4°, θ y Above -6°C.

[0062] The result of using the compliant continuous variable structure control method of the present invention is as follows: Figure 10 , Figure 11 , Figure 12 , Figure 13 and Figure 14 As shown. Among them, Figure 10 The phase plane diagram of the angles of the inner frame and the middle frame when using compliant continuous variable structure control; Figure 11 This describes the change of frame angle over time when using compliant continuous variable structure control. Figure 12 This describes the change of the angular velocity of the platform relative to the inertial space over time when using compliant continuous variable structure control. Figure 13 This describes the change in the angle of the platform relative to the inertial space over time when using compliant continuous variable structure control. Figure 14 This demonstrates the correction of angular velocity over time when using compliant continuous variable structure control. It can be seen that the angular velocity and angle of the platform relative to inertial space are close to zero, thus avoiding singular values ​​and achieving high-precision stability of the platform.

[0063] The contents not described in detail in this specification are common knowledge to those skilled in the art.

[0064] Although the present application has been disclosed with reference to the preferred embodiments, it is not intended to limit the present application, and any person skilled in the art can make possible changes and modifications to the technical solutions of the present application using the disclosed methods and technical contents without departing from the spirit and scope of the present application. Therefore, any simple modification, equivalent change and modification made to the above embodiments according to the technical essence of the present application without departing from the technical solutions of the present application shall fall within the protection scope of the technical solutions of the present application.

Claims

1. A method for angular velocity correction of a compliant, continuously variable structure servo loop in a four-axis inertial platform system, the four-axis inertial platform system comprising a base, a platform body, an outer frame, a middle frame, and an inner frame, characterized in that, Angular velocity correction methods include: Measuring the X-axis of the outer frame around the coordinate system of the middle frame p2 Angle of rotation of the shaft The Y-axis of the middle frame around the inner frame's body coordinate system p1 Angle of rotation of the shaft ; Given compliant continuous variable structure parameters k The value of , and the corresponding angle values ​​of several singular points; in accordance with k Calculate the angular velocity correction value for the compliant continuous variable structure based on the corresponding angle values ​​of several singular points: N is the number of singularities, and i is the index of the singularity. , Let be the angle value of the i-th singularity.

2. The angular velocity correction method according to claim 1, characterized in that, There are 9 singularities in total, namely ( β yk1 , β xk1 ) = (90°, 0°), ( β yk2 , β xk2 ) = (90°, 180°), ( β yk3 , β xk3 ) = (90°, -180°), ( β yk4 , β xk4 ) = (270°, 0°), ( β yk5 , β xk5 ) = (270°, 180°), ( β yk6 , β xk6 ) = (270°, -180°), ( β yk7 , β xk7 ) = (-90°, 180°), ( β yk8 , β xk8 ) = (-90°, 0°), ( β yk9 , β xk9 ) = (-90°, -180°).

3. A method for determining angular velocity based on the angular velocity correction method of claim 1, characterized in that, include: Based on the angular velocity output by the gyroscope mounted on the platform, the position of the platform in the X direction is obtained. p Axis, Y p Axis and Z p angular velocity components on the axis , , ; Measuring the Z-axis of the inner frame around the body coordinate system of the platform p Angle of rotation of the shaft ; Based on the measurement , , , , and and angular velocity correction value Δ ω The combined rotational angular velocity of the platform, inner frame, middle frame, and outer frame is calculated using the following formula: in, For Taiwan Z p The resultant rotational angular velocity of the shaft; For the inner frame Y p1 The resultant rotational angular velocity of the shaft; For the medium frame X p2 The resultant rotational angular velocity of the shaft; For the outer frame Y p3 The resultant rotational angular velocity of the shaft.

4. The method for determining angular velocity according to claim 3, characterized in that, The relative rotation angles inside the four-axis inertial platform system were measured using the following method: X in the middle frame p2 An angle sensor is mounted on the axis to measure the X-axis of the outer frame around the coordinate system of the middle frame. p2 Angle of rotation of the shaft ; in the inner frame Y p1 An angle sensor is installed on the axis to measure the Y-axis of the middle frame around the inner frame's coordinate system. p1 Angle of rotation of the shaft ; in Taiwan Z p A sensor mounted on the axis measures the Z-axis coordinate of the inner frame around the body coordinate system of the stage. p Angle of rotation of the shaft .

5. The method for determining angular velocity according to claim 3, characterized in that, The value range is -90 to 270°; rotation angle , , The value range is -180 to 180°.

6. The method for determining angular velocity according to claim 3, characterized in that, The base, outer frame, middle frame, inner frame, and platform have corresponding body coordinate systems: base body coordinate system X1Y1Z1, outer frame body coordinate system X... p3 Y p3 Z p3 , Medium frame body coordinate system X p2 Y p2 Z p2 The inner frame body coordinate system X p1 Y p1 Z p1 and the X coordinate system of the platform body p Y p Z p The origins of the five coordinate systems coincide, and: the Z-axis of the platform body coordinate system... p Z-axis and the coordinate system of the inner frame p1 The axes coincide, and the Y-axis of the body coordinate system of the middle frame is... p2 Y-axis and the coordinate system of the inner frame p1 The axes coincide, and the X coordinate of the outer frame body coordinate system is... p3 The X-axis of the axis and the body coordinate system of the middle frame p2 The axes coincide, the X1 axis of the base body coordinate system and the Y axis of the outer frame body coordinate system. p3 The axes coincide; wherein, the base is fixedly connected to the carrier, and when the four-axis inertial platform system undergoes internal relative rotation under the drive of the carrier, the base revolves around the Y-axis of the outer frame's body coordinate system. p3 The axis rotates, and the outer frame revolves around the X coordinate system of the middle frame. p2 The axis rotates, and the middle frame revolves around the Y-axis of the inner frame's body coordinate system. p1 The axis rotates, and the inner frame rotates around the Z-axis of the platform's coordinate system. p The shaft rotates.

7. A computer-readable storage medium having stored thereon computer program instructions, which, when loaded and run by a processor, cause the processor to perform the method as described in any one of claims 1 to 6.

Images