A closed-loop control method for a rope-driven flexible manipulator with multi-space force-position fusion

Abstract

A closed-loop control method for a flexible manipulator with multi-space force-position fusion, comprising: calculating a joint angular velocity feedforward control planning value; calculating a joint angular velocity feedback control offset; calculating a joint angular velocity planning value; inputting the angular velocity planning value of the joint angular motion into a mapping relationship between the joint angle and the rope length, and converting it into an expected value of the rope motion speed; passing the current rope tension information and the expected tension value through a rope space feedback controller to obtain a constant force control feedback offset of the rope speed; calculating a gain coefficient of the driving rope, multiplying the rope speed constant force control feedback offset and then superimposing the resultant value on the rope motion speed planning value, and obtaining a rope motion speed planning value; inputting the rope motion speed planning value into a mapping relationship from the rope space to the driving space, and obtaining an expected value of the driving space motion; and inputting the feedback value of the motor encoder and the expected value of the motion into the driving space feedback controller in the driving space, and outputting a driving motor to rotate.

Description

Technical Field

[0001] The present invention belongs to the technical field of robots, and in particular relates to a closed-loop control method for a rope-driven flexible robotic arm with multi-space force-position fusion. Background Art

[0002] Compared to traditional articulated manipulators, rope-driven flexible manipulators offer superior aspect ratios, greater flexibility, and higher degrees of freedom, offering significant advantages for working in confined spaces. Current rope-driven flexible manipulators mostly utilize three motors driving a single joint with two degrees of freedom. Due to the control characteristics of this single-joint drive, such as redundancy, and the presence of rope elasticity and model errors, traditional position control strategies often fail to achieve optimal results for rope-driven manipulators. This often results in abnormally loose or over-tensioned ropes after repeated manipulator movement, significantly limiting the control accuracy and stability of rope-driven flexible manipulators. Given the need for high-precision joint motion and moderate rope tension in rope-driven manipulator control, a control strategy that simultaneously ensures both motion accuracy and rope stability is crucial for controlling rope-driven flexible manipulators. Summary of the Invention

[0003] This invention provides a closed-loop control method for a rope-driven flexible manipulator that integrates multiple spatial forces and positions, aiming to address at least one of the technical problems existing in the prior art. The invention proposes a closed-loop control method for a rope-driven flexible manipulator that can simultaneously ensure joint motion accuracy and rope tension.

[0004] The technical solution of the present invention is a closed-loop control method for a flexible manipulator with multi-space force-position fusion. The flexible manipulator includes a control box, a plurality of arms and joints connected in sequence, each of the joints having two degrees of freedom, yaw and pitch, and each of the joints connected to a drive box by three independent ropes. The method performs closed-loop position control in the flexible manipulator joint space based on feedback values from joint sensors, performs closed-loop rope tension control in the rope space based on feedback values from tension sensors, and performs closed-loop position control in the drive space based on feedback values from motor encoders. The method includes the following steps:

[0005] S100, obtain the expected value q of the joint angle motion of the flexible manipulator in the last cycle d，t-1 and the expected value q of the joint angle motion of the flexible manipulator in the current cycle d，t , calculate the feedforward control planning value of the joint angular velocity of the flexible manipulator in the current cycle

[0006]

[0007] Where ΔT is the length of the time period;

[0008] S200, the expected value of the joint angle movement of the flexible manipulator in the current cycle q d and the current joint angle information q of the flexible robotic arm cur As input to the joint space PID controller, calculate the joint angular velocity feedback control offset of the flexible manipulator in the current cycle

[0009] S300, feedback the joint angular velocity to control the offset And the feedforward control planning value of the flexible manipulator's current cycle joint angular velocity Sum and get the angular velocity planning value of the joint angular motion Right now

[0010]

[0011] S400, planning the angular velocity of the joint angle movement The mapping relationship between the input joint angle and the rope length is converted to the expected value of the rope movement speed

[0012] S500, obtaining the current tension information F of the flexible manipulator rope cur , the current tension information F of the flexible manipulator rope cur and the expected tensile force F d The constant force control feedback offset of the rope speed is obtained through the rope space PID controller

[0013] S600, according to the current tension information F of the flexible manipulator rope cur , calculate the gain coefficient K of the rope constant force control planning value of each driving rope n ;

[0014] S700, the gain coefficient K of the rope constant force control planning value n Constant force control feedback offset with rope speed Multiplied and added to the planned value of the rope movement speed The rope motion speed planning quantity is obtained

[0015]

[0016] Where n represents the nth driving rope;

[0017] S800, the rope movement speed planning amount Input the mapping relationship from rope space to drive space and get the expected value of the drive space motion

[0018] S900, in the drive space according to the feedback value of the motor encoder Expectations of exercise The input is sent to the driving space PID controller, and the output drives the motor of the robotic arm to rotate.

[0019] Furthermore, the input-output transfer function relationship of the joint space PID controller is:

[0020]

[0021] in, is the joint angular velocity feedback control offset of the flexible manipulator in the current cycle, q d，t is the expected value of the joint angle motion of the flexible manipulator in the current cycle, q cur is the current joint angle information of the flexible robotic arm.

[0022] Furthermore, the input-output transfer function relationship of the rope space PID controller is:

[0023]

[0024] in, is the constant force control feedback offset of the rope speed, F cur is the tension information fed back by the current rope tension sensor of the flexible manipulator, F d is the expected tensile force value.

[0025] Furthermore, the step S600 includes: obtaining the tension information F of the current rope of all ropes connected to the single joint. cur , calculate the rope with the smallest tension, and set the gain coefficient K of the rope with the smallest tension n Set to 1, the gain coefficient K of the remaining ropes n Set to 0.

[0026] Furthermore, the input-output transfer function relationship of the driving space PID controller is:

[0027]

[0028] in, is the feedback information of the flexible robotic arm motor encoder, is the desired motor speed value, It is the planned value of output motor speed.

[0029] Furthermore, the step S400 includes that the mapping relationship from the joint angle to the rope length is:

[0030]

[0031] Among them, q curThe current joint angle fed back by the joint encoder includes the joint yaw angle q1 and the joint pitch angle q2, l cur is the current rope length of the flexible manipulator.

[0032] Furthermore, the joint angle q in the next cycle next for:

[0033]

[0034] Among them, q cur The current joint angle of the flexible robotic arm fed back by the joint encoder, is the planned value of the angular velocity of the joint angular motion, and ΔT is the length of the time period.

[0035] Further, calculate the rope length l for the next cycle next for:

[0036]

[0037] Among them, q next The joint angle of the next cycle fed back by the joint encoder, the joint angle of the next cycle q next Including the joint yaw angle q of the next cycle next1 and the joint pitch angle q in the next cycle next2 .

[0038] Further, calculate the expected value of the rope movement speed in the next cycle for:

[0039]

[0040] Among them, l next is the rope length for the next cycle, l cur is the current rope length of the flexible manipulator, and ΔT is the length of the time period.

[0041] Furthermore, step S800 includes: the process from motor movement to rope movement also passes through the motor reducer and the ball screw, and the mapping relationship from rope speed to motor movement is:

[0042]

[0043] in, is the expected value of the driving space motion, K is the planned value of rope motion speed, n is the reduction ratio of the motor reducer, and S is the lead of the screw.

[0044] The beneficial effects of the present invention are:

[0045] The present invention proposes a closed-loop control method for a rope-driven flexible manipulator with multi-space force-position fusion. Position closed-loop control is performed in the flexible manipulator's joint space to ensure the accuracy of joint movement; rope tension closed-loop control is performed in the rope space to ensure that the rope will not be loose or over-tightened; and position closed-loop control is performed in the drive space to ensure the accuracy of motor movement under a given planned motion amount. The multi-space force-position fusion control method is used to achieve high-precision, high-stability control of the flexible manipulator and high stiffness when subjected to external forces, solving the problems of low motion accuracy of rope-driven flexible manipulators in the prior art due to control errors, and abnormal looseness and over-tightening of the rope after repeated movement of the manipulator due to rope deformation and model errors. BRIEF DESCRIPTION OF THE DRAWINGS

[0046] Figure 1 This is a flow chart of a closed-loop control method for a rope-driven flexible manipulator with multi-space force-position fusion in the present invention;

[0047] Figure 2 This is a control block diagram of a closed-loop control method for a rope-driven flexible manipulator with multi-space force-position fusion in the present invention;

[0048] Figure 3 This is a schematic diagram of the kinematic spatial relationship of a closed-loop control method for a rope-driven flexible manipulator with multiple spatial force-position fusion in the present invention;

[0049] Figure 4 is a schematic diagram of the rope-driven flexible manipulator joint of the present invention; DETAILED DESCRIPTION

[0050] The following will provide a clear and complete description of the concept, specific structure and technical effects of the present invention in conjunction with the embodiments and drawings to fully understand the purpose, scheme and effects of the present invention.

[0051] It should be noted that, unless otherwise specified, when a feature is referred to as being "fixed" or "connected" to another feature, it may be directly fixed or connected to another feature, or it may be indirectly fixed or connected to another feature. The singular forms "a", "said" and "the" used herein are also intended to include plural forms, unless the context clearly indicates otherwise. In addition, unless otherwise defined, all technical and scientific terms used herein have the same meaning as those generally understood by those skilled in the art. The terms used in this specification are only for describing specific embodiments and are not intended to limit the present invention. The term "and / or" used herein includes any combination of one or more related listed items. The use of any and all examples or exemplary language ("for example", "such as", etc.) provided herein is intended only to better illustrate the embodiments of the present invention and does not impose limitations on the scope of the present invention unless otherwise required.

[0052] Reference Figures 1 to 4 The present invention describes a closed-loop control method for a flexible robotic arm with multi-space force-position fusion. The flexible robotic arm includes a control box, a plurality of arms and joints connected in sequence, each of the joints has two degrees of freedom, yaw and pitch, and each of the joints is connected to a drive box by three independent ropes. The method performs closed-loop position control in the flexible robotic arm joint space according to the feedback value of the joint sensor, performs closed-loop rope tension control in the rope space according to the feedback value of the tension sensor, and performs closed-loop position control in the drive space according to the feedback value of the motor encoder.

[0053] Each joint of the flexible robotic arm is provided with a joint angle encoder, which can obtain the angle information of the joint; each rope of the flexible robotic arm is provided with a tension sensor, which can obtain the tension information of the rope. The flexible robotic arm has the capability of real-time kinematic analysis, and can obtain the planning value of the rope space movement based on the planning value of the joint space movement, and obtain the planning value of the drive space movement based on the planning value of the rope space movement. The joints of the flexible robotic arm are universal joints, which have two degrees of freedom with mutually perpendicular rotation axes. The joint angle value is the amount of motion of the joint rotation on the two degrees of freedom. Each joint is driven by three independent ropes, which is a redundant drive robotic arm. For other details about the flexible robotic arm, please refer to the invention disclosure document of the applicant previously entitled "A flexible robotic arm with end force feedback" with publication number CN111993400A.

[0054] Reference Figure 1 and Figure 2 In one embodiment, the method according to the present invention comprises the following procedural steps:

[0055] S100, obtain the expected value q of the joint angle motion of the flexible manipulator in the last cycle d，t-1 and the expected value q of the joint angle motion of the flexible manipulator in the current cycle d，t , calculate the feedforward control planning value of the joint angular velocity of the flexible manipulator in the current cycle

[0056]

[0057] Where ΔT is the length of the time period;

[0058] S200, the expected value of the joint angle movement of the flexible manipulator in the current cycle q d，t and the current joint angle information q of the flexible robotic arm cur As input to the joint space PID controller, calculate the joint angular velocity feedback control offset of the flexible manipulator in the current cycle

[0059] S300, feedback the joint angular velocity to control the offset And the feedforward control planning value of the flexible manipulator's current cycle joint angular velocity Sum and get the angular velocity planning value of the joint angular motion Right now

[0060]

[0061] S400, planning the angular velocity of the joint angle movement The mapping relationship between the input joint angle and the rope length is converted to the expected value of the rope movement speed

[0062] S500, obtaining the current tension information F of the flexible manipulator rope cur , the current tension information F of the flexible manipulator rope cur and the expected tensile force F d The constant force control feedback offset of the rope speed is obtained through the rope space PID controller

[0063] S600, according to the current tension information F of the flexible manipulator rope cur , calculate the gain coefficient K of the rope constant force control planning value of each driving rope n ;

[0064] S700, the gain coefficient K of the rope constant force control planning value n Constant force control feedback offset with rope speed Multiplied and added to the planned value of the rope movement speed The rope motion speed planning quantity is obtained

[0065]

[0066] Where n represents the nth driving rope;

[0067] S800, the rope movement speed planning amount Input the mapping relationship from rope space to drive space and get the expected value of the drive space motion

[0068] S900, in the drive space according to the feedback value of the motor encoder Expectations of exercise The input is sent to the driving space PID controller, and the output drives the motor of the robotic arm to rotate.

[0069] The present invention proposes a control method for an electrical device, which includes: calculating a joint angular velocity feedforward control planning value; calculating a joint angular velocity feedback control offset; calculating the joint angular velocity planning value through a joint space feedforward-feedback controller; inputting the angular velocity planning value of the joint angular motion into the mapping relationship between the joint angle and the rope length, and converting it into an expected value of the rope motion speed; passing the current rope tension information and the expected tension value through a rope space feedback controller to obtain a constant force control feedback offset of the rope speed; calculating the gain coefficient of the driving rope, multiplying the constant force control feedback offset of the rope speed and then superimposing it on the planning value of the rope motion speed to obtain a rope motion speed planning value; inputting the rope motion speed planning value into the mapping relationship from the rope space to the driving space to obtain the expected value of the driving space motion; in the driving space, inputting the feedback value of the motor encoder and the expected value of the motion into the driving space feedback controller, and outputting the driving motor to rotate. The present invention enables closed-loop position control in the flexible manipulator's joint space to ensure joint motion accuracy; closed-loop rope tension control in the rope space to ensure rope tension is neither loose nor over-tightened; and closed-loop position control in the drive space to ensure motor motion accuracy for a given planned motion volume. This multi-space force-position fusion control method achieves high-precision, high-stability control of the flexible manipulator, as well as high stiffness when subjected to external forces. This addresses the existing problems of low motion accuracy in rope-driven flexible manipulators due to control errors, as well as abnormal rope slack and over-tightening after repeated manipulator movement due to rope deformation and model errors.

[0070] Further, refer to Figure 2 , the input-output transfer function relationship of the joint space PID controller is:

[0071]

[0072] in, is the joint angular velocity feedback control offset of the flexible manipulator in the current cycle, q d，t is the expected value of the joint angle motion of the flexible manipulator in the current cycle, q cur The current joint angle information of the flexible manipulator is provided. The joint space position closed-loop control is achieved using a joint space PID controller, a feedforward-feedback controller where the feedforward is the desired value of the joint motion and the feedback is the output of the PID controller. Using a stable PID controller, the actual planning value is obtained based on the given desired and feedback values, and the PID controller planning can be implemented within the computer system's cycle.

[0073] The expected value of the joint angle motion at the current moment q d And the current joint angle information q fed back by the joint encoder curThe difference is used as the input of the controller, and the joint angular velocity feedback control offset of the next cycle is output through the joint space PID (proportional-integral-differential) controller. The closed loop of joint spatial position enables the flexible robotic arm to correct the planned amount of joint layer motion according to the error of joint angle, thus ensuring the accuracy of the robotic arm joint motion.

[0074] Further, refer to Figure 2 , the input-output transfer function relationship of the rope space PID controller is:

[0075]

[0076] in, is the constant force control feedback offset of the rope speed, F cur is the tension information fed back by the current rope tension sensor of the flexible manipulator, F d The stable PID controller is applied to obtain the actual planning value according to the given expected value and feedback value, and the planning of the PID controller can be realized in the cycle of the computer system.

[0077] The tension information F fed back by the current rope tension sensor of the flexible manipulator cur and the expected tensile force F d The difference is used as the input of the controller, and the feedback offset of the rope constant force control is output through the rope space PID controller.

[0078] Further, refer to Figure 2 , the step S600 includes:

[0079] Get the tension information F of all ropes connected to a single joint cur , calculate the rope with the smallest tension, and set the gain coefficient K of the rope with the smallest tension n Set to 1, the gain coefficient K of the remaining ropes n Set to 0.

[0080] The flexible manipulator's single joint has two degrees of freedom, yaw and pitch. Each joint is driven by three independent ropes, which is a three-to-two redundant drive. Since two drive ropes are required for precise position control, only one drive rope can be controlled by constant force, and it is impossible to ensure that the tension of all ropes is in a constant state. In the embodiment of the present invention, the rope with the smallest tension is selected for constant force control. During the movement of the manipulator, the tension of each rope is compared in real time, and the gain coefficient K of the rope with the smallest tension is used. n Set to 1, the remaining rope gain coefficients K n Set it to 0, which means that the minimum tension of the three driving ropes is controlled to a constant value.

[0081] The rope spatial force closed loop corrects the amount of rope movement, loosens the rope when the tension is too large, and tightens it when the tension is too small, so that the flexible robotic arm can ensure that the minimum tension of the three driving ropes of each joint is constant. When the minimum tension is constant, the tension values of the other two ropes corresponding to it will not be too large. By controlling the minimum tension, it is ensured that the rope will not be in an abnormally loose or over-tightened state.

[0082] Further, refer to Figure 2 , the input-output transfer function relationship of the driving space PID controller is:

[0083]

[0084] in, is the feedback information of the flexible robotic arm motor encoder, is the desired motor speed value, The planned value of the output motor speed is obtained by applying a stable PID controller, which can obtain the actual planned value according to the given expected value and feedback value, and can realize the planning of the PID controller in the cycle of the computer system.

[0085] The feedback information θ of the flexible manipulator motor encoder is cur and the desired motor speed value θ d The difference is used as the input of the controller, and the planned value of the motor speed is output by driving the spatial PID controller. Drive the motor to move. The closed-loop driving space position enables the flexible robotic arm to follow the motor instructions well, ensuring the accuracy of the robotic arm motor movement.

[0086] Through this multi-loop and multi-space force-position fusion control method in different spaces, high-precision and high-stability control of the joint motion of the flexible robotic arm is achieved, which solves the problems of low motion accuracy of the rope-driven flexible robotic arm due to control errors in the existing technology, and abnormal relaxation and over-tension of the rope after repeated movement of the robotic arm due to rope deformation and model errors.

[0087] The mathematical model transfer functions of the joint-space PID controller, the rope-space PID controller, and the drive-space PID controller are all continuous systems. However, in a computer system, the entire manipulator's control system is implemented in a periodic discrete system, requiring control planning for the manipulator in each time period. In the mathematical model of the PID controller, the input variables include the differential term, the integral term, and the input itself. These control variables need to be calculated within a cycle. The specific method is as follows:

[0088] In the computer program, the input deviation e, differential term de, and integral term ie are defined respectively, and the following calculations are performed in the loop of each control period:

[0089] de=(x d -x cur )-e,

[0090] e=x d -x cur ,

[0091] ie=ie+e,

[0092] The differential term of the current cycle is obtained by taking the difference between the input deviation of the previous cycle and the input deviation of the current cycle, and the integral term of the current cycle is obtained by taking the sum of the integral term of the previous cycle and the input deviation of the current cycle, and then solving it to get the output.

[0093] Further, refer to Figure 2 , the step S400 includes, the mapping relationship from the joint angle to the rope length is:

[0094]

[0095] Among them, q cur The current joint angle fed back by the joint encoder includes the joint yaw angle q1 and the joint pitch angle q2, l cur is the current rope length of the flexible manipulator.

[0096] Further, refer to Figure 2 , the joint angle q in the next cycle next for:

[0097]

[0098] Among them, q cur The current joint angle of the flexible robotic arm fed back by the joint encoder, is the planned value of the angular velocity of the joint angular motion, and ΔT is the length of the time period.

[0099] Further, refer to Figure 2 , calculate the rope length l for the next cycle next for:

[0100]

[0101] Among them, q next The joint angle of the next cycle fed back by the joint encoder, the joint angle of the next cycle q next Including the joint yaw angle q of the next cycle next1 and the joint pitch angle q in the next cycle next2 .

[0102] Further, refer to Figure 2 , calculate the expected value of the rope movement speed in the next cycle for:

[0103]

[0104] Among them, l next is the rope length for the next cycle, l cur is the current rope length of the flexible manipulator, and ΔT is the length of the time period.

[0105] Further, refer to Figure 1 and Figure 2 , the step S800 includes: the process from motor motion to rope motion also passes through the motor reducer and ball screw, and the mapping relationship from rope speed to motor motion is:

[0106]

[0107] in, is the expected value of the driving space motion, K is the planned value of rope motion speed, n is the reduction ratio of the motor reducer, and S is the lead of the screw.

[0108] Reference Figure 4 , the principle of maintaining high control stiffness under high-precision position closed-loop control of this method is analyzed from the perspective of a single joint of the flexible manipulator. Specifically, when the manipulator is subjected to an external force in a certain direction (such as force F in the figure), the tension of rope ③ close to the force increases, while the tension of rope ① farther away decreases. At this time, the tension of rope ① is the smallest, and the corresponding constant force control planning gain coefficient K n =1, which is a force-controlled rope. The gain coefficient K of the constant force control planning corresponding to ropes ②③ is n When it is 0, that is, it is in a simple position closed-loop control mode, it will not relax due to the increase of rope tension, thereby curbing the movement of the joint under the action of external force. The slight movement of the joint caused by rope deformation will be compensated by the joint position closed-loop control, achieving high stiffness of joint control.

[0109] It should be appreciated that the method steps in the embodiments of the present invention can be implemented or executed by computer hardware, a combination of hardware and software, or by computer instructions stored in a non-transitory computer-readable memory. The method can use standard programming techniques. Each program can be implemented in a high-level procedural or object-oriented programming language to communicate with the computer system. However, if desired, the program can be implemented in assembly or machine language. In any case, the language can be a compiled or interpreted language. In addition, for this purpose, the program can be run on a programmed application-specific integrated circuit.

[0110] Furthermore, the operations of the processes described herein may be performed in any suitable order unless otherwise indicated herein or otherwise clearly contradicted by the context. The processes described herein (or variations and / or combinations thereof) may be performed under the control of one or more computer systems configured with executable instructions and may be implemented as code (e.g., executable instructions, one or more computer programs, or one or more applications) that is executed collectively on one or more processors, by hardware, or a combination thereof. The computer program includes a plurality of instructions that can be executed by one or more processors.

[0111] Further, the methods can be implemented in any type of computing platform that is operably connected to a suitable computer, including but not limited to a personal computer, a minicomputer, a mainframe, a workstation, a network or distributed computing environment, a separate or integrated computer platform, or in communication with a charged particle tool or other imaging device, etc. Various aspects of the present invention can be implemented as machine-readable code stored on a non-transitory storage medium or device, whether removable or integrated into a computing platform, such as a hard disk, an optical read and / or write storage medium, RAM, ROM, etc., so that it can be read by a programmable computer, and when the storage medium or device is read by the computer, it can be used to configure and operate the computer to perform the processes described herein. In addition, the machine-readable code, or portions thereof, can be transmitted over a wired or wireless network. When such media includes instructions or programs that implement the steps described above in conjunction with a microprocessor or other data processor, the invention described herein includes these and other different types of non-transitory computer-readable storage media. When programmed according to the methods and techniques of the present invention, the present invention can also include the computer itself.

[0112] The computer program can be applied to input data to perform the functions described herein, thereby converting the input data to generate output data that is stored in a non-volatile memory. The output information can also be applied to one or more output devices such as a display. In a preferred embodiment of the present invention, the converted data represents a physical and tangible object, including a specific visual depiction of the physical and tangible object produced on the display.

[0113] The above description is merely a preferred embodiment of the present invention. The present invention is not limited to the aforementioned embodiments. As long as the technical effects of the present invention are achieved by the same means, any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention shall be included within the scope of protection of the present invention. Within the scope of protection of the present invention, various modifications and variations of the technical solutions and / or implementation methods are possible.

Claims

1. A closed-loop control method for a flexible manipulator with multi-space force-position fusion. The flexible manipulator comprises a control box, a plurality of sequentially connected arms and joints, each joint having two degrees of freedom (yaw and pitch), and each joint connected to a drive box by three independent cables. The method performs closed-loop position control in the flexible manipulator's joint space based on feedback from joint sensors, performs closed-loop cable tension control in the cable space based on feedback from tension sensors, and performs closed-loop position control in the drive space based on feedback from motor encoders. It is characterized by: The method comprises the following steps: S100, obtain the expected value of the joint angle movement of the flexible manipulator in the last cycle and the expected value of the joint angle motion of the flexible manipulator in the current cycle , calculate the feedforward control planning value of the joint angular velocity of the flexible manipulator in the current cycle , ， in, is the length of the time period; S200, the expected value of the joint angle movement of the flexible manipulator in the current cycle And the current joint angle information of the flexible robotic arm As input to the joint space PID controller, calculate the joint angular velocity feedback control offset of the flexible manipulator in the current cycle ; S300, feedback the joint angular velocity to control the offset And the feedforward control planning value of the flexible manipulator's current cycle joint angular velocity Sum and get the angular velocity planning value of the joint angular motion ,Right now ； S400, planning the angular velocity of the joint angle movement The mapping relationship between the input joint angle and the rope length is converted to the expected value of the rope movement speed ; S500, obtain the current tension information of the flexible robotic arm rope , the current tension information of the flexible manipulator rope and expected tensile force The constant force control feedback offset of the rope speed is obtained through the rope space PID controller ; S600, according to the current tension information of the flexible manipulator rope , calculate the gain coefficient of the rope constant force control planning value of each driving rope ; S700, the gain coefficient of the rope constant force control planning value Constant force control feedback offset with rope speed Multiplied and added to the planned value of the rope movement speed The rope motion speed planning quantity is obtained , ， Where n represents the nth driving rope; S800, the rope movement speed planning amount Input the mapping relationship from rope space to drive space and get the expected value of the drive space motion ; S900, in the drive space according to the feedback value of the motor encoder Expectations of exercise The input is sent to the driving space PID controller, and the output drives the motor of the robotic arm to rotate.

2. The method according to claim 1, wherein the input-output transfer function relationship of the joint space PID controller is: ， in, is the joint angular velocity feedback control offset of the flexible manipulator in the current cycle, is the expected value of the joint angle motion of the flexible manipulator in the current cycle, is the current joint angle information of the flexible manipulator, s is the complex frequency variable of the Laplace transform of the control system transfer function, and s represents the dynamic characteristics of the system.

3. The method according to claim 1, wherein The input-output transfer function relationship of the rope space PID controller is: ， in, is the constant force control feedback offset of the rope speed, is the tension information fed back by the current rope tension sensor of the flexible manipulator, is the expected tension value, s is the complex frequency variable of the Laplace transform of the control system transfer function, and s represents the dynamic characteristics of the system.

4. The method according to claim 1, wherein The step S600 includes: Get the tension information of all ropes connected to a single joint , calculate the rope with the smallest tension, and set the gain coefficient of the rope with the smallest tension Set to 1, the gain coefficient of the remaining ropes Set to 0.

5. The method according to claim 1, wherein The input-output transfer function relationship of the driving space PID controller is: ， in, is the feedback information of the flexible robotic arm motor encoder, is the desired motor speed value, is the planned value of the output motor speed, s is the complex frequency variable of the Laplace transform of the control system transfer function, and s represents the dynamic characteristics of the system.

6. The method according to claim 1, wherein The step S400 includes: The mapping relationship from joint angle to rope length is: ， in, The current joint angle fed back by the joint encoder, including the joint yaw angle and joint pitch angle , is the current rope length of the flexible manipulator, r is the radius of the rope hole distribution circle, d is the distance between the front and rear discs of the center block in the joint, is the angle of the kth rope hole relative to the y-axis in the current DH coordinate system.

7. The method according to claim 6, wherein: Joint angle in the next cycle for: ， in, The current joint angle of the flexible robotic arm fed back by the joint encoder, The angular velocity planning value for the joint angular motion, is the length of the time period.

8. The method according to claim 7, wherein: Calculate the rope length for the next cycle for: ， in, The joint angle of the next cycle fed back by the joint encoder, the joint angle of the next cycle Including the joint yaw angle of the next cycle and the joint pitch angle of the next cycle , r is the radius of the rope hole distribution circle, d is the distance between the front and rear discs of the center block in the joint, is the angle of the kth rope hole relative to the y-axis in the current DH coordinate system.

9. The method according to claim 8, wherein Calculate the expected value of the rope movement speed in the next cycle for: ， in, is the rope length for the next cycle, is the current rope length of the flexible manipulator, is the length of the time period.

10. The method according to claim 1, wherein The step S800 includes: The process from motor motion to rope motion also passes through the motor reducer and ball screw. The mapping relationship from rope speed to motor motion is: ， in, is the expected value of the driving space motion, is the planned amount of rope movement speed, is the reduction ratio of the motor reducer, and S is the lead of the screw.

Images