Rotational inertia compensation method of three-axis stabilized platform based on mechanical model

Abstract

The invention discloses a mechanical model-based rotational inertia compensation method for a three-axis stabilized platform, which comprises the following steps of: taking a platform body axis angle beta zk, an inner frame axis angle beta yk, rotational inertia # imgabs0 # imgabs1 # of a platform body for xp, yp and zp axes, rotational inertia # imgabs3 # of the platform body for a # imgabs2 # axis, rotational inertia # imgabs3 # of the platform body for a # imgabs2 # axis, and rotational inertia # imgabs5 # of the platform body for a # imgabs4 # axis as input information of a decoupling link; the compensation value of the rotational inertia is calculated through a mechanical model and is compensated for the outer ring shaft stable loop, and the gain change of the servo control loop caused by the change of the rotational inertia of the frame system is compensated. According to the method, a mechanical model between the angular acceleration of the frame and the total torque on the frame shaft is given for the first time and used for gain compensation of an outer ring servo control loop when the inner ring shaft is at a large angle, and the stability margin of the servo control loop when a platform frame system is at any position of the inner ring is guaranteed.

Description

Technical Field

[0001] The invention relates to a rotational inertia compensation method of a three-axis stable platform based on a mechanical model, and belongs to the field of inertial navigation. Background Art

[0002] With the increasing demand for maneuvering and changing orbits of vehicles, the inertial navigation system is required to achieve precise navigation and guidance under large maneuvers and high dynamic conditions. When the vehicle needs to perform large maneuvers and change orbits, the inner ring axis of the three-axis stabilized platform will have a large attitude angle. Under such conditions, the platform servo control 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 control loop to change. The change of the servo control loop gain will cause the change of the servo control loop amplitude stability margin, which will further affect the reliability and control accuracy.

[0004] In order to reduce the change in servo control loop gain caused by the change in frame moment of inertia, the three-axis stabilized platform currently works mainly within the ±45° range of the inner ring. The change in frame moment of inertia caused by the small angle of the inner ring is within the controllable range. The existing measures can only meet the requirements of carriers with limited maneuvering postures, and it is difficult to meet the requirements of large maneuvering movements of carriers. 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 rotational inertia compensation method for a three-axis stabilized platform based on a mechanical model, which can effectively improve the stability and reliability of the vehicle under large maneuvering conditions when the inner ring axis works at a large angle.

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

[0007] A method for compensating the moment of inertia of a three-axis stabilized platform based on a mechanical model, wherein the three-axis stabilized platform comprises a base, an outer frame, an inner frame and a platform body, and the corresponding base body coordinate system x b y b z b , External frame body coordinate system Internal frame body coordinate system and the platform body coordinate system x p y p z p ; z of the platform body coordinate system p Axis and inner frame body coordinate system Axis coincidence, the coordinate system of the inner frame Axis and external frame body coordinate system The axes coincide, the coordinate system of the outer frame The x axis and the base body coordinate system b Axis coincidence;

[0008] The rotation inertia compensation method of the three-axis stabilized platform based on the mechanical model is implemented as follows:

[0009] The coordinate system x of the three-axis stable platform is measured. p ,y p 、z p Moment of inertia of the shaft Internal frame to internal frame body coordinate system Moment of inertia of the shaft External frame to external frame body coordinate system Moment of inertia of the shaft

[0010] The relative rotation angle of the three-axis stable platform is measured, including the outer frame axis of the base around the outer frame body coordinate system Angle of shaft rotation β xk , the outer frame revolves around the inner frame axis of the inner frame body coordinate system Angle of shaft rotation β yk , the inner frame revolves around the table body axis z of the table body coordinate system p Angle of shaft rotation β zk ;

[0011] Calculate the moment of inertia gain compensation value of the outer frame axis The calculation formula is as follows:

[0012]

[0013] J xx The coupling between the table body axis and the inner frame axis during rotation The principal moments of inertia of the shaft.

[0014] Preferably, the coordinate system x of the platform body of the three-axis stable platform is measured by the following method: p ,y p 、z p Moment of inertia of the shaft Internal frame to internal frame body coordinate system Moment of inertia of the shaft External frame to external frame body coordinate system Moment of inertia of the shaft

[0015]

[0016] After assembling the platform, use the moment of inertia test equipment to test the moment of inertia of the platform in three directions using the torsion method. After assembling the inner frame, use the moment of inertia test equipment to test the moment of inertia of the inner frame in three directions using the torsion method. After assembling the outer frame, use the moment of inertia test equipment to test the moment of inertia of the outer frame using the torsion method.

[0017] Preferably, the outer frame axis of the base around the outer frame body coordinate system is measured by the following method: Angle of shaft rotation β xk , the outer frame revolves around the inner frame axis of the inner frame body coordinate system Angle of shaft rotation β yk , the inner frame revolves around the table body axis z of the table body coordinate system p Angle of shaft rotation β zk :

[0018] On the outer frame shaft Angle sensor is installed on the shaft to measure the base rotation Angle of shaft rotation β xk ; In the inner frame axis Angle sensor is installed on the shaft to measure the angle of the outer frame. Angle of shaft rotation β yk ; on the platform axis z p The sensor installed on the shaft measures the inner frame around z p Angle of shaft rotation β zk .

[0019] Preferably,

[0020] Preferably, the base body coordinate system x b y b z b , External frame body coordinate system Internal frame body coordinate system and the platform body coordinate system x p y p z p , the origins of the four coordinate systems coincide.

[0021] Preferably, the coordinate transformation matrix from the platform body coordinate system to the inner frame body coordinate system is

[0022]

[0023] Preferably, the coordinate transformation matrix from the inner frame body coordinate system to the outer frame body coordinate system is

[0024]

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

[0026] (1) The present invention provides a rotational inertia compensation method for a three-axis stabilized platform based on a mechanical model, which can compensate for the gain changes of the platform body axis and the inner frame axis at any angle, especially when the inner ring is at a large angle.

[0027] (2) The present invention provides a method for compensating the moment of inertia of a three-axis stabilized platform based on a mechanical model. The inner frame can work at any large angle when the value is non-singular, thereby increasing the reliability of the large maneuvering flight of the carrier. BRIEF DESCRIPTION OF THE DRAWINGS

[0028] Figure 1 It is a schematic diagram of the relationship between the four coordinates in the three-axis stable platform;

[0029] Figure 2 It is a schematic diagram of the change of the moment of inertia of the outer frame shaft based on the mechanical model of the present invention;

[0030] Figure 3 The present invention is a flow chart of a method for compensating the moment of inertia of a three-axis stabilized platform based on a mechanical model. DETAILED DESCRIPTION

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

[0032] The three-axis stable platform of the present invention is based on the rotational inertia compensation method of the mechanical model, through the frame system rotational inertia information: the platform body is x p ,y p 、z p Moment of inertia of the shaft Inner frame pair x p1 ,y p1 、z p1 Moment of inertia of the shaft Outer frame Shaft moment of inertia and the platform axis angle β zk , inner ring axis angle β yk As the input information of the compensation link, the change value of the moment of inertia is obtained through mechanical model calculation to realize the gain compensation value of the servo control loop.

[0033] The present invention is implemented based on a three-axis stable platform system, which includes a base, an outer frame, an inner frame and a platform body. The coordinate system of the three-axis stable platform is defined as follows Figure 1 As shown in the figure, it is a schematic diagram describing the relationship between the coordinate systems of each frame of the three-axis platform. The corresponding base body coordinate system x b y b z b , External frame body coordinate system Internal frame body coordinate system and the platform body coordinate system x p y p z p The origins of the four coordinate systems coincide, and the z coordinate system of the platform body p Axis and inner frame body coordinate system The axes coincide, the y coordinate system of the inner frame p1 Axis and external frame body coordinate system The axes coincide, the coordinate system of the outer frame The x axis and the base body coordinate system b The base, outer frame and inner frame are centered around the platform axis z. p Turn to β zk Angle, base and outer frame around the inner frame axis Turn to β yk Angle, base around outer frame axis Turn to β xk horn.

[0034] Taiwan body x p ,y p 、z p Moment of inertia of the shaft Internal frame pair Moment of inertia of the shaft External frame pair Moment of inertia of the shaft

[0035]

[0036] J xx The coupling between the table body axis and the inner frame axis during rotation Principal moment of inertia of the shaft, J yy The coupling between the table body axis and the inner frame axis during rotation Principal moment of inertia of the shaft, J xy The coupling between the table body axis and the inner frame axis during rotation The moment of inertia of the shaft, J yx The coupling between the table body axis and the inner frame axis during rotation The moment of inertia of the shaft, J xz The coupling between the table body axis and the inner frame axis during rotation The moment of inertia of the shaft.

[0037] The specific implementation process is as follows: According to the linear superposition principle of mechanical motion,

[0038]

[0039] Where, L p , are the momentum of each frame axis of the platform, Mp , Represents the total moment acting on the three supporting axes respectively.

[0040] According to the momentum theorem, the moment vector equation of the table axis is established as follows:

[0041]

[0042] is the moment of the platform in the z direction.

[0043] The moment vector equation for the inner frame is established as follows:

[0044]

[0045] in,

[0046] The torque in the direction of the inner frame axis is calculated as follows:

[0047]

[0048] is the moment on the inner frame in the y direction, are the angular velocities of the inner frame in the x, y, and z directions, respectively, are the angular velocities of the stage in the x, y, and z directions, respectively. The base, outer frame and inner frame are around the platform axis z p Angular velocity of rotation. are the angular accelerations of the inner frame in the x and y directions respectively.

[0049] The moment vector equation for the outer frame is established as follows:

[0050]

[0051] in,

[0052] The torque in the direction of the outer frame axis is calculated as follows:

[0053]

[0054] is the moment on the outer frame in the x direction, are the angular velocities of the outer frame in the x, y, and z directions, respectively, The base, outer frame and inner frame are around the platform axis z p The angular acceleration of rotation. are the angular accelerations of the inner frame in the x and y directions respectively. The base and the outer frame are around the inner frame axis The angular acceleration of rotation. The base and the outer frame are around the inner frame axis Angular velocity of rotation.

[0055] Ignore and The irrelevant small quantities give the torque-angular acceleration equations about the three frame axes as follows.

[0056]

[0057] According to the above dynamic model, the rotational inertia gain compensation value of the outer frame axis is calculated. The specific calculation formula is as follows:

[0058]

[0059] Figure 3 It is a schematic diagram of the change of the moment of inertia of the outer frame shaft based on the mechanical model of the present invention.

[0060] Based on the above principles, Figure 2 The processing flow chart shown in the figure, the implementation steps of the present invention are as follows:

[0061] (1) Measure the x-axis of the three-axis stable platform p ,y p 、z p Moment of inertia of the shaft Inner frame pair x p1 ,y p1 、z p1 Moment of inertia of the shaft External frame pair Moment of inertia of the shaft

[0062] (2) Measure the relative rotation angle of the three-axis stabilized platform system, including: the base around the outer frame body coordinate system Angle of shaft rotation β xk , the outer frame revolves around the inner frame body coordinate system Angle of shaft rotation β yk , the z coordinate system of the inner frame around the platform p Angle of shaft rotation β zk ;

[0063] (3) Calculate the rotational inertia gain compensation value of the outer frame axis. The specific calculation formula is as follows:

[0064]

[0065] in, It is the rotation inertia gain compensation value of the outer frame axis.

[0066] The rotation inertia compensation method of the three-axis stabilized platform based on the mechanical model: In step (1), the platform body of the three-axis stabilized platform is measured by the following method to obtain the x p ,y p 、z p Moment of inertia of the shaft Inner frame pair x p ,y p 、z p Moment of inertia of the shaft External frame pair Moment of inertia of the shaft

[0067] After assembling the platform, use the moment of inertia test equipment to test the moment of inertia of the platform in three directions using the torsion method. After assembling the inner ring, use the moment of inertia test equipment to test the moment of inertia of the inner ring in three directions using the torsion method. After assembling the outer ring, use the moment of inertia test equipment to test the outer ring and outer frame using the torsion method. Moment of inertia of the shaft

[0068] The above three-axis stable platform is based on the rotational inertia compensation method of the mechanical model: In step (2), the base around the outer frame body coordinate system is measured by the following method: Angle of shaft rotation β xk , the outer frame revolves around the inner frame body coordinate system Angle of shaft rotation β yk , the z coordinate system of the inner frame around the platform p Angle of shaft rotation β zk :

[0069] Outside the frame Angle sensor is installed on the axis to measure the base around the outer frame body coordinate system Angle of shaft rotation β xk ; In the inner frame Angle sensor is installed on the axis to measure the outer frame around the inner frame body coordinate system. Angle of shaft rotation β yk ; In Taiwan p The sensor installed on the axis measures the z-axis of the inner frame around the table body coordinate system p Angle of shaft rotation β zk .

[0070] The present invention presents for the first time a mechanical model between the absolute angular acceleration of the frame and the total torque on the frame axis, which is used to compensate for the gain of the servo control loop of the outer frame axis when the inner frame axis is at a large angle, thereby ensuring the stability margin of the servo control loop when the platform frame system is at any position of the inner frame.

[0071] Embodiment 1:

[0072] In this embodiment, the calculation formula of the present invention is used to calculate the moment of inertia compensation, and the setting conditions are as follows: the base is around the outer frame coordinate system Angle of shaft rotation β xk =0°; outer frame around inner frame coordinate system Angle of shaft rotation β yk =0°; inner frame around the platform coordinate system z p Angle of shaft rotation β zk =0°.

[0074] According to the calculation formula provided by the present invention, it can be obtained: This principal inertia is the outer frame axis when the frame system is in zero position. The main inertia in the direction, at this time the gain of the servo loop at zero position is K.

[0075] Embodiment 2:

[0076] In this embodiment, the calculation formula of the present invention is used to calculate the moment of inertia compensation, and the setting conditions are as follows: the base is around the outer frame coordinate system Angle of shaft rotation β xk =0°, outer frame around inner frame coordinate system Angle of shaft rotation β yk =70°; inner frame around the platform coordinate system z p Angle of shaft rotation β zk =0°.

[0078] According to the calculation formula provided by the present invention, it can be obtained: The equivalent gain of the servo control loop becomes

[0079] It can be seen from the above calculation results that when the position of the inner frame axis changes, the moment of inertia of the frame platform in the direction of the outer frame axis is strongly correlated with the position of the inner frame axis. The gain of the servo control loop must be compensated for the moment of inertia based on the mechanical model. Otherwise, the increase in the gain of the servo control loop will lead to a decrease in the stability margin, which will affect the stability and reliability of the system.

[0080] The inner frame axis angle rotates from 0° to 70°, and the servo control loop gain increases by 1.736 times. The calculation formula is By compensating this gain compensation value into the servo control loop, the gain change of the servo control loop caused by the change of the inner frame axis can be eliminated.

[0081] The above two embodiments can verify the correctness and rationality of the rotational inertia compensation method based on the mechanical model of the present invention.

[0082] 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.

[0083] 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 compensating the moment of inertia of a three-axis stabilized platform based on a mechanical model, characterized in that: The three-axis stable platform includes a base, an outer frame, an inner frame and a platform body. The corresponding base body coordinate system x b y b z b , External frame body coordinate system Internal frame body coordinate system and the platform body coordinate system x p y p z p ; z of the platform body coordinate system p Axis and inner frame body coordinate system The axes coincide with each other, and the coordinate system of the inner frame body Axis and external frame body coordinate system The axes coincide, the coordinate system of the outer frame The x axis and the base body coordinate system b Axis coincidence; The rotation inertia compensation method of the three-axis stabilized platform based on the mechanical model is implemented as follows: The coordinate system x of the three-axis stable platform is measured. p ,y p 、z p Moment of inertia of the shaft Inner frame to inner frame body coordinate system Moment of inertia of the shaft External frame to external frame body coordinate system Moment of inertia of the shaft The relative rotation angle of the three-axis stable platform is measured, including the outer frame axis of the base around the outer frame body coordinate system Angle of shaft rotation β xk , the outer frame revolves around the inner frame axis of the inner frame body coordinate system Angle of shaft rotation β yk , the inner frame revolves around the table body axis z of the table body coordinate system p Angle of shaft rotation β zk ; Calculate the moment of inertia gain compensation value of the outer frame axis The calculation formula is as follows: J xx The coupling between the table body axis and the inner frame axis during rotation The principal moments of inertia of the shaft.

2. The method for compensating the moment of inertia of a three-axis stabilized platform based on a mechanical model according to claim 1, characterized in that: The coordinate system x of the three-axis stable platform to the platform body is measured by the following method: p ,y p 、z p Moment of inertia of the shaft Inner frame to inner frame body coordinate system Moment of inertia of the shaft External frame to external frame body coordinate system Moment of inertia of the shaft After assembling the platform, use the moment of inertia test equipment to test the moment of inertia of the platform in three directions using the torsion method. After assembling the inner frame, use the moment of inertia test equipment to test the moment of inertia of the inner frame in three directions using the torsion method. After assembling the outer frame, use the moment of inertia test equipment to test the moment of inertia of the outer frame using the torsion method.

3. The method for compensating the moment of inertia of a three-axis stabilized platform based on a mechanical model according to claim 1, characterized in that: The outer frame axis of the base around the outer frame body coordinate system is measured by the following method Angle of shaft rotation β xk , the outer frame revolves around the inner frame axis of the inner frame body coordinate system Angle of shaft rotation β yk , the inner frame revolves around the table body axis z of the table body coordinate system p Angle of shaft rotation β zk : On the outer frame shaft Angle sensor is installed on the shaft to measure the base rotation Angle of shaft rotation β xk ; In the inner frame axis Angle sensor is installed on the shaft to measure the angle of the outer frame. Angle of shaft rotation β yk ; on the platform axis z p The sensor installed on the shaft measures the inner frame around z p Angle of shaft rotation β zk .

4. The method for compensating the moment of inertia of a three-axis stabilized platform based on a mechanical model according to claim 1, characterized in that:

5. The method for compensating the moment of inertia of a three-axis stabilized platform based on a mechanical model according to claim 1, characterized in that: Base body coordinate system x b y b z b , External frame body coordinate system Internal frame body coordinate system and the platform body coordinate system x p y p z p , the origins of the four coordinate systems coincide.

6. The method for compensating the moment of inertia of a three-axis stabilized platform based on a mechanical model according to claim 1, characterized in that: Coordinate transformation matrix from platform body coordinate system to internal frame body coordinate system 7. The method for compensating the moment of inertia of a three-axis stabilized platform based on a mechanical model according to claim 1, characterized in that: Coordinate transformation matrix from the inner frame body coordinate system to the outer frame body coordinate system

Images