A method for controlling assembly force of fiber optic gyroscope

Abstract

A force control method for fiber optic gyroscope assembly, belonging to the field of fiber optic assembly technology, enables the automated assembly of fiber optic gyroscopes using a dual-arm robotic system. During the collaborative handling of fiber optic gyroscope components, the method maintains the force on the optical fibers between the components within a required range, preventing damage to the fibers during transport. The dual robotic arms comprise two arm assemblies, each including a robotic arm, a six-dimensional force sensor, a fiber optic gyroscope component gripping device, and the fiber optic gyroscope components. An optical fiber connects the two fiber optic gyroscope components. This invention allows for three modes of tracking between the two robotic arm assemblies: translational tracking, rotational tracking, and force-limited tracking.

Description

Technical Field

[0001] This invention relates to a method for controlling the assembly force of a fiber optic gyroscope, belonging to the field of fiber optic assembly technology. Background Technology

[0002] Fiber optic gyroscopes consist of interconnected optoelectronic devices such as fiber optic rings, Y-waveguides, and detectors. The assembly of the fiber optic gyroscope's optical path involves rigid structural components (device housings) and flexible optical fibers. For a long time, manual assembly has been used, which makes it impossible to quantitatively detect and control the assembly force. The quality of the optical path assembly depends on the skill level of the assembly workers, making it difficult to improve the consistency of quality. This has also restricted the development and application of automated assembly equipment for fiber optic gyroscopes, and affected the large-scale industrialization of fiber optic gyroscopes.

[0003] Currently, the production process of fiber optic rings, one of the components of fiber optic gyroscopes, is carried out by automatic or semi-automatic fiber optic winding machines. These machines use tension sensors for real-time monitoring and control of fiber tension. However, the fiber winding process involves hundreds to thousands of meters of fiber, and the shape and stress patterns of the fiber during winding are relatively simple, which cannot meet the requirements of tension sensors regarding the length of the fiber being measured and its installation method on the winding machine. In the fiber optic gyroscope assembly process, there is a mixed assembly of rigid components (device housing) and flexible components (device pigtails and the fiber optics fused together between devices). During assembly, the shape and orientation of the fiber are constantly changing, and the length of the fiber involved is only tens of centimeters to a few meters. This differs greatly from the stress conditions and installation methods of the fiber in the fiber winding production process. Therefore, the tension detection method used in the fiber optic winding machine is irrelevant to this invention. Summary of the Invention

[0004] The technical problem solved by this invention is to overcome the shortcomings of the prior art and provide a fiber optic gyroscope assembly force control method, so as to keep the force on the optical fibers between the fiber optic gyroscope components being transported and assembled within the required range during the process of automatic assembly of fiber optic gyroscope components by the collaborative handling and assembly of the fiber optic gyroscope components by the dual robotic arms.

[0005] The technical solution of this invention is: a fiber optic gyroscope assembly force control method for a first robotic arm and a second robotic arm to collaboratively transport fiber optic gyroscope components. The fiber optic gyroscope components include a first fiber optic gyroscope component and a second fiber optic gyroscope component, 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 both the first and second robotic arms, and a fiber optic gyroscope component gripping device is installed at the other end of the six-dimensional force sensor. The control method includes:

[0006] The relative distance between the ends of the first and second robotic arms during the handling process is determined based on the preset fiber optic length between the fiber optic gyroscope components. The distance between the ends of the first and second robotic arms is less than the fiber optic length between the fiber optic gyroscope components. First, the first and second robotic arms move to their initial positions. The two robotic arms then synchronously grasp the two fiber optic gyroscope components connected by flexible optical fibers.

[0007] The first robotic arm actively moves in a set direction, and the second robotic arm follows 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 fiber length between the fiber optic gyroscope components, that the relative rotation is consistent, and that the force on the fiber optic fibers 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: the opening signal and clamping signal of the end-effectors of the two robotic arms are triggered simultaneously.

[0009] Furthermore, the second robotic arm's following of the first robotic arm's movements includes translational following:

[0010] Calculate the difference in the amount of movement of the first and second robotic arms relative to their original positions in each direction, and use the difference as the input to the PID controller to obtain the velocity component of the second robotic arm in that direction.

[0011] The translational vector of the second robotic arm is obtained by calculating the three directions of the coordinate axes and used as a control parameter. It is refreshed cyclically at time intervals to keep the second robotic arm following the translation of the first robotic arm.

[0012] Furthermore, the second robotic arm's following of the first robotic arm's movements includes rotational following:

[0013] When the end effector of the first robotic arm rotates, the rotation amount drz1 of its end effector relative to its original position is obtained, and the difference e between this rotation amount drz2 and the rotation amount drz2 of the end effector of the second robotic arm is calculated. rz The deviation value e rz The rotational speed ω at the end effector of the second robotic arm is calculated as the input to the PID controller. rz ;

[0014] Simultaneously, the deviation (e) between the theoretical and actual coordinate positions of the second robotic arm relative to the first robotic arm is calculated. x e y ); will (e x ,e y The input of the PID controller controls the V at the end effector of the second robotic arm. x , V yThe velocity component in the direction keeps the second robotic arm following the motion as the end of the first robotic arm rotates.

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

[0016] Furthermore, k1T pax and k2F max As a warning value for system control, k1 and k2 are set coefficients between 0.0 and 1.0, respectively; once the force collected in real time exceeds the warning value, force adjustment is initiated, T max and F pax These are the maximum limits for torque and tension during the handling process, respectively.

[0017] Furthermore, the force-limiting follower includes:

[0018] If the instantaneous value F of the tension in a certain direction exceeds the warning value k2F max Then the displacement adjustment amount dL = K is calculated. p (k2F max -F), and incorporate the displacement adjustment into the translational and rotational following calculations;

[0019] If it is the instantaneous value of torque T rz Exceeded the warning value k1T max Then the angle adjustment amount dθ = K is calculated. p (k1T max -T rz The angle adjustment amount is incorporated into the rotational following calculation;

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

[0021] Furthermore, the force limiting follow includes: adjusting the proportional coefficient K p By setting a coefficient k, the priority and sensitivity of the force control strategy in terms of tensile and torque responses can be adjusted.

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

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

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

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

[0026] (1) The present invention overcomes the limitation that it is difficult to implement the tension sensor for fiber tension in the automatic assembly of fiber optic gyroscope by setting six-dimensional force / torque sensors at the ends of the two robotic arms for collaborative handling and assembly, and realizes effective fiber force protection in the automatic assembly of fiber optic gyroscope optical path.

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

[0028] (3) This invention increases the lever arm to amplify the torque on the optical fiber and comprehensively utilizes force and torque information to monitor the force on the optical fiber, thereby reducing the requirements for the force detection accuracy of the force sensor in the micro-force control process and realizing a high-performance micro-force detection. Attached Figure Description

[0029] Various other advantages and benefits will become apparent to those skilled in the art upon reading the following detailed description of preferred embodiments. The accompanying drawings are for illustrative purposes only and are not intended to limit the invention. Furthermore, the same reference numerals denote the same parts throughout the drawings. In the drawings:

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

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

[0032] Figure 3 This is a schematic diagram of the data acquisition system of the robotic 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 rotational following of the present invention;

[0036] Figure 7This is a PID block diagram for calculating the end joint angle of the robotic arm during rotational motion in this invention.

[0037] Figure 8 This is a schematic diagram of the rotational following control process of the present invention. Detailed Implementation

[0038] To better understand the above technical solutions, the technical solutions of the present invention will be described in detail below with reference to 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 solutions of the present invention, rather than limitations on the technical solutions 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 description, in conjunction with the accompanying drawings, provides a more detailed explanation of a fiber optic gyroscope assembly force control method provided by an embodiment of the present invention. Specific implementation methods may include:

[0040] A method for controlling the assembly force of a fiber optic gyroscope, used for the coordinated handling of fiber optic gyroscope components by a first robotic arm and a second robotic arm, such as... Figure 1 The fiber optic gyroscope components include a first fiber optic gyroscope component and a second fiber optic gyroscope component, connected by an optical fiber. During transportation, the force on the optical fiber is controlled within a safe range. Six-dimensional force sensors are installed at the ends of both the first and second robotic arms, and a fiber optic gyroscope component gripping device is installed at the other end of each force sensor. Figure 2 The control method includes:

[0041] The relative distance between the ends of the first and second robotic arms during the handling process is determined based on the preset fiber optic length between the fiber optic gyroscope components. The distance between the ends of the first and second robotic arms is less than the fiber optic length between the fiber optic gyroscope components. First, the first and second robotic arms move to their initial positions. The two robotic arms then synchronously grasp the two fiber optic gyroscope components connected by flexible optical fibers.

[0042] The first robotic arm actively moves in a predetermined direction, and the second robotic arm follows the first robotic arm's movement. During this following motion, the second robotic arm adjusts its relative position to the first robotic arm in real time to ensure that the relative distance between them is less than the length of the optical fiber between the fiber optic gyroscope components, that their relative rotation is consistent, and that the force on the optical fiber between the fiber optic gyroscope components is within a preset safety range.

[0043] In the solution provided in this embodiment of the invention, a first robotic arm and a second robotic arm work together to transport fiber optic gyroscope components. The fiber optic gyroscope components consist of two parts—a first fiber optic gyroscope component and a second fiber optic gyroscope component—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 both the first and second robotic arms. A six-dimensional force sensor is a sensor capable of collecting torque in the Tx, Ty, and Tz directions, and tensile and compressive forces in the Fx, Fy, and Fz directions at the end of the robotic arm. A fiber optic gyroscope component gripping device is installed at the other end of the six-dimensional force sensor. The gripping method for the fiber optic gyroscope components includes, but is not limited to, mechanical clamping or opening, pneumatic suction, or electromagnetic suction.

[0044] The entire control system is scheduled by the main controller. The main controller can acquire data in real time from the first robotic arm, the second robotic arm, the first six-dimensional force sensor, and the second six-dimensional force sensor, such as... Figure 3 The acquired robotic arm state includes data on the six degrees of freedom (X, Y, Z, RX, RY, RZ) at the end of the robotic arm. The acquired six-dimensional force sensor data includes torque in the three rotational directions (Tx, Ty, Tz) and tensile and compressive forces in the three directions (Fx, Fy, Fz).

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

[0046] Step 1: Read the sensor calibration matrix

[0047]

[0048] Step 2: Acquire strain data in six directions using an amplifier circuit.

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

[0050] Step 3: After startup, continuously sample for a period of time, 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 matrix

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

[0054] Since the actual product installation location is somewhat offset from the center point of the force sensor in space, it is necessary to convert the final result to the product side.

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

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

[0057]

[0058] Where dz represents the product's offset on the Z-axis, dy represents the product's offset on the Y-axis, and dx represents the product's offset on the X-axis. The XYZ axes are defined in the same way as the coordinate system of the robotic arm's end effector.

[0059] The present invention discloses a fiber optic gyroscope assembly force control method, the collaborative handling process of which 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 fiber optic cable lengths between the fiber optic gyroscope components. If the fiber optic cable length between the fiber optic gyroscope components is L0, then the distance between the first and second robotic arms should be less than L0. First, move the first and second robotic arms to their initial positions. Let the coordinates of the first robotic arm be (x1, y1, z1) and the coordinates of the second robotic arm be (x2, y2, z2). Ensure the following equation is satisfied:

[0061]

[0062] Two robotic arms synchronously grasp both ends of a flexible fiber optic gyroscope component. Synchronization here refers to the simultaneous triggering of the opening and clamping signals of the end-effectors on both robotic arms.

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

[0064] Step 2.1: Translational Following

[0065] refer to Figure 4 , Figure 5 The relative movement distance of the first robotic arm's end effector with respect to its initial position is (dx1, dy1, dz1), and the relative movement distance of the second robotic arm's end effector with respect to its initial position is (dx2, dy2, dz2), where the z-direction is perpendicular to the plane of the paper and is not shown in the figure. For following motion, the increments of movement of the first and second robotic arms with respect to their initial positions should be consistent. The deviation in relative movement distance is used as the input parameter for feedback adjustment to control the movement of the second robotic arm. Taking the x-direction as an example, the following method of the second robotic arm in translational motion first calculates the difference in movement between the end effectors of the first and second robotic arms relative to their original positions.

[0066] e x = dx1 - dx2

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

[0068]

[0069] The same calculation is performed for the y and z directions to obtain the translation vector (Vx, Vy, Vz) of the second robotic arm as control parameters. The vector is refreshed cyclically at time intervals of dt to keep the second robotic arm following the translation of the first robotic arm.

[0070] · Step 2.2 Rotation Follow

[0071] refer to Figures 6-8 When the end effector of the first robotic arm rotates, the rotation amount drz1 of its end effector relative to its original position can be obtained. The difference e between this rotation amount drz2 and the rotation amount drz2 of the end effector of the second robotic arm can then be calculated. rz The end effector angular velocity ω of the second robotic arm is calculated using this as input to the PID controller. rz First, calculate the deviation value of the end angle increment.

[0072] e rz =drz1-drz2

[0073] The deviation value e rz The rotational speed ω of the second robotic arm's end effector is calculated using the following formula, as input to the PID controller. rz .

[0074]

[0075] Simultaneously, the deviation (e) between the theoretical and actual coordinate positions of the second robotic arm relative to the first robotic arm is calculated.x e y ).

[0076]

[0077] As in step 2.1, (e) x ,e y The input of the PID controller controls the V at the end of the second robotic arm. x V y The velocity component in the direction is used to maintain the second robotic arm's following motion while the first robotic arm's end effector rotates. The rotation angle of the first robotic arm's end effector, combined with the distance between the first and second robotic arms, is used to calculate the deviation value with respect to the current angle and coordinates of the second robotic arm. The control speed of the second robotic arm is then output using PID control.

[0078] · Step 2.3 Force Limitation

[0079] To ensure that the torque and tensile force of the entire system remain within the specified range during the handling process. max and F max Within the system, k1Tmax and k2Fmax are used as early warning values ​​for system control, where k1 and k2 are set coefficients between 0.0 and 1.0, respectively. Once the force collected in real time exceeds the early warning value, the force adjustment logic is activated, 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 of the x, y, and z directions) exceeds the warning value k2Fmax, then the displacement adjustment amount dL is calculated.

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

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

[0083] dL will be included in the follow-up calculations in steps 2.1 and 2.2.

[0084] If Fx exceeds the warning value, then dL will be involved in e. x If Fy or Fz exceeds the warning value, then dL will be included in the calculation. y e z In the calculation. Using e x For example, the calculation method after exceeding the warning value is as follows:

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

[0086] In the formula, x20 is the x-coordinate of the starting position of the second robotic arm's end effector, and x10 is the x-coordinate of the starting position of the first robotic arm's end effector.

[0087] Incorporate dL into the calculation in step 2.2

[0088]

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

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

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

[0092] dθ is incorporated into the follow-up calculation in step 2.2.

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

[0094] In the formula, rz20 is the angle rz at the starting position of the second robotic arm end effector, and rz10 is the angle rz at the starting position of the first robotic arm end effector.

[0095] During collaborative handling, the main controller detects that the force or torque at the end of any robotic arm exceeds the warning value and performs posture adjustment. When posture adjustment is triggered, regardless of whether the two robotic arms move simultaneously or one robotic arm moves before the other, the force on the flexible components can be reduced.

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

[0097] This invention provides a computer-readable storage medium storing computer instructions that, when executed on a computer, cause the computer to perform... 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. Thus, the present invention may take the form of an entirely hardware embodiment, an entirely software embodiment, or an embodiment combining software and hardware. Furthermore, 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 magnetic disk storage and optical storage, etc.) containing computer-usable program code.

[0099] This invention is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart illustrations and / or block diagrams. Figure 1 a process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0100] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 a process or multiple processes and / or boxes Figure 1 The function specified in one or more boxes.

[0101] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 a process or multiple processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0102] It should be noted that, in this document, relational terms such as "first" and "second" are used only to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such process, method, article, or apparatus.

[0103] Obviously, those skilled in the art can make various modifications and variations to this invention without departing from its spirit and scope. Therefore, if these modifications and variations fall within the scope of the claims of this invention and their equivalents, this invention also intends to include these modifications and variations.

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

Claims

1. A method for controlling the assembly force of a fiber optic gyroscope, used for a first robotic arm and a second robotic arm to collaboratively transport fiber optic gyroscope components. The fiber optic gyroscope components include a first fiber optic gyroscope component and a second fiber optic gyroscope component, 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 both the first and second robotic arms, and a fiber optic gyroscope component gripping device is installed at the other end of the six-dimensional force sensor. The method is characterized in that… The control method includes: The relative distance between the ends of the first and second robotic arms during the handling process is determined based on the preset fiber optic length between the fiber optic gyroscope components. The distance between the ends of the first and second robotic arms is less than the fiber optic length between the fiber optic gyroscope components. First, the first and second robotic arms move to their initial positions. The two robotic arms then synchronously grasp the two fiber optic gyroscope components connected by flexible optical fibers. The first robotic arm actively moves in a set direction, and the second robotic arm follows 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 fiber length between the fiber optic gyroscope components, that the relative rotation is consistent, and that the force on the fiber optic fibers between the fiber optic gyroscope components is within a preset safety range.

2. The fiber optic gyroscope assembly force control method according to claim 1, characterized in that, The two robotic arms synchronously grasp two fiber optic gyroscope components connected by flexible optical fibers, including: the opening signal and clamping signal of the end-effectors of the two robotic arms are triggered simultaneously.

3. The fiber optic gyroscope assembly force control method according to claim 1, characterized in that, The second robotic arm follows the movement of the first robotic arm, including translational following: Calculate the difference in the amount of movement of the first and second robotic arms relative to their original positions in each direction, and use the difference as the input to the PID controller to obtain the velocity component of the second robotic arm in that direction. The translational vector of the second robotic arm is obtained by calculating the three directions of the coordinate axes and used as a control parameter. It is refreshed cyclically at time intervals to keep the second robotic arm following the translation of the first robotic arm.

4. The fiber optic gyroscope assembly force control method according to claim 3, characterized in that, The second robotic arm follows the movement of the first robotic arm, including rotational following: When the end effector of the first robotic arm rotates, the rotation amount drz1 of its end effector relative to its original position is obtained, and the difference e between this rotation amount drz2 and the rotation amount drz2 of the end effector of the second robotic arm is calculated. rz The deviation value e rz The rotational speed ω at the end effector of the second robotic arm is calculated as the input to the PID controller. rz ; Simultaneously, the deviation (e) between the theoretical and actual coordinate positions of the second robotic arm relative to the first robotic arm is calculated. x e y ); will (e x ,e y The input of the PID controller controls the V at the end of the second robotic arm. x V y The velocity component in the direction keeps the second robotic arm following the motion as the end of the first robotic arm rotates. The rotation angle at the end of the first robotic arm is combined with the distance between the first and second robotic arms. The deviation value is calculated by comparing it with the current angle and coordinates of the second robotic arm, and the control speed of the second robotic arm is output using PID control.

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

6. The fiber optic gyroscope assembly force control method according to claim 5, characterized in that, The force-limiting follow includes: If the instantaneous value F of the tension in a certain direction exceeds the warning value k2F max Then the displacement adjustment amount dL = K is calculated. p (k2F max -F), and incorporate the displacement adjustment into the translational and rotational following calculations; If it is the instantaneous value of torque T rz Exceeded the warning value k1T max Then the angle adjustment amount dθ = K is calculated. p (k1T max -T rz The angle adjustment amount is incorporated into the rotational following calculation; Among them, K p This is the proportional adjustment coefficient.

7. The fiber optic gyroscope assembly force control method according to claim 6, characterized in that, The force-limiting follow includes: adjusting the proportional coefficient K. p By setting a coefficient k, the priority and sensitivity of the force control strategy in terms of tensile and torque responses can be adjusted.

8. The fiber optic gyroscope assembly force control method according to claim 1, characterized in that, The gripping methods for the fiber optic gyroscope components include mechanical clamping or opening, pneumatic suction, or electromagnetic suction.

9. The fiber optic gyroscope assembly force control method according to claim 1, characterized in that, The six-dimensional force sensor is used to collect the torque in the three directions of Tx, Ty, and Tz at the end of the robotic arm, as well as the tensile and compressive forces in the 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, it implements the steps of the method as described in any one of claims 1 to 9.

Images