Fiber-optic gyroscope assembly force control method

Abstract

The invention relates to a fiber-optic gyroscope assembly force control method, which belongs to the technical field of optical fiber assembly, and can keep the stress of an optical fiber between carried fiber-optic gyroscope parts within a required range in the process of carrying the fiber-optic gyroscope parts through cooperation of double mechanical arms for automatically assembling the fiber-optic gyroscope. And the optical fibers of the parts of the fiber-optic gyroscope are prevented from being damaged in the carrying process. The double mechanical arms comprise two mechanical arm assemblies, and each mechanical arm assembly comprises a mechanical arm, a six-dimensional force sensor, a fiber-optic gyroscope part grabbing device and a fiber-optic gyroscope part; and the two optical fiber gyroscope parts are connected through an optical fiber. Three modes of translation following, rotation following and force limiting following between the two mechanical arm assemblies can be achieved.

Description

Technical Field

[0001] The invention relates to a method for controlling assembly force of a fiber optic gyroscope, and belongs to the technical field of optical fiber assembly. Background Art

[0002] The optical path of a fiber optic gyroscope is made up of interconnected optoelectronic devices such as optical fiber rings, Y waveguides, and detectors. The assembly of the optical path of a fiber optic gyroscope involves rigid structural parts (device tubes and shells) and flexible optical fibers. Manual assembly has long been used, and quantitative detection and quantitative control of assembly force cannot be achieved. The quality of optical path assembly depends on the skill level of the assembly workers, and it is difficult to improve quality consistency. This also restricts the development and application of automatic assembly equipment for fiber optic gyroscopes, affecting the large-scale industrial development of fiber optic gyroscopes.

[0003] At present, the winding production process of the optical fiber ring, one of the components of the optical fiber gyroscope, is realized by an automatic or semi-automatic optical fiber winding machine. The optical fiber winding machine uses a tension sensor to monitor and control the optical fiber tension in real time. However, the optical fiber winding process involves hundreds to thousands of meters of optical fiber, and the shape mode and force mode of the optical fiber in the winding process are relatively simple, which can meet the requirements of the tension sensor for the length of the measured optical fiber and the installation method on the winding machine. For the assembly process of the optical fiber gyroscope, there is a situation where rigid parts (device tube shell) and flexible parts (device pigtail and optical fiber after fusion connection between devices) are mixed and assembled. During the assembly process, the shape and posture of the optical fiber are in a state of changing at any time, and the length of the optical fiber involved is only tens of centimeters to several meters, which is very different from the force condition and installation method of the optical fiber in the optical fiber winding production process. Therefore, the tension detection method used by the optical fiber winding machine equipment is irrelevant to the present invention. Summary of the invention

[0004] The technical problem solved by the present invention is: to overcome the shortcomings of the prior art and provide a method for controlling the assembly force of a fiber optic gyroscope, so as to realize the process of dual robotic arms cooperating to carry and assemble the fiber optic gyroscope components for automatic assembly of the fiber optic gyroscope, while keeping the force on the optical fiber between the fiber optic gyroscope components being carried and assembled within the required range.

[0005] The technical solution of the present invention is: a method for controlling the assembly force of a fiber optic gyroscope, which is used for a first mechanical arm and a second mechanical arm to coordinately carry a fiber optic gyroscope component, wherein the fiber optic gyroscope component includes a first fiber optic gyroscope component and a second fiber optic gyroscope component, and the two components are connected by an optical fiber. During transportation, the force of the optical fiber is controlled within a safe range; a six-dimensional force sensor is installed at the end of the first mechanical arm and the second mechanical arm, and a fiber optic gyroscope component grasping device is installed at the other end of the six-dimensional force sensor, and the control method comprises:

[0006] The relative distance between the ends of the first mechanical arm and the second mechanical arm during the handling process is determined according to the preset optical fiber length between the optical fiber gyroscope components, and the distance between the ends of the first mechanical arm and the second mechanical arm is less than the optical fiber length between the optical fiber gyroscope components; firstly, the first mechanical arm and the second mechanical arm move to the initial position; the two mechanical arms synchronously grab the two optical fiber gyroscope components connected by the flexible optical fiber;

[0007] The first robotic arm actively moves in a set direction, and the second robotic arm follows the movement of the first robotic arm; during the following movement, the second robotic arm adjusts its relative position with the first robotic arm in real time to ensure that the relative distance between the two is less than the optical fiber length between the fiber optic gyroscope components, the relative rotation is consistent, and the force exerted on the optical fiber between the fiber optic gyroscope components is within a preset safety range.

[0008] Furthermore, the two robotic arms synchronously grasp two fiber optic gyroscope components connected by flexible optical fibers, including: opening signals and clamping signals of clamping tools at the ends of the two robotic arms are triggered simultaneously.

[0009] Furthermore, the second robotic arm following the movement of the first robotic arm includes translational following:

[0010] Calculate the difference between the movement of the first and second robot arms relative to the original position in each direction, use the difference as the input of the PID controller, and obtain the velocity component of the second robot arm in that direction;

[0011] The translation vector of the second robot arm is obtained by calculating the three directions of the coordinate axis as a control parameter, which is cyclically refreshed at time intervals to keep the second robot arm following the translation of the first robot arm.

[0012] Further, the second mechanical arm follows the movement of the first mechanical arm including rotation following:

[0013] When the end of the first robot arm rotates, the rotation amount drz1 of the end relative to the original position is obtained, and the difference e between the rotation amount drz2 of the end of the second robot arm and the end of the second robot arm is calculated. rz , the deviation value e rz As the input of the PID controller, the rotation speed ω of the end of the second robot arm is calculated rz ;

[0014] At the same time, the deviation between the theoretical coordinate position of the second robot arm relative to the first robot arm and the actual coordinate position (e x , e y ); x ,e y ) as the input of the PID controller to control the end of the second robot arm V x , V yThe velocity component in the direction keeps the second robot arm following the rotation of the first robot arm;

[0015] The rotation angle of the end of the first robotic arm is combined with the distance between the first robotic arm and the second robotic arm, and the deviation value is calculated by the current angle and coordinates of the second robotic arm, and the control speed of the second robotic arm is output by PID control.

[0016] Furthermore, k1T pax and k2F max As the warning value of system control, k1 and k2 are set coefficients between 0.0 and 1.0 respectively; once the real-time collected force is greater than the warning value, the force adjustment is started, T max and F pax They are the maximum limits for torque and tension during handling.

[0017] Further, further, the force limit following comprises:

[0018] If the instantaneous value F of the pulling force in a certain direction exceeds the warning value k2F max , then the displacement adjustment dL=K p (k2F max -F), and the displacement adjustment amount is included in the following calculation of translation following and rotation following;

[0019] If the instantaneous torque value T rz Exceeded the warning value k1T max , then the angle adjustment amount dθ=K p (k1T max -T rz ), the angle adjustment amount is included in the following calculation of the rotation following;

[0020] Among them, K p is the proportional adjustment coefficient.

[0021] Furthermore, the force limiting following includes: adjusting the proportional coefficient K p By setting the coefficient k, the priority and sensitivity of the force control strategy in tension and torque response can be adjusted.

[0022] Furthermore, the gripping method of the fiber optic gyroscope component includes a mechanical clamping or opening method, a pneumatic suction method, or an electromagnetic suction method.

[0023] Furthermore, the six-dimensional force sensor is used to collect torques in three directions of Tx, Ty, and Tz at the end of the robotic arm, and tensile forces in three directions of Fx, Fy, and Fz.

[0024] A fiber optic gyroscope assembly force control device comprises a memory, a processor and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, the steps of the fiber optic gyroscope assembly force control method are implemented.

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

[0026] (1) The present invention provides a method for controlling the optical fiber tension during the whole process of handling and assembly by respectively arranging six-dimensional force / torque sensors at the ends of two mechanical arms for coordinated handling and assembly, thereby overcoming the limitation that the optical fiber tension sensor is difficult to implement during the automatic assembly of the optical fiber gyroscope, and realizing effective optical fiber force protection during the automatic assembly of the optical path of the optical fiber gyroscope;

[0027] (2) The present invention avoids force control failure caused by force interference of rigid-flexible hybrid devices in automatic assembly equipment through a redundant control scheme of two six-dimensional force / torque sensors, and realizes highly reliable force control of the complex assembly process of rigid-flexible hybrid devices;

[0028] (3) The present invention reduces the requirements for force detection accuracy of the force sensor in the micro-force control process by increasing the force arm to amplify the torque applied to the optical fiber and comprehensively utilizing force and torque information to monitor the force applied to the optical fiber, thereby achieving micro-force detection with high cost performance. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] Various other advantages and benefits will become apparent to those of ordinary skill in the art by reading the detailed description of the preferred embodiments below. The accompanying drawings are only for the purpose of illustrating the preferred embodiments and are not to be considered as limiting the present invention. Moreover, the same reference symbols are used throughout the accompanying drawings to represent the same components. In the accompanying drawings:

[0030] Figure 1 This is a schematic diagram of the framework of the robotic arm system of the present invention;

[0031] Figure 2 This is the overall control flow diagram of the present invention;

[0032] Figure 3 It is a schematic diagram of the data acquisition system of the robot arm system of the present invention;

[0033] Figure 4 This is a schematic diagram of the translational following of the present invention;

[0034] Figure 5 This is a schematic diagram of the translational following control process of the present invention;

[0035] Figure 6 This is a schematic diagram of the rotation following of the present invention;

[0036] Figure 7This is a PID block diagram of the robot arm end joint angle following calculation during the rotation movement of the present invention;

[0037] Figure 8 It is a schematic diagram of the rotation following control process of the present invention. DETAILED DESCRIPTION

[0038] In order to better understand the above technical scheme, the technical scheme of the present invention is described in detail below through the accompanying drawings and specific embodiments. It should be understood that the embodiments of the present invention and the specific features in the embodiments are detailed descriptions of the technical scheme of the present invention, rather than limitations on the technical scheme of the present invention. In the absence of conflict, the embodiments of the present invention and the technical features in the embodiments can be combined with each other.

[0039] The following is a further detailed description of a fiber optic gyroscope assembly force control method provided by an embodiment of the present invention in conjunction with the accompanying drawings of the specification. The specific implementation method may include:

[0040] A fiber optic gyroscope assembly force control method is used for a first mechanical arm and a second mechanical arm to coordinately carry fiber optic gyroscope parts, such as Figure 1 The fiber optic gyroscope component includes a first fiber optic gyroscope component and a second fiber optic gyroscope component. The two components are connected by an optical fiber. During transportation, the force on the optical fiber is controlled within a safe range. A six-dimensional force sensor is installed at the end of the first robotic arm and the second robotic arm, and a fiber optic gyroscope component grasping device is installed at the other end of the six-dimensional force sensor. Figure 2 , the control method comprises:

[0041] The relative distance between the ends of the first mechanical arm and the second mechanical arm during the handling process is determined according to the preset optical fiber length between the optical fiber gyroscope components, and the distance between the ends of the first mechanical arm and the second mechanical arm is less than the optical fiber length between the optical fiber gyroscope components; firstly, the first mechanical arm and the second mechanical arm move to the initial position; the two mechanical arms synchronously grab the two optical fiber gyroscope components connected by the flexible optical fiber;

[0042] The first robotic arm actively moves in a set direction, and the second robotic arm follows the movement of the first robotic arm; during the following movement, the second robotic arm adjusts its relative position with the first robotic arm in real time to ensure that the relative distance between the two is less than the optical fiber length between the optical fiber gyroscope components, the relative rotation is consistent, and the force on the optical fiber between the optical fiber gyroscope components is within a preset safety range

[0043] In the solution provided in the embodiment of the present invention, the first mechanical arm and the second mechanical arm cooperate to carry the fiber optic gyroscope components. The fiber optic gyroscope components are divided into two parts - the first fiber optic gyroscope component and the second fiber optic gyroscope component. The two parts are connected by an optical fiber. During transportation, the force on the optical fiber needs to be controlled within a safe range. Six-dimensional force sensors are installed at the ends of the first mechanical arm and the second mechanical arm. The so-called six-dimensional force sensor refers to a sensor that can collect torque in three directions of Tx, Ty, and Tz at the end of the mechanical arm, and tensile pressure in three directions of Fx, Fy, and Fz. A fiber optic gyroscope component grasping device is installed at the other end of the six-dimensional force sensor. The grasping method of the fiber optic gyroscope component here includes but is not limited to mechanical clamping or opening method, air pressure suction method, or electromagnetic suction method.

[0044] The entire control system is dispatched by the main controller. The main controller can obtain the data of the first robot arm, the second robot arm, the first six-dimensional force sensor and the second six-dimensional force sensor in real time, such as Figure 3 The acquired robot arm state includes the data of six degrees of freedom X, Y, Z, RX, RY, and RZ at the end of the robot arm, and the acquired six-dimensional force sensor includes the torque in the three rotation directions of Tx, Ty, and Tz and the tensile pressure in the three directions of Fx, Fy, and Fz.

[0045] The steps to obtain the signal conditioning hardware are as follows:

[0046] Step 1: Read the sensor's calibration matrix

[0047]

[0048] Step 2: Collect strain data in six directions through the amplification circuit

[0049] [G] 6×1 =[G0 G1 G2 G3 G4 G5] T

[0050] Step 3: After startup, continuously sample for a period of time and calculate the average static offset of each directional component to obtain the static deviation matrix

[0051] [B] 6×1 =[Bias0 Bias1 Bias2 Bias3 Bias4 Bias5] T

[0052] Step 4: Calculate the force and torque matrices

[0053] [CL] 6×6 ([G] 6×1 -[B] 6×1 )=[F x Fy F z T x T y T z ] T

[0054] Since there is a certain spatial offset between the actual product installation position and the center point of the force sensor, the final result needs to be converted to the product side.

[0055] Step 5: Coordinate transformation, let the force and torque matrix obtained in step 4 be F 6x1 , the translation offset matrix is ​​D 6x6 , then the six-dimensional force parameter Fnew at the product is:

[0056] [D] 6×6 [F] 6×1 =[Fnew] 6×1

[0057]

[0058] Among them, dz represents the offset of the product on the Z axis, dy represents the offset of the product on the Y axis, dx represents the offset of the product on the X axis, and the definition of the XYZ axis is consistent with the coordinate system of the end of the robot arm.

[0059] The present invention discloses a method for controlling the assembly force of a fiber optic gyroscope, and the process of coordinated handling is as follows:

[0060] Step 1: Determine the relative distance between the ends of the first and second robotic arms during the handling process based on the length of the optical fiber between the components of the FOG. If the length of the optical fiber between the components of the FOG is L0, the distance between the first and second robotic arms should be less than L0. First, the first and second robotic arms move to their initial positions. The coordinates of the first robotic arm are (x1, y1, z1), and the coordinates of the second robotic arm are (x2, y2, z2). Ensure that the following equation is satisfied:

[0061]

[0062] The two robotic arms synchronously grasp the two ends of the flexible fiber optic gyroscope component. Synchronization here means that the opening signal and clamping signal of the clamping tools at the ends of the two robotic arms are triggered at the same time.

[0063] Step 2: The first robot actively moves in the set direction, and the second robot follows the first robot. During the following movement, the second robot needs to adjust its relative position with the first robot in real time to ensure that the relative distance between the two is less than L0, the relative rotation is consistent, and the product force is less than the safety range.

[0064] Step 2.1: Translational following

[0065] refer to Figure 4 , Figure 5 , the relative movement distance of the end of the first robot arm relative to the initial position is (dx1, dy1, dz1), and the relative movement distance of the end of the second robot arm relative to the initial position is (dx2, dy2, dz2), where the z direction is perpendicular to the paper direction and is not shown in the figure. For following motion, the increments of the first and second robot arms relative to the initial position should be consistent, and the deviation value of the relative movement distance is used as the input parameter of feedback adjustment to control the movement of the second robot arm. The following method of the second robot arm in the case of translation, taking the x direction as an example, first calculates the difference in the movement of the ends of the first and second robot arms relative to the original positions.

[0066] e x =dx1-dx2

[0067] The difference e x As the input of the PID controller, the value of the velocity Vx component of the second robot in the X direction is obtained.

[0068]

[0069] The y and z directions are calculated in the same way to obtain the translation vector (Vx, Vy, Vz) of the second robotic arm as control parameters, which are cyclically refreshed at a time interval of dt to keep the second robotic arm following the translation of the first robotic arm.

[0070] · Step 2.2 Rotation and follow

[0071] refer to Figure 6 to Figure 8 When the end of the first robot arm rotates, the rotation amount drz1 of the end relative to the original position can be obtained, and the difference e between the rotation amount drz2 of the end of the second robot arm and the end of the second robot arm can be calculated. rz As the input of the PID controller, the angular velocity ω of the end of the second robot arm is calculated rz First, calculate the deviation value of the end angle increment

[0072] e rz =drz1-drz2

[0073] The deviation value e rz As the input of the PID controller, the rotation speed ω of the end of the second robot arm is calculated according to the following formula: rz .

[0074]

[0075] At the same time, the deviation between the theoretical coordinate position of the second robot arm relative to the first robot arm and the actual coordinate position (ex , e y ).

[0076]

[0077] As in step 2.1, (e x ,e y ) as the input of the PID controller to control the end of the second robot arm V x , V y The speed component in the direction of the first robot arm is used to keep the second robot arm following the rotation of the first robot arm. The rotation angle of the first robot arm end is combined with the distance between the first robot arm and the second robot arm 2 to calculate the deviation value with the current angle and coordinates of the second robot arm, and the control speed of the second robot arm is output by PID control.

[0078] · Step 2.3 Force Limitation

[0079] To ensure that the torque and tension of the entire system are always kept within the specified range during the handling process, max and F max In the system, k1Tmax and k2Fmax are used as the warning values ​​of the system control, where k1 and k2 are set coefficients between 0.0 and 1.0 respectively; once the real-time collected force is greater than the warning value, the force adjustment logic is started, and the specific adjustment method is as follows:

[0080] 1) If the instantaneous value of the tension F (F can be a component in any direction of x, y, or z) exceeds the warning value k2Fmax, the displacement adjustment dL is calculated.

[0081] dL=K p1 (k2F max -F)

[0082] In the formula, K p1 It is the proportional coefficient of tension adjustment control.

[0083] Include dL in the subsequent calculations in steps 2.1 and 2.2.

[0084] If Fx exceeds the warning value, dL will be added to e x If Fy and Fz exceed the warning value, dL will be included in e y 、e z In the calculation of e x For example, the calculation method after exceeding the warning value is:

[0085] e x =dx1-dx2-dL(x20-x10) / |x20-x10|

[0086] In the formula, x20 is the x coordinate of the starting position at the end of the second robot arm, and x10 is the x coordinate of the starting position at the end of the first robot arm.

[0087] Include dL in the calculation of step 2.2

[0088]

[0089] 2) If it is T rz When the instantaneous value of the torque exceeds the warning value k1Tmax, the angle adjustment dθ is calculated:

[0090] dθ=K p2 (k1T max -T rz )

[0091] In the formula, K p2 is the proportional coefficient of torque regulation control.

[0092] Include dθ in the follow-up calculation in step 2.2.

[0093] e rz =drz1-drz2-dθ(rz20-rz10) / |rz20-rz10|

[0094] Wherein, rz20 is the rz angle of the starting position at the end of the second robot arm, and rz10 is the rz angle of the starting position at the end of the first robot arm.

[0095] During the collaborative handling process, the main controller will adjust the posture when it recognizes that the force or torque at the end of any robotic arm exceeds the warning value. When the posture adjustment is triggered, no matter the two robotic arms move at the same time or one robotic arm moves before the other, the force on the flexible component can be reduced.

[0096] In addition, by adjusting K p1 , K p2 The relative size of k1 and k2 can adjust the priority of tension and torque adjustment. The force control strategy with higher priority has a larger K p value and k value.

[0097] The present invention provides a computer-readable storage medium, wherein the computer-readable storage medium stores computer instructions, and when the computer instructions are executed on a computer, the computer executes Figure 2 The method described.

[0098] Those skilled in the art will appreciate that embodiments of the present invention may be provided as methods, systems, or computer program products. Therefore, the present invention may take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Moreover, the present invention may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage and optical storage, etc.) containing computer-usable program code.

[0099] The present invention is described with reference to flowcharts and / or block diagrams of methods, devices (systems), and computer program products according to embodiments of the present invention. It should be understood that each process and / or block in the flowchart and / or block diagram, as well as the combination of processes and / or blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0100] These computer program instructions may also be stored in a computer-readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer-readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0101] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

[0102] It should be noted that, in this article, relational terms such as first and second, etc. are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device.

[0103] Obviously, those skilled in the art can make various changes and modifications to the present invention without departing from the spirit and scope of the present invention. Thus, if these modifications and variations of the present invention fall within the scope of the claims of the present invention and their equivalents, the present invention is also intended to include these modifications and variations.

[0104] The contents not described in detail in the specification of the present invention belong to the common knowledge of those skilled in the art.

Claims

1. A method for controlling the assembly force of a fiber optic gyroscope, wherein a first mechanical arm and a second mechanical arm cooperate to carry a fiber optic gyroscope component, wherein the fiber optic gyroscope component comprises a first fiber optic gyroscope component and a second fiber optic gyroscope component, and the two components are connected by an optical fiber. During transportation, the force on the optical fiber is controlled within a safe range; a six-dimensional force sensor is installed at the end of each of the first mechanical arm and the second mechanical arm, and a fiber optic gyroscope component grabbing device is installed at the other end of the six-dimensional force sensor, wherein: The control method comprises: The relative distance between the ends of the first mechanical arm and the second mechanical arm during the handling process is determined according to the preset optical fiber length between the optical fiber gyroscope components, and the distance between the ends of the first mechanical arm and the second mechanical arm is less than the optical fiber length between the optical fiber gyroscope components; firstly, the first mechanical arm and the second mechanical arm move to the initial position; the two mechanical arms synchronously grab the two optical fiber gyroscope components connected by the flexible optical fiber; The first robotic arm actively moves in a set direction, and the second robotic arm follows the movement of the first robotic arm; during the following movement, the second robotic arm adjusts its relative position with the first robotic arm in real time to ensure that the relative distance between the two is less than the optical fiber length between the fiber optic gyroscope components, the relative rotation is consistent, and the force exerted on the optical fiber between the fiber optic gyroscope components is within a preset safety range.

2. The method for controlling the assembly force of a fiber optic gyroscope according to claim 1, characterized in that: The two mechanical arms synchronously grasp two fiber optic gyroscope components connected by flexible optical fibers, including: opening signals and clamping signals of clamping tools at the ends of the two mechanical arms are triggered simultaneously.

3. The method for controlling assembly force of a fiber optic gyroscope according to claim 1, characterized in that: The second robot arm follows the first robot arm to move in translation: Calculate the difference between the movement of the first and second robot arms relative to the original position in each direction, use the difference as the input of the PID controller, and obtain the velocity component of the second robot arm in that direction; The translation vector of the second robot arm is obtained by calculating the three directions of the coordinate axis as a control parameter, which is cyclically refreshed at time intervals to keep the second robot arm following the translation of the first robot arm.

4. The method for controlling the assembly force of a fiber optic gyroscope according to claim 3, characterized in that: The second mechanical arm follows the movement of the first mechanical arm including rotation following: When the end of the first robot arm rotates, the rotation amount drz1 of the end relative to the original position is obtained, and the difference e between the rotation amount drz2 of the end of the second robot arm and the end of the second robot arm is calculated. rz , the deviation value e rz As the input of the PID controller, the rotation speed ω of the end of the second robot arm is calculated rz ; At the same time, the deviation between the theoretical coordinate position of the second robot arm relative to the first robot arm and the actual coordinate position (e x , e y ); x ,e y ) as the input of the PID controller to control the end of the second robot arm V x , V y The velocity component in the direction keeps the second robot arm following the rotation of the first robot arm; The rotation angle of the end of the first robotic arm is combined with the distance between the first robotic arm and the second robotic arm, and the deviation value is calculated by the current angle and coordinates of the second robotic arm, and the control speed of the second robotic arm is output by PID control.

5. The method for controlling assembly force of a fiber optic gyroscope according to claim 4, characterized in that: k1T max and k2F max As the warning value of system control, k1 and k2 are set coefficients between 0.0 and 1.0 respectively; once the real-time collected force is greater than the warning value, the force adjustment is started, T max and F max They are the maximum limits for torque and tension during handling.

6. The method for controlling assembly force of a fiber optic gyroscope according to claim 5, characterized in that: The force limitation follows: If the instantaneous value F of the pulling force in a certain direction exceeds the warning value k2F max , then the displacement adjustment dL=K p (k2F max -F), and the displacement adjustment amount is included in the following calculation of translation following and rotation following; If the instantaneous torque value T rz Exceeded the warning value k1T max , then the angle adjustment amount dθ=K p (k1T max -T rz ), the angle adjustment amount is included in the following calculation of the rotation following; Among them, K p is the proportional adjustment coefficient.

7. The method for controlling assembly force of a fiber optic gyroscope according to claim 6, characterized in that: The force limiting following includes: by adjusting the proportional coefficient K p By setting the coefficient k, the priority and sensitivity of the force control strategy in tension and torque response can be adjusted.

8. The method for controlling assembly force of a fiber optic gyroscope according to claim 1, characterized in that: The grasping method of the fiber optic gyroscope component includes a mechanical clamping or opening method, a pneumatic suction method, or an electromagnetic suction method.

9. The method for controlling assembly force of a fiber optic gyroscope according to claim 1, characterized in that: The six-dimensional force sensor is used to collect torques in three directions of Tx, Ty, and Tz at the end of the robotic arm, and tensile forces in three directions of Fx, Fy, and Fz.

10. A fiber optic gyroscope assembly force control device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the computer program, the steps of the method according to any one of claims 1 to 9 are implemented.

Images