A post-failure planning method for a rope-driven super-redundant manipulator

Abstract

The present invention relates to a post-failure planning method for a rope-driven super-redundant manipulator, the method comprising the following steps: S100, when one of the driving ropes is broken, the manipulator joint corresponding to the broken driving rope portion is pulled to a mechanical limit by using the remaining driving rope; S200, according to the mechanical limit bending angle between the first joint panel and the second joint panel, the rope length driving amount of the remaining two driving ropes is calculated respectively, and the manipulator joint is controlled to move to the limit configuration; S300, the driving amount of the remaining driving rope is calculated, and the manipulator is controlled to move to the corresponding limit configuration; S400, according to the angle of the limit configuration, the angle of the joint segment where the broken driving rope is located is locked, and the speed-level Jacobi pseudo-method is used to solve the next planning task. The post-failure planning method for a rope-driven super-redundant manipulator disclosed by the present invention uses the remaining driving rope to pull the joint to the mechanical limit, and the remaining joints can complete the manipulator trajectory planning and the execution of the manipulator task.

Description

Technical Field

[0001] The invention relates to a planning method and system for a rope-driven super-redundant mechanical arm after a failure, and belongs to the field of robot control. Background Art

[0002] Rope-driven flexible manipulators have the advantages of slender arms, electromechanical separation, and multiple degrees of freedom. They have been widely used in the fields of medical health, nuclear power, and aircraft engine maintenance. Rope-driven serial super-redundant manipulators generally use manipulator joints connected by cross axes. Each manipulator joint has two degrees of freedom. The manipulator joints between the manipulators are usually driven by three driving ropes. However, due to possible failures such as the breakage of the driving rope, some joints of the manipulator fail or lose control, affecting the performance of the rope-driven super-redundant manipulator and even making it unable to continue working. Therefore, a new planning method is needed to solve the above problems. Summary of the invention

[0003] The present invention provides a post-failure planning method and system for a rope-driven super-redundant manipulator, aiming to solve at least one of the technical problems existing in the prior art.

[0004] The technical solution of the present invention relates to a post-failure planning method for a rope-driven super-redundant manipulator, comprising: a plurality of arm rods, connecting the manipulator joints of the adjacent arm rods, the manipulator joints comprising a first joint panel and a second joint panel which are arranged correspondingly, the first joint panel and the second joint panel are connected by a cross axis with multiple degrees of freedom, a plurality of first drive rope through holes are arranged around the first joint panel, a plurality of second drive rope through holes are arranged around the second joint panel, a plurality of drive ropes pass through a plurality of pairs of first drive rope through holes and second drive rope through holes in sequence, and the method comprises the following steps:

[0005] S100, when one of the driving ropes is broken, the mechanical arm joint corresponding to the broken driving rope portion is pulled to a mechanical limit position by using the remaining driving rope;

[0006] S200, according to the mechanical limit bending angle between the first joint panel and the second joint panel, combined with the equivalent bending plane direction angle, then respectively calculate the rope length driving amount of the remaining two driving ropes, and control the robot arm joint to move to the limit configuration;

[0007] S300, calculating the driving amount of the remaining driving rope, and controlling the robot arm to move to a corresponding limit configuration;

[0008] S400. Lock the angle of the joint segment where the broken drive rope is located according to the angle of the extreme configuration, use the speed-level Jacobi pseudo-method to solve the next planning task, and analyze whether the task is reachable under the fault configuration. If it is not reachable or there is environmental interference, further adjust the posture of the robot arm and solve it again until the remaining robot arm joints complete the execution of the robot arm trajectory planning task.

[0009] Further, the step S100 includes: in the robot arm joint corresponding to the broken driving rope portion, the following relationship is satisfied:

[0010] β d =β1+λ(β2-β1)

[0011] Wherein, β1 and β2 are the first angle and the second angle between the first driving rope through hole and the second driving rope through hole and the horizontal plane respectively corresponding to the remaining two driving ropes, β d is the equivalent bending plane direction angle, and, β d The first driving rope through hole and the second driving rope through hole corresponding to the remaining two driving ropes are located between them.

[0012] Further, the step S200 includes: a first connecting rod is provided on one side of the first joint panel, and a second connecting rod is provided on one side of the second joint panel, the first connecting rod and the second connecting rod are hingedly connected by a center block, and a first central axis and a second central axis pass through the center block and are perpendicular to each other, wherein, in the initial state of the robot arm joint, the angle in the direction of the equivalent bending plane and the angle of the limit bending angle are both zero, and the Z axis and Y axis of the coordinate system of the first connecting rod, the center block and the second connecting rod are parallel to the first central axis and the second central axis, respectively.

[0013] Further, the step S200 includes: the first link, the central block and the second link are defined as coordinate systems {i,1}, {i,0} and {i,2} respectively, the first medial axis and the second medial axis are defined as ξ1 and ξ2 respectively, and the homogeneous transformation matrix from the coordinate system {i,1} to {i,0} meets the following relationship,

[0014] i,1 T i,0 =Trans(d,0,0)Rot(Z,θ i,1 )

[0015] Where d = O i,0 O i,1 =O i,0 O i,2 , Trans() is the function of the robot arm joint translation.

[0016] Further, the step S200 includes:

[0017] S210, the homogeneous transformation matrix from the coordinate system {i,0} to {i,2} conforms to the following relationship,

[0018] i,0 T i,2 =Rot(Y,θ i,2 )Trans(d,0,0)

[0019] Among them, Rot() is the function of the robot arm joint translation.

[0020] Further, the step S200 includes:

[0021] S220, transform the coordinate system {i,1} to {i,2} according to the following formula:

[0022] i,1 T i,2 = i,1 T i,0 i,0 T i,2

[0023] Among them, from the coordinate system {i,1} to {i,2}, the robot arm joint undergoes at least three rotations, which conforms to the following relationship:

[0024] i,1 T i,2′ =Rot(X,β d )Rot(ξ e ,θ l )Rot(X,-β d′ )

[0025] Among them, β d′ is the auxiliary rotation angle of the robot joint; ξ e is the resultant axis in the manipulator joint that is perpendicular to the bending plane; θ l is the bending angle of the robot joint.

[0026] Further, the step S200 includes:

[0027] S230, when the coordinate systems {i,1} and {i,2} are consistent, the following relationship is obtained:

[0028] i,1 T i,2′ = i,1 T i,2

[0029] Then we get the following relationship:

[0030]

[0031]

[0032] Among them, sinβ d ≠0, sinβ d =0,

[0033] When β d ,θ l When is a known value, the robot arm joint is calculated by the following formula:

[0034]

[0035] Further, the step S300 includes: calculating the driving amounts of the remaining two driving ropes by:

[0036]

[0037] Among them, f k is the driving amount of the two driving ropes, and k is the number of the two driving ropes.

[0038] Furthermore, the first joint panel and the second joint panel are connected by a cross axis with two degrees of freedom, three drive ropes pass through multiple pairs of first drive rope through holes and second drive rope through holes in sequence, and the head end and the tail end of the robotic arm are respectively connected to a drive box and a robotic arm end effector.

[0039] A computer-readable storage medium stores program instructions, and the program instructions implement the above method when executed by a processor.

[0040] The beneficial effects of the present invention are as follows.

[0041] The above-mentioned rope-driven flexible robotic arm failure: When the driving rope of the rope-driven flexible robotic arm encounters possible breakage or other failures, causing some joints of the robotic arm to fail or lose control, the remaining driving rope is used to pull the joints to the mechanical limit, forming force closure and balance, thereby achieving locking of the robotic arm joints. The remaining joints can be used to complete the robotic arm trajectory planning and the execution of robotic arm tasks. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] Figure 1 It is an overall flow chart of the post-failure planning method of the rope-driven super-redundant manipulator according to the present invention.

[0043] Figure 2 Schematic diagram of the connection of the robot arm according to an embodiment of the present invention.

[0044] Figure 3 Schematic diagram of a rope-driven super-redundant manipulator according to an embodiment of the present invention.

[0045] Figure 4 Schematic diagram of the structure of the robot arm joint in the straight state according to an embodiment of the present invention.

[0046] Figure 5 Schematic diagram of the structure of the working state of the robot arm joint according to an embodiment of the present invention.

[0047] Figure 6 It is a schematic diagram of the structure in which one of the driving ropes of a robot arm joint is broken according to an embodiment of the present invention.

[0048] Figure 7 Schematic diagram of the distribution of rope holes of a driving rope according to an embodiment of the present invention.

[0049] Figure 8 Schematic diagram of a robot arm joint coordinate system according to an embodiment of the present invention.

[0050] Fig. 9 It is a general kinematic geometric model corresponding to the joints of the robot arm in the embodiment of the present invention.

[0051] Fig.10 It is an equivalent geometric model corresponding to the joint of the robotic arm in the embodiment of the present invention. DETAILED DESCRIPTION

[0052] The concept, specific structure and technical effects of the present invention will be clearly and completely described below in combination with the embodiments and drawings to fully understand the purpose, scheme and effect of the present invention.

[0053] 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 the 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 commonly understood by those skilled in the art. The terms used in this specification are intended only to describe specific embodiments and are not intended to limit the invention. The term "and / or" used herein includes any combination of one or more of the related listed items.

[0054] It should be understood that, although the term first, second, third etc. may be adopted to describe various elements in the present disclosure, these elements should not be limited to these terms. These terms are only used to distinguish the same type of elements from each other. For example, without departing from the scope of the present disclosure, the first element may also be referred to as the second element, and similarly, the second element may also be referred to as the first element. The use of any and all examples or exemplary language ("for example", "such as" etc.) provided herein is only intended to better illustrate embodiments of the present invention, and unless otherwise required, the scope of the present invention will not be limited.

[0055] Reference Figures 1 to 10In some embodiments, the present invention discloses a post-failure planning method for a rope-driven hyper-redundant manipulator, comprising:

[0056] Reference Figure 2 Multiple arm rods are connected to the mechanical arm joints 20 of the adjacent arm rods. In the figure, there are 6 mechanical arm joints 20 combined with the arm rods. Specifically, refer to Figures 4 to 5 The robotic arm joint 20 includes a first joint panel 21 and a second joint panel 22 that are correspondingly arranged. The first joint panel 21 and the second joint panel 22 are connected by a cross axis 23 with multiple degrees of freedom. A plurality of first drive rope through holes are arranged around the first joint panel 21, and a plurality of second drive rope through holes are arranged around the second joint panel 22.

[0057] Specifically, the robotic arm also includes the following structure, wherein the first joint panel 21 and the second joint panel 22 are connected by a cross axis 23 having at least two degrees of freedom, and three drive ropes 30 pass through multiple pairs of first drive rope through holes and second drive rope through holes in sequence, and the head end and the tail end of the robotic arm are respectively connected to the drive box 10 and the robotic arm end effector 40.

[0058] Reference Figure 4 , a plurality of driving ropes 30 pass through a plurality of pairs of first driving rope through holes and second driving rope through holes in sequence. Specifically, the robot arm joint 20 in the figure has three driving ropes 30 passing through.

[0059] Reference Figure 1 The method comprises the following steps:

[0060] S100: When one of the driving ropes breaks, the remaining driving ropes are used to pull the robot arm joint corresponding to the broken driving rope portion to a mechanical limit.

[0061] S200, according to the mechanical limit bending angle between the first joint panel and the second joint panel, combined with the equivalent bending plane direction angle, then respectively calculate the rope length driving amount of the remaining two driving ropes, and control the robot arm joint movement to the limit configuration.

[0062] S300, calculating the driving amount of the remaining driving rope, and controlling the robot arm to move to the corresponding limit configuration.

[0063] S400. Lock the angle of the joint segment where the broken drive rope is located according to the angle of the extreme configuration, use the speed-level Jacobi pseudo-method to solve the next planning task, and analyze whether the task is reachable under the fault configuration. If it is not reachable or there is environmental interference, further adjust the posture of the robot arm and solve it again until the remaining robot arm joints complete the execution of the robot arm trajectory planning task.

[0064] Further implementation of step S100

[0065] Reference Figure 5 In the robot arm joint 20, assuming that one of the driving ropes 30 is broken and missing, the remaining two driving ropes 30 in the robot arm joint 20 pull the first joint panel 21 to reach the position as shown in FIG. Figure 6 Mechanical limit. In the mechanical arm joint corresponding to the broken drive rope part, the following relationship is met:

[0066] β d =β1+λ(β2-β1)

[0067] Wherein, β1 and β2 are the first angle and the second angle between the first driving rope through hole and the second driving rope through hole and the horizontal plane respectively corresponding to the remaining two driving ropes, β d is the equivalent bending plane direction angle, and, β d The first driving rope through hole and the second driving rope through hole corresponding to the remaining two driving ropes are located between them.

[0068] Further implementation of step S200

[0069] Reference Figure 7 A schematic diagram of the distribution of the driving rope holes is shown in FIG. 2 , wherein a first connecting rod 211 is provided on one side of the first joint panel 21, and a second connecting rod 221 is provided on one side of the second joint panel 22. The first connecting rod 211 and the second connecting rod 221 are hingedly connected by a central block, and pass through a first central axis ξ1 and a second central axis ξ2 of the central block which are perpendicular to each other.

[0070] Among them, in the initial state of the robot arm joint, the angle of the equivalent bending plane direction and the extreme bending angle are both zero, and the Z axis and Y axis of the coordinate system of the first link, center block and second link are parallel to the first central axis ξ1 and the second central axis ξ2 respectively.

[0071] Specifically, the first connecting rod 211, the central block and the second connecting rod 221 define coordinate systems as {i, 1}, {i, 0} and {i, 2}, respectively, and the first central axis and the second central axis are defined as ξ1 and ξ2, respectively.

[0072] The above-mentioned mechanical arm joint 20 is in a mechanical limit position, corresponding to Fig. 9 The general kinematic geometry model then corresponds to Fig.10 The equivalent geometric model of the coordinate system {i,1} to {i,0} is a homogeneous transformation matrix that conforms to the following relationship:

[0073] i,1 T i,0 =Trans(d,0,0)Rot(Z,θ i,1 )

[0074] Reference Fig. 9 , where d = O i,0 O i,1 =O i,0 O i,2 , Trans() is the function of the robot arm joint translation.

[0075] The step S200 further includes:

[0076] S210, the homogeneous transformation matrix from the coordinate system {i,0} to {i,2} conforms to the following relationship,

[0077] i,0 T i,2 =Rot(Y,θ i,2 )Trans(d,0,0)

[0078] Among them, Rot() is the function of the robot arm joint translation.

[0079] S220, transform the coordinate system {i,1} to {i,2} according to the following formula:

[0080] i,1 T i,2 = i,1 T i,0 i,0 T i,2

[0081] Among them, from the coordinate system {i,1} to {i,2}, the robot arm joint undergoes at least three rotations, which conforms to the following relationship:

[0082] i,1 T i,2′ =Rot(X,β d )Rot(ξ e ,θ l )Rot(X,-β d′ )

[0083] Among them, β d′ is the auxiliary rotation angle of the robot joint; ξ e is the resultant axis in the manipulator joint that is perpendicular to the bending plane; θ l is the bending angle of the robot joint.

[0084] S230, when the coordinate systems {i,1} and {i,2} are consistent, the following relationship is obtained:

[0085] i,1 T i,2′ = i,1 T i,2

[0086] Then we get the following relationship:

[0087]

[0088] Among them, sinβ d ≠0, sinβ d =0,

[0089] When β d ,θ l When is a known value, the robot arm joint is calculated by the following formula:

[0090]

[0091] Further implementation of step S300

[0092] Reference Figures 6 to 10 , calculate the driving amount of the remaining two driving ropes by the following method,

[0093]

[0094] Among them, f k is the driving amount of the two driving ropes, and k is the number of the two driving ropes.

[0095] The above solution is also applicable to the super-redundant segmented linkage arm.

[0096] It should be appreciated that the method steps in the embodiments of the present invention can be implemented or implemented 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 process or object-oriented programming language to communicate with a computer system. However, if necessary, the program can be implemented in an assembly or machine language. In any case, the language can be a compiled or interpreted language. In addition, the program can be run on a programmed ASIC for this purpose.

[0097] Furthermore, the operations of the processes described herein may be performed in any suitable order unless otherwise indicated herein or otherwise clearly contradicted by 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 may be executed by one or more processors.

[0098] Further, the method 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 in 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, a RAM, a 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 process described herein. In addition, the machine-readable code, or portions thereof, can be transmitted via 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.

[0099] The computer program can be applied to input data to perform the functions described herein, thereby converting the input data to generate output data 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 physical and tangible objects, including specific visual depictions of physical and tangible objects produced on the display.

[0100] The above is only a preferred embodiment of the present invention. The present invention is not limited to the above implementation. As long as the technical effect of the present invention is achieved by the same means, any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the scope of protection of the present invention. Within the scope of protection of the present invention, its technical scheme and / or implementation method may have various modifications and changes.

Claims

1. A post-failure planning method for a rope-driven super-redundant manipulator, comprising: Multiple booms, A mechanical arm joint connecting adjacent arm rods, wherein the mechanical arm joint includes a first joint panel and a second joint panel that are correspondingly arranged, wherein the first joint panel and the second joint panel are connected via a cross axis having multiple degrees of freedom, wherein a plurality of first drive rope through holes are arranged around the first joint panel, and a plurality of second drive rope through holes are arranged around the second joint panel, A plurality of driving ropes are sequentially passed through a plurality of pairs of first driving rope through holes and second driving rope through holes, wherein the method comprises the following steps: S100, when one of the driving ropes is broken, the mechanical arm joint corresponding to the broken driving rope portion is pulled to a mechanical limit position by using the remaining driving rope; S200, according to the mechanical limit bending angle between the first joint panel and the second joint panel, combined with the equivalent bending plane direction angle, then respectively calculate the rope length driving amount of the remaining two driving ropes, and control the robot arm joint to move to the limit configuration; S300, calculating the driving amount of the remaining driving rope, and controlling the robot arm to move to a corresponding limit configuration; S400. Lock the angle of the joint segment where the broken drive rope is located according to the angle of the extreme configuration, use the speed-level Jacobi pseudo-method to solve the next planning task, and analyze whether the task is reachable under the fault configuration. If it is not reachable or there is environmental interference, further adjust the posture of the robot arm and solve it again until the remaining robot arm joints complete the execution of the robot arm trajectory planning task.

2. The method according to claim 1, wherein: The step S100 includes: In the robot arm joint corresponding to the broken drive rope part, the following relationship is met: b d =β1+λ(β2-β1); Wherein, β1 and β2 are the first angle and the second angle between the remaining two driving ropes corresponding to the first driving rope through hole and the second driving rope through hole and the horizontal plane, respectively. βd is the equivalent bending plane direction angle, and, βd The first driving rope through hole and the second driving rope through hole corresponding to the remaining two driving ropes are located between them.

3. The method according to claim 1, wherein: The step S200 includes: A first connecting rod is provided on one side of the first joint panel, and a second connecting rod is provided on one side of the second joint panel. The first connecting rod and the second connecting rod are hingedly connected by a central block, and pass through a first central axis and a second central axis perpendicular to each other of the central block. Among them, in the initial state of the robot arm joint, the angle of the equivalent bending plane direction and the limit bending angle are both zero, and the Z axis and Y axis of the coordinate system of the first link, the center block and the second link are parallel to the first central axis and the second central axis respectively.

4. The method according to claim 1, wherein: The first joint panel and the second joint panel are connected by a cross axis with two degrees of freedom, and three driving ropes pass through multiple pairs of first driving rope through holes and second driving rope through holes in sequence. The head end and the tail end of the robot arm are respectively connected to the driving box and the robot arm end effector.

5. A computer-readable storage medium having program instructions stored thereon, wherein the program instructions implement the method according to any one of claims 1 to 3 when executed by a processor.

Images