A closed-loop control method and system for a linked rope-driven flexible manipulator

Abstract

The present application relates to a linkage type rope-driven flexible mechanical arm closed-loop control method and system, wherein the method comprises: obtaining the desired target configuration of the linkage type rope-driven flexible mechanical arm at the next time; obtaining all angle sensor data of the linkage type rope-driven flexible mechanical arm; calculating the equivalent joint rotation angle of each linkage arm segment at the current time; calculating the joint closed-loop output; converting the joint closed-loop output into a motion speed; converting the motion speed of the linkage type rope-driven flexible mechanical arm rope into a motor motion speed; controlling the motor to follow the motor motion speed; judging whether the trajectory is completed, if not, entering the next control cycle, otherwise ending the control. The system comprises a matching linkage type rope-driven flexible mechanical arm, a joint sensor data acquisition system and a computer device implementing the above method. The present application solves the problems of unstable signal transmission, wiring difficulty and improves the motion control precision of the linkage type flexible arm.

Description

Technical Field

[0001] This invention relates to a closed-loop control method and system for a linkage-type rope-driven flexible robotic arm, belonging to the field of robotics technology. Background Technology

[0002] Rope-driven flexible robotic arms, especially linkage-type rope-driven flexible robotic arms, have advantages such as long and narrow arm segments, high degrees of freedom, good bending effect, large range of motion, and fewer motors. Therefore, linkage-type rope-driven flexible robotic arms are highly flexible in confined spaces and complex scenarios, and are widely used in precision operations in aerospace, nuclear power, and other confined spaces. However, in reality, the intense friction generated by the contact between the rope and the mechanism, as well as the elastoplastic deformation of the rope, greatly affect the motion accuracy of linkage-type rope-driven flexible robotic arms. These factors cause the joints within the linkage joint segment to move in a non-strictly uniform manner, resulting in a significant difference between the theoretical motion model and the actual motion model, making closed-loop control of the system difficult. On the other hand, the numerous degrees of freedom and excessively long arm segments of linkage-type rope-driven flexible robotic arms can easily lead to problems such as difficulties in wiring within the arm and unstable and unreliable sensor signal transmission. Summary of the Invention

[0003] This invention provides a closed-loop control method and system for a linkage-type rope-driven flexible robotic arm, aiming to solve at least one of the technical problems existing in the prior art, mainly including unstable signal transmission of joint angle sensors, difficulty in wiring, and improving the motion control accuracy of the linkage-type flexible arm.

[0004] The technical solution of this invention relates to a closed-loop control method for a linkage-type rope-driven flexible manipulator, used to control the linkage-type rope-driven flexible manipulator to perform a work task. The method according to this invention includes the following steps:

[0005] S110. Obtain the desired target configuration q of the linked rope-driven flexible manipulator at the next moment. d ;

[0006] S120. Obtain all angle sensor data q of the linkage-type rope-driven flexible robotic arm. real ;

[0007] S130. Calculate the equivalent joint rotation angle q of each of the aforementioned linked arm segments at the current moment. eq ;

[0008] S140, The desired target configuration q d The equivalent joint angle q of each of the aforementioned linkage arm segments eq The difference is used as the input to the joint closed-loop control, and the PID output of the joint closed-loop control is calculated. out ;

[0009] S150, PID the closed-loop output of the joint out Converted to Vel of motion of the rope in a linkage-type rope-driven flexible robotic arm l ;

[0010] S160, the motion speed Vel of the linkage-type rope-driven flexible robotic arm rope is... l This is converted into the speed of the motor.

[0011] S170, Control the motor to follow the motor's movement speed;

[0012] S180. Determine the configuration q of the current linkage-type cable-driven flexible robotic arm. cur Is it the desired configuration q of the final target? tar Otherwise, return to S110 to enter the next control cycle; otherwise, end the control.

[0013] Furthermore, the procedure before step S110 includes:

[0014] S101. Obtain the current configuration q of the linkage-type cable-driven flexible robotic arm based on the joint angle sensor. cur And obtain the final target desired configuration q based on user input. tar The configuration q cur and the desired final target configuration q tar The input is fed into a trajectory planner, which includes, but is not limited to, a quintic spline planner, a cubic spline planner, and a trapezoidal planner;

[0015] S102. Obtain a trajectory by acquiring a series of configuration state points that change over time, output by the trajectory planner, and adjust the trajectory from the current configuration q of the linkage-type cable-driven flexible manipulator according to the control period T. cur To the final target desired configuration q tar Discretized into a series of trajectory points, the current configuration q of the linkage-type cable-driven flexible manipulator... cur The target desired trajectory configuration q of the linked rope-driven flexible manipulator is the first of a series of trajectory state points. tar This is the last of a series of trajectory state points described by the joint rotation angles of each joint of the linked rope-driven flexible manipulator, where the joint rotation angle is the relative angle between adjacent joints and the arm about the axis of rotation of the linked rope-driven flexible manipulator.

[0016] Furthermore, in step S110, the desired target configuration q of the linked rope-driven flexible manipulator at the next moment is obtained. d include:

[0017] The trajectory planner obtains the configuration-time curve of the linkage-type cable-driven flexible manipulator, thereby obtaining the desired configuration for each control cycle T.

[0018] The trajectory configuration of the linkage-type cable-driven flexible robotic arm is as follows:

[0019]

[0020] t tar = [t1, t2, ..., t n [This is] the time matrix associated with the trajectory configuration.

[0021] In the subinterval t∈[t i , t i+1 In the figure, the interpolated configuration time motion curve is as follows:

[0022] q d =a i +b i (t d -t i )+c i (t d -t i ) 2 +d i (t d -t i ) 3

[0023] in,

[0024] a i =q i

[0025]

[0026]

[0027]

[0028] Among them, t d For the desired configuration q d The location of the moment.

[0029] Furthermore, in step S130, the equivalent joint rotation angle q of each of the linked arm segments at the current moment is calculated. eq ;include,

[0030] The equivalent joint equivalent rotation angle q eq It is the weighted average of all pitch angles or the average of all yaw angles of each joint within the aforementioned linkage boom segment.

[0031] Furthermore, in step S140, the desired target configuration q is... d The equivalent joint angle q of each of the aforementioned linkage arm segments eqThe difference Δq is used as the input to the joint closed-loop control, and the joint closed-loop output PID is calculated. out ,include,

[0032]

[0033] Where, Δq=q d -q eq K p K d and K i All are constants.

[0034] Further, in step S150, the joint closed-loop output PID is... out This is further converted into the motion velocity Vel of the rope of the linked rope-driven flexible robotic arm. l ,,include,

[0035] Vel l =(l d -l eq ) / T

[0036] Among them, Vel l Let l be the motion velocity of the rope of each of the linked rope-driven flexible robotic arms at that moment. d To determine the desired rope length for control at the next moment, l eq The current rope length is T, where T is the control period, and the desired rope length for the next control moment is l. d The PID output of the joint closed loop out The desired control position q at the next moment is calculated using the control period T. c =PIDout*T is then calculated based on the kinematic model of joint angle to rope length.

[0037] Furthermore, in step S160, the motion speed Vel of the linkage-type rope-driven flexible robotic arm rope is... l Converted into the speed of the motor Vel θ for:

[0038]

[0039] Vel l Let S be the velocity of the rope in the linked rope-driven flexible robotic arm, S be the lead of the ball screw in the transmission mechanism of the linked rope-driven flexible robotic arm, and n be the reduction ratio of the motor in the linked rope-driven flexible robotic arm.

[0040] Furthermore, step S170 includes:

[0041] The servo driver of the linked rope-driven flexible robotic arm controls the movement speed of the follow motor Vel. θ Rotate.

[0042] The present invention also relates to a computer-readable storage medium storing computer program instructions thereon, which, when executed by a processor, implement the above-described method.

[0043] The technical solution of the present invention also relates to a control system for a linkage-type rope-driven flexible robotic arm, comprising: a drive control box connected in sequence and a plurality of linkage arm segments connected in series. Each linkage arm segment includes several arm rods, joints, and angle sensors disposed inside the arm rods, as well as linkage ropes that connect the joints together to make equal-angle movements. Adjacent arm rods and joints are rotatably connected by bearings. The angle sensors are used to measure the rotation angle between the joints and adjacent arm rods.

[0044] Within the same linkage boom segment, two joints connected in series with one boom are connected by linkage ropes. Any boom in each linkage boom segment is connected to the drive control box by its own drive rope and is driven by the prime mover and transmission mechanism set in the drive control box. The transmission mechanism includes, but is not limited to, a linear mechanism composed of a lead screw and nut, and a rope pulling mechanism composed of a stranded disc. The prime mover includes, but is not limited to, an electric motor.

[0045] A miniature angle data acquisition board is installed inside a hollow section of the boom in one of the linked boom segments. The miniature angle data acquisition board is connected to all angle sensors located nearby inside the boom via a high-speed communication protocol. The miniature angle data acquisition boards in each linked boom segment act as distributed data processing centers and are connected to a processing unit via a bus-type communication method. The processing unit is electrically connected to the miniature angle data acquisition board and includes the aforementioned computer-readable storage medium and a host computer, which is electrically connected to the processing unit.

[0046] The sensor data acquisition method described in this invention is as follows:

[0047] The miniature angle data acquisition board is installed inside a hollow section of the linkage arm. This board connects to all nearby angle sensors via a high-speed communication protocol such as SPI. It periodically acquires and processes data from all angle sensors in the linkage arm. Each miniature angle data acquisition board in each linkage arm acts as a distributed data processing center, connected to a processing unit via a bus-type communication method. This processing unit can be a microcontroller, a PC, or any computing unit that can act as a bus-type communication master station. The bus communication method includes, but is not limited to, CAN, CANOpen, and EtherCat. The processing unit periodically acquires data from each miniature angle data acquisition board through polling and then transmits the data to a host computer. Data exchange between the computer and the host computer can be conducted via wired or wireless communication. After processing by the host computer, the data participates in subsequent control and planning algorithm calculations.

[0048] The beneficial effects of this invention are as follows:

[0049] The present invention proposes a joint sensor data acquisition scheme for a linkage-type rope-driven flexible manipulator, which effectively solves the problems of difficult wiring, large delay, and instability in joint sensor data acquisition under long arms. At the same time, it proposes a closed-loop control method for the linkage-type rope-driven flexible manipulator, which significantly improves the motion accuracy of the linkage-type rope-driven flexible manipulator. Attached Figure Description

[0050] Figure 1 This is a schematic diagram of a mechanism of one embodiment of the linkage-type flexible robotic arm.

[0051] Figure 2 for Figure 1 A schematic diagram of the structure of one linkage arm segment in an embodiment of a medium-linkage flexible robotic arm.

[0052] Figure 3 This is a schematic diagram of sensor data acquisition according to an embodiment of the present invention.

[0053] Figure 4 This is a schematic diagram of the boom according to an embodiment of the present invention.

[0054] Figure 5 This is a control flowchart of a closed-loop control method according to an embodiment of the present invention.

[0055] Figure 6 This is a control block diagram of a closed-loop control method according to an embodiment of the present invention.

[0056] Figure label:

[0057] Arm segment 1, linkage arm segment 110, boom 111, joint 112, angle sensor 113, linkage rope 114, drive rope 115, miniature angle data acquisition board 116, drive control box 2, processing unit 3, host computer 4. Detailed Implementation

[0058] 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 accompanying drawings, so as to fully understand the purpose, solution, and effects of the present invention.

[0059] It should be noted that, unless otherwise specified, when a feature is referred to as "fixed" or "connected" to another feature, it can be directly fixed or connected to the other feature, or indirectly fixed or connected to the other feature. The singular forms "a," "described," and "the" used herein are also intended to include the plural forms, unless the context clearly indicates otherwise. Furthermore, unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art. The terminology used in this specification is for the purpose of describing particular embodiments only and not for limiting the invention. The term "and / or" as used herein includes any combination of one or more of the associated listed items.

[0060] Reference Figures 1 to 4A control system for a linkage-type rope-driven flexible robotic arm includes: a drive control box 2 connected in sequence and multiple linkage arm segments 110 connected in series. Each linkage arm segment 110 includes several arm rods 111, joints 112, and angle sensors 113 disposed inside the arm rods 111. Adjacent arm rods 111 and joints 112 are rotatably connected by bearings. The angle sensors 113 are used to measure the rotation angle between the joints 112 and adjacent arm rods 111. Two joints 112 connected in series with one arm rod 111 within the same linkage arm segment 110 are connected by linkage ropes 114. Any arm rod 111 in each linkage arm segment 110 is connected to the drive control box 2 by its respective drive rope 115 and is driven by a prime mover disposed in the drive control box 2. The mechanism is driven by a machine and a transmission mechanism, including but not limited to a linear mechanism composed of a lead screw and nut and a rope pulling mechanism composed of a stranded disc. The prime mover includes but is not limited to an electric motor. A miniature angle data acquisition board 116 is installed in a hollow arm 111 of one of the linkage arm segments 110. The miniature angle data acquisition board 116 is connected to all the angle sensors 113 located in the arm 111 in a close position through a high-speed communication protocol. The miniature angle data acquisition board 116 in each linkage arm segment 110 acts as a distributed data processing center and is connected to a processing unit 3 through a bus-type communication method. The processing unit 3 is electrically connected to the miniature angle data acquisition board 116. The system also includes a host computer 4, which is electrically connected to the processing unit 3.

[0061] Reference Figures 1 to 2In one embodiment, two joints 112 connected in series with a lever 111 within the same linkage arm segment 110 are linked by a linkage rope 114. When one of the joints 112 rotates, the remaining joints 112 connected to that joint via the linkage rope 114 will theoretically rotate at the same angle in the opposite direction. Therefore, when one joint 112 within a linkage arm segment 110 rotates, all the remaining joints 112 within the linkage arm segment 110 will follow suit and rotate at the same angle. Although theoretically, rotations of the same attribute within the linkage arm segment 110 will be linked at the same angle, in practice, this is limited by the fact that... The uneven pretension of the linkage ropes 114 on the left and right sides of the joint, and the friction and deformation between the linkage ropes 114 and the drive ropes 115, prevent the complete equiangular linkage from being achieved. Therefore, the linkage-type rope-driven flexible robotic arm actually has a certain degree of non-equiangular linkage. Any (or only) arm 111 in each linkage arm segment 110 is connected to the drive control box 2 through its respective drive rope 115 and is driven by the prime mover and transmission mechanism in the drive control box 2. The transmission mechanism includes, but is not limited to, a linear mechanism composed of a lead screw and nut, and a rope pulling mechanism composed of a stranded disc. The prime mover includes, but is not limited to, an electric motor.

[0062] The sensor data acquisition method described in this invention is as follows:

[0063] A miniature angle data acquisition board 116 is installed inside a hollow arm 111 of a linkage arm segment 110. This miniature angle data acquisition board 116 connects to all nearby angle sensors 113 via a high-speed communication protocol such as SPI. The miniature angle data acquisition board 116 periodically acquires and processes data from all angle sensors in the linkage arm segment. Each miniature angle data acquisition board 116 in each linkage arm segment 110 acts as a distributed data processing center and is connected to a processing unit 3 via a bus-type communication method. The processing unit 3 can be a microcontroller, a PC, or any computing unit that can act as a bus-type communication master station. The bus communication method includes, but is not limited to, CAN, CANopen, EtherCat, etc. The processing unit 3 periodically acquires data collected by each miniature angle data acquisition board through polling and then transmits the data to a host computer 4. The processing unit 3 and the host computer 4 can interact via wired or wireless communication. After being processed by the host computer 4, the data participates in the calculation of subsequent control and planning algorithms.

[0064] Based on the structure of the robotic arm described above, some key terms mentioned below are as follows.

[0065] Configuration: In this paper, this refers to the mechanism model of a linkage-type cable-driven flexible robotic arm, which is typically represented by a matrix of the relative angles between each joint 112 and the arm 111 about the axis of rotation. For example... Figure 1 The example of a linkage-type cable-driven flexible robotic arm has 24 degrees of freedom, therefore the configuration angle of the robotic arm is a 24x1 matrix q. real = [q1, q2, ..., q 23 q 24 ] T .

[0066] Actual controllable angle: Within each of the linked arm segments 110 of the linkage-type cable-driven flexible robotic arm, only one joint 112 with two degrees of freedom can actually control the angle. The remaining joints 112 are actually angularly responsive. The actual controllable angle of the linkage-type cable-driven flexible robotic arm can be represented by a matrix of the controllable angles of the linked arm segments 110. For example... Figure 1 The example of a linkage-type flexible robotic arm has 6 controllable degrees of freedom; therefore, the actual controllable angle of the robotic arm is a 6x1 matrix q. c =[q1, q2, q3, q4, q5, q6] T .

[0067] Equivalent joint angle: Since the actual controllable angle within each linkage arm segment 110 of the linkage-type cable-driven flexible robotic arm is only two degrees of freedom of one joint 112, this paper proposes an actual controllable angle that can represent the overall motion state of one linkage arm segment 110, such as... Figure 1 The example linked cable-driven flexible manipulator has 6 controllable degrees of freedom. Therefore, the equivalent joint angle of the linked cable-driven flexible manipulator is a 6x1 matrix q. c =[q 1d q 2d q 3d q 4d q 5d q 6d ] T .

[0068] Drive rope length: refers to the amount by which a set of ropes in the drive control box 2, driven by various sets of motors and transmission mechanisms, extends or contracts. Figure 1 In the example, there are a total of 9 drive ropes, so there are 9 sets of motors and transmission mechanisms. Therefore, the length of the drive rope is a 9x1 matrix: L = |l1, ..., l9|.

[0069] Desired rope length: refers to the length of the rope to achieve the desired target configuration q. d The length of the driving rope corresponding to the time.

[0070] Control cycle: refers to the time elapsed after one control cycle.

[0071] Reference Figure 3 In some embodiments, the data acquisition method of the angle sensor 113 in this invention is as follows:

[0072] The miniature angle data acquisition board 116 is installed in the second arm 111 from bottom to top in each of the linked arm segments 110. The miniature angle data acquisition board 116 is connected to all the angle sensors 113 in the nearby position via the high-speed communication protocol SPI. The miniature angle data acquisition board 116 periodically collects and processes the data of all angle sensors in the linked arm segments 110. The miniature angle data acquisition board 116 in each of the linked arm segments 110 of the arm segment 1 serves as a distributed data processing center and is connected to the processing unit 3 via a bus communication method. In one embodiment, a microcontroller is used as the processing unit 3. The bus communication method includes, but is not limited to, CAN, CANOpen, EtherCat, etc. In this embodiment, the CAN bus is used as the bus communication method. The processing unit 3 periodically acquires the data collected by each of the micro angle data acquisition boards 116 through polling, and then transmits the data to the host computer 4. The processing unit 3 and the host computer 4 can interact with each other through wired or wireless communication. In this embodiment, Bluetooth communication is used for data transmission. After being processed by the host computer 4, the data participates in the subsequent control and planning algorithm calculations.

[0073] Another aspect of the technical solution of the present invention relates to a closed-loop motion control system for a linkage-type flexible robotic arm, which includes a motion controller integrating the kinematic mode program of the linkage-type rope-driven flexible robotic arm, a servo motor driver, and a controller containing the above-mentioned method program. The controller can be any programmable controller, including microcontrollers, PCs, PLCs, etc.

[0074] The processing unit 3 runs an application program to implement the method according to the invention.

[0075] Reference Figures 5 to 6 In some embodiments, the method according to the present invention includes the following steps:

[0076] S110. Obtain the desired target configuration q of the linked rope-driven flexible manipulator at the next moment. d ;

[0077] S120. Obtain all angle sensor data q of the linkage-type rope-driven flexible robotic arm. real ;

[0078] S130. Calculate the equivalent joint rotation angle q of each of the aforementioned linked arm segments at the current moment. eq The equivalent joint rotation angle q eqThe angle that best reflects the overall motion state of the linked arm segment;

[0079] S140, The desired target configuration q d The equivalent joint angle q of each of the aforementioned linkage arm segments eq The difference is used as the input to the joint closed-loop control, and the PID output of the joint closed-loop control is calculated. out ;

[0080] S150, PID the closed-loop output of the joint out Converted to Vel of motion of the rope in a linkage-type rope-driven flexible robotic arm l ;

[0081] S160, the motion speed Vel of the linkage-type rope-driven flexible robotic arm rope is... l This is converted into the speed of the motor.

[0082] S170, Control the motor to follow the motor's movement speed;

[0083] S180. Determine the configuration q of the current linkage-type cable-driven flexible robotic arm. cur Is it the desired configuration q of the final target? tar Otherwise, return to S110 to enter the next control cycle; otherwise, end the control.

[0084] The procedure prior to step S110 also includes:

[0085] S101. Obtain the current configuration q of the linkage-type cable-driven flexible robotic arm based on the joint angle sensor. cur And obtain the final target desired configuration q based on user input. tar , configuration q cur and the final target desired configuration q tar The input is fed into a trajectory planner, which includes, but is not limited to, a quintic spline planner, a cubic spline planner, and a trapezoidal planner;

[0086] S102. Obtain a trajectory by acquiring a series of configuration state points that change over time, output by the trajectory planner, and adjust the trajectory from the current configuration q of the linkage-type cable-driven flexible robotic arm according to the control period T. cur To the final target desired configuration q tar Discretized into a series of trajectory points, the current configuration q of the linkage-type cable-driven flexible manipulator... cur The target desired trajectory configuration q of the linked rope-driven flexible manipulator is the first of a series of trajectory state points. tarThe last of a series of trajectory state points described by the joint rotation angles of the linkage-type rope-driven flexible manipulator, wherein the joint rotation angle is the relative angle between adjacent joints and the arm about the axis of rotation of the linkage-type rope-driven flexible manipulator.

[0087] Detailed Implementation of Step S110

[0088] The trajectory planner obtains the configuration-time curve of the linkage-type cable-driven flexible manipulator, thereby obtaining the desired configuration for each control cycle T. The trajectory planner is a cubic spline planner.

[0089] Assume the trajectory configuration of the robotic arm is as follows:

[0090]

[0091] t tar = [t1, t2, ..., t n [ ] is the time matrix associated with the trajectory configuration, serving as the time data for the motion trajectory planner.

[0092] In the subinterval t∈[t i , t i+1 In the figure, the interpolated configuration time motion curve is as follows:

[0093] q d =a i +b i (t d -t i )+c i (t d -t i ) 2 +d i (t d -t i ) 3

[0094] in,

[0095] a i =q i

[0096]

[0097]

[0098]

[0099] Among them, t d For the desired configuration q d The location of the moment.

[0100] Detailed Implementation of Step S130

[0101] Calculate the equivalent joint rotation angle q of each of the linked arm segments at the current moment. eq The equivalent joint rotation angle q eq To best reflect the overall motion state of the linked arm segment, in this embodiment, the angle that most closely approximates the overall motion state of the linked arm segment should be used as the equivalent joint rotation angle to replace the motion effect of the entire segment. The equivalent joint rotation angle can be the weighted average of all rotation angles within the linked arm segment, or it can be obtained through neural network training. In one embodiment, the equivalent joint rotation angle q... eq The equivalent joint angle is the weighted average of all pitch angles or the average of all yaw angles within the linked arm segment. In one embodiment, using the average of all pitch angles as the equivalent pitch angle and the average of all yaw angles as the equivalent yaw angle has been verified to yield good results. This embodiment uses the arithmetic mean of angles of the same attribute within the linked arm segment as the equivalent joint angle. It should be noted that the equivalent joint angle selected in this embodiment is the optimal value under experimental conditions. Other equivalent joint angles, such as the weighted average of each joint angle, are applicable to this patent as long as they reflect the overall motion effect of the joint segment.

[0102] Detailed Implementation of Step S140

[0103] The desired target configuration q d The equivalent joint angle q of each of the aforementioned linkage arm segments eq The difference Δq is used as the input to the joint closed-loop control, and the joint closed-loop output PID is calculated. out ,include,

[0104]

[0105] Where, Δq=q d -q eq K p K d and K i All are constants.

[0106] Detailed Implementation of Step S150

[0107] The joint closed-loop output PID out This is further converted into the motion velocity VeI of the linkage-type rope-driven flexible robotic arm rope. l ,include,

[0108] Vel l =(l d -l eq ) / T

[0109] Among them, Vel lLet l be the motion velocity of the rope of each of the linked rope-driven flexible robotic arms at that moment. d To determine the desired rope length for control at the next moment, l eq The current rope length is T, where T is the control period, and the desired rope length for the next control moment is l. d The PID output of the joint closed loop out The desired control position q at the next moment is calculated using the control period T. c =PIDout*T is then calculated based on the kinematic model of joint angle to rope length.

[0110] Detailed Implementation of Step S160

[0111] The velocity of the rope in the linked rope-driven flexible robotic arm is Vel l Converted into the speed of the motor Vel θ for:

[0112]

[0113] Vel l S is the motion speed of the rope in the linked rope-driven flexible robotic arm, S is the lead of the ball screw in the transmission mechanism of the linked rope-driven flexible robotic arm, n is the reduction ratio of the motor of the linked rope-driven flexible robotic arm, and the motor servo driver of the linked rope-driven flexible robotic arm controls the motor of the linked rope-driven flexible robotic arm to follow the current control motion speed.

[0114] Detailed Implementation of Step S170

[0115] The motor movement, controlled by a closed-loop motor speed control system, follows the converted motor speed input and includes:

[0116] The servo driver of the linked rope-driven flexible robotic arm controls the movement speed of the follow motor Vel. θ Rotate.

[0117] It should be understood that the method steps in all embodiments of the present invention can be implemented or carried out by computer hardware, a combination of hardware and software, or by computer instructions stored in a non-transitory computer-readable storage medium. The methods 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 necessary, the program can be implemented in assembly or machine language. In any case, the language can be a compiled or interpreted language. Furthermore, for this purpose, the program can run on a programmed application-specific integrated circuit (ASIC).

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

[0119] Furthermore, the method can be implemented in any suitable type of computing platform, including but not limited to personal computers, minicomputers, mainframes, workstations, networked or distributed computing environments, standalone or integrated computer platforms, or in communication with charged particle tools or other imaging devices, etc. Aspects of the 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, optical read and / or write storage medium, RAM, ROM, etc., such that it is readable 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. Furthermore, the machine-readable code, or portions thereof, can be transmitted via wired or wireless networks. The invention described herein includes these and other different types of non-transitory computer-readable storage media when such media comprises instructions or programs that implement the steps described above in conjunction with a microprocessor or other data processor. When programmed according to the methods and techniques described in the invention, the invention may also include the computer itself.

[0120] A computer program can be applied to input data to perform the functions described herein, thereby transforming the input data to generate output data stored in 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 invention, the transformed data represents physical and tangible objects, including specific visual depictions of physical and tangible objects generated on the display.

[0121] The above description is merely a preferred embodiment of the present invention. The present invention is not limited to the above-described embodiments. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention, as long as they achieve the technical effects of the present invention by the same means, should be included within the scope of protection of the present invention. Within the scope of protection of the present invention, the technical solutions and / or implementation methods can have various modifications and variations.

Claims

1. A closed-loop control method for a linkage-type rope-driven flexible robotic arm, used to control the linkage-type rope-driven flexible robotic arm to perform work tasks, characterized in that, The method includes the following steps: S110. Obtain the desired target configuration of the linked rope-driven flexible manipulator at the next moment. ; S120. Obtain all angle sensor data of the linkage-type rope-driven flexible robotic arm. ; S130. Calculate the equivalent joint rotation angle of each linkage arm segment at the current moment. ; S140, Configure the desired target configuration Equivalent joint angles of each of the aforementioned linkage arm segments The difference is used as the input for joint closed-loop control, and the joint closed-loop output is calculated. ; S150, The joint closed-loop output amount Converted into the motion velocity of the rope in a linkage-type rope-driven flexible robotic arm ; S160, The motion speed of the linkage-type rope-driven flexible robotic arm rope is... This is converted into the speed of the motor. S170, Control the motor to follow the motor's movement speed; S180. Determine the configuration of the current linkage-type cable-driven flexible robotic arm. Is this the desired configuration for the final target? Otherwise, return to S110 to enter the next control cycle; otherwise, end the control.

2. The method according to claim 1, wherein, The procedure before step S110 also includes: S101. Obtain the current configuration of the linkage-type cable-driven flexible robotic arm based on the joint angle sensor. And obtain the final target desired configuration based on user input. The configuration and the desired configuration of the final target The input is fed into a trajectory planner, which includes, but is not limited to, a quintic spline planner, a cubic spline planner, and a trapezoidal planner; S102. Obtain a trajectory by acquiring a series of configuration state points that change over time, output by the trajectory planner, and adjust the trajectory according to the control period T from the current configuration of the linkage-type cable-driven flexible robotic arm. To the final target configuration Discretized into a series of trajectory points, the current configuration of the linkage-type cable-driven flexible manipulator. The target desired trajectory configuration of the linked cable-driven flexible manipulator is the first of a series of trajectory state points. The last of a series of trajectory state points described by the joint rotation angles of the linkage-type rope-driven flexible manipulator, wherein the joint rotation angle is the relative angle between adjacent joints and the arm about the axis of rotation of the linkage-type rope-driven flexible manipulator.

3. The method according to claim 2, wherein, Step S110 includes: The trajectory planner obtains the configuration-time curve of the linkage-type cable-driven flexible manipulator, thereby obtaining the desired configuration for each control cycle T. The trajectory configuration of the linkage-type cable-driven flexible robotic arm is as follows: ； The time matrix associated with the trajectory configuration, In subinterval In the interpolated configuration time motion curve, the following is: ； in, ； ； ； ； in, For the desired configuration At what moment, It is the joint position at the i-th trajectory configuration point. It is the joint position at the (i+1)th trajectory configuration point. It is the velocity at the i-th trajectory configuration point, i.e. , It is the velocity at the (i+1)th trajectory configuration point, i.e. , It is the time length of the i-th segment, i.e. .

4. The method according to claim 1, wherein, In step S130, the equivalent joint rotation angle of each of the linked arm segments at the current moment is calculated. ,include, The equivalent joint angle It is the weighted average of all pitch angles or the average of all yaw angles of each joint within the aforementioned linkage boom segment.

5. The method according to claim 1, wherein, Step S140: The desired target configuration is... Equivalent joint angles of each of the aforementioned linkage arm segments The difference As the input to the joint closed-loop control, the joint closed-loop output is calculated. ,include, ； in, , , and All are constants.

6. The method according to claim 1, wherein, In step S150, the joint closed-loop output amount is... This is further converted into the motion velocity of the rope of the linked rope-driven flexible robotic arm. ,include, ； in, The velocity of the ropes in each of the linked rope-driven flexible robotic arms at that moment. To control the desired rope length in the next moment, The current rope length is T, where T is the control period, and the desired rope length for the next control moment is T. Output of the joint closed loop The desired control position at the next moment is calculated using the control period T. It is then calculated based on the kinematic model of joint angle to rope length.

7. The method according to claim 1, wherein, In step S160, the motion speed of the linkage-type rope-driven flexible robotic arm rope is measured. Converted into the speed of the motor for: ； in Let S be the velocity of the rope in the linked rope-driven flexible robotic arm, S be the lead of the ball screw in the transmission mechanism of the linked rope-driven flexible robotic arm, and n be the reduction ratio of the motor in the linked rope-driven flexible robotic arm.

8. The method according to claim 1, wherein, Step S170 includes: The servo driver of the linked rope-driven flexible robotic arm controls the movement speed of the follow motor of the servo motor. Rotate.

9. A computer-readable storage medium having stored thereon program instructions that, when executed by a processor, perform the method as described in any one of claims 1 to 8.

10. A control system for a linkage-type rope-driven flexible robotic arm, characterized in that, The control system of the linkage-type rope-driven flexible robotic arm includes a drive control box connected in sequence and multiple linkage arm segments connected in series. Each linkage arm segment includes several arm rods, joints, angle sensors installed inside the arm rods, and linkage ropes that connect the joints together to make equal-angle movements. Adjacent arm rods and joints are rotatably connected by bearings. The angle sensors are used to measure the rotation angle between the joints and adjacent arm rods. Within the same linkage boom segment, two joints connected in series with one boom are connected by linkage ropes. Any boom in each linkage boom segment is connected to the drive control box by its own drive rope and is driven by the prime mover and transmission mechanism set in the drive control box. The transmission mechanism includes, but is not limited to, a linear mechanism composed of a lead screw and nut, and a rope pulling mechanism composed of a stranded disc. The prime mover includes, but is not limited to, an electric motor. A miniature angle data acquisition board is installed inside a hollow section of the boom in one of the linked boom segments. The miniature angle data acquisition board is connected to all angle sensors located nearby inside the boom via a high-speed communication protocol. The miniature angle data acquisition boards in each of the linked boom segments act as distributed data processing centers and are connected to a processing unit via a bus-type communication method. The processing unit includes a computer-readable storage medium as described in claim 9 and a host computer, which is electrically connected to the processing unit.

Images