Rigidity control algorithm and system for variable-rigidity rope-driven anthropomorphic mechanical arm

Abstract

The invention provides a rigidity control algorithm and system for a variable-rigidity rope-driven anthropomorphic mechanical arm, and relates to the field of rope-driven anthropomorphic mechanical arms. The algorithm comprises the steps that the relation between the length variation of a driving rope and the rotation angle of a small arm is determined; establishing a driving rope rigidity-driving rope length variation relationship among the rigidity of the variable rigidity module, the rigidity of the driving rope and the length variation of the driving rope; according to the relation between the rigidity of the driving rope and the length variation of the driving rope, the relation between the length variation of the driving rope and the rotation angle of the forearm and the current length variation of the driving rope, the overall rigidity of the elbow assembly is determined; the rigidity of the rope-driven anthropomorphic mechanical arm is improved. The rigidity modeling method is provided for the rope-driven anthropomorphic mechanical arm with the variable-rigidity elbow assembly, the rigidity of the elbow assembly is deduced and determined, the rigidity of the elbow assembly can be changed by adjusting the length of the driving rope, the rigidity of the mechanical arm can be actively adjusted to the proper rigidity according to different task scenes, and the rigidity of the mechanical arm can be changed. And the method is suitable for wider application scenes.

Description

Technical Field

[0001] The present invention relates to the technical field of rope-driven humanoid manipulators, and in particular to a stiffness control algorithm and system of a variable-stiffness rope-driven humanoid manipulator. Background Art

[0002] With the rapid development of robotics technology, rope-driven manipulators have been widely used in many fields due to their advantages of light weight, high flexibility, and flexible movement. Compared with traditional rigid manipulators, rope-driven manipulators show higher safety in situations where they interact with people because they can generate less impact force when in contact.

[0003] In practical applications, if the stiffness of the rope-driven manipulator is too large, it may cause unpredictable damage to the operator in the event of an unexpected situation. If the stiffness of the rope-driven manipulator is too low, its low stiffness may also become a factor limiting its application scope in certain application scenarios that require precise control and protection, such as medical assistance and space exploration.

[0004] At present, some researchers have conducted preliminary explorations on the stiffness of rope-driven manipulators. For example, the team of Professor Kim, Yong Jae in South Korea has conducted in-depth research on the structure and control of 7-DOF rope-driven manipulators (Reference: Kim Y J. Anthropomorphic Low-Inertia High-Stiffness Manipulator for High-Speed ​​Safe Interaction [J]. IEEE Transactions on Robotics, 2017, PP (6): 1-17.). Many domestic researchers have also conducted extensive research on the structure and control of similar 7-DOF rope-driven manipulators. However, no researchers have conducted relevant research on variable stiffness rope-driven manipulators so far, and there is also a research gap in the variable stiffness control algorithm for variable stiffness rope-driven manipulators. Summary of the invention

[0005] To solve the above problems, an embodiment of the present invention provides a stiffness control algorithm for a variable stiffness rope-driven anthropomorphic manipulator, wherein the rope-driven anthropomorphic manipulator comprises a shoulder assembly, an elbow assembly, and a wrist assembly, wherein the elbow assembly comprises a driving assembly for driving a forearm to rotate relative to a large arm, and the driving assembly comprises a driving rope provided with a variable stiffness module; the algorithm comprises: determining the relationship between the change in length of the driving rope and the rotation angle of the forearm; establishing a driving rope stiffness-driving rope length change relationship between the stiffness of the variable stiffness module and the driving rope stiffness and the change in length of the driving rope; determining the overall stiffness of the elbow assembly according to the relationship between the driving rope stiffness-driving rope length change, the relationship between the driving rope length change and the forearm rotation angle, and the current change in length of the driving rope; and determining the stiffness of the rope-driven anthropomorphic manipulator according to the stiffness of the shoulder assembly, the overall stiffness of the elbow assembly, and the stiffness of the wrist assembly.

[0006] The stiffness control algorithm of the variable stiffness rope-driven anthropomorphic manipulator provided in the embodiment of the present invention proposes a stiffness modeling method for the rope-driven anthropomorphic manipulator with a variable stiffness elbow component, and derives and determines the stiffness of the elbow component. The stiffness of the elbow component is affected by the change in the length of the driving rope. The stiffness of the elbow component can be changed by adjusting the length of the driving rope, so that the stiffness of the manipulator arm can be actively adjusted to an appropriate stiffness according to different task scenarios, and is suitable for a wider range of application scenarios.

[0007] Optionally, the algorithm further includes: establishing a load-drive rope stiffness relationship between the load and the drive rope stiffness and the drive rope length change; and determining a load-drive rope length change relationship by combining the drive rope stiffness-drive rope length change relationship and the load-drive rope stiffness relationship.

[0008] The embodiment of the present invention establishes the relationship between the load and the stiffness of the drive rope, and the change in the length of the drive rope. If the robotic arm wants to maintain a balanced state when picking up a certain load, it is necessary to satisfy the relationship between the load and the change in the length of the drive rope. The change in the length of the drive rope can be adjusted to obtain appropriate stiffness, so that the robotic arm is suitable for the current application scenario.

[0009] Optionally, the variable stiffness module includes four connecting rods, the four connecting rods are rotatably connected in pairs to form a quadrilateral structure, a spring is connected between two opposite vertices of the quadrilateral structure, and the other two vertices are connected to a driving rope; the driving rope includes rope 1 and rope 2;

[0010] The relationship between the driving rope stiffness and the driving rope length change is as follows:

[0011]

[0012] Among them, R is the length of the connecting rod, x is the distance between the other two vertices, K is the elastic coefficient of the spring, k1 is the stiffness of rope 1, k2 is the stiffness of rope 2, Δl1 is the change in rope length of rope 1 caused by the deformation of the variable stiffness module, and Δl2 is the change in rope length of rope 2 caused by the deformation of the variable stiffness module.

[0013] The embodiment of the present invention provides an expression for the relationship between the driving rope stiffness and the driving rope length variation. The driving rope stiffness includes the influence of the variable stiffness module on the rope stiffness. The rope stiffness can be obtained by solving for different rope length variations.

[0014] Optionally, the rope 1 and the rope 2 are both wound around the rotating disk of the forearm and the rotating disk of the upper arm; the overall stiffness of the elbow assembly is as follows:

[0015]

[0016] Where r is the radius of the rotating disk, K e is the overall stiffness of the elbow joint.

[0017] The embodiment of the present invention provides an expression for the overall stiffness of the elbow assembly, based on which the stiffness of the elbow of the drive rope in the current state can be determined.

[0018] Optionally, establishing a load-rope stiffness relationship between the load and the driving rope stiffness and the driving rope length change includes:

[0019] The Lagrangian mechanical equilibrium equation for the elbow joint at any position within the rotation range is:

[0020]

[0021] Among them, T i is the pulling force of the driving rope on the arm rotating disk, r i is the instantaneous force arm of the driving rope tension, F is the mechanical arm's own gravity G and load F s The combined force, r F is the instantaneous moment arm of the resultant force F;

[0022] F=F S +G

[0023] The following mechanical equilibrium equation is obtained:

[0024]

[0025] Among them, α is the rotation angle of the line connecting the centers of the two rotating disks, and R(θ) is the force arm of the resultant force F;

[0026] R(θ)=a1·sin(|90°-θ|)

[0027] Wherein, θ is the rotation angle of the end of the forearm, and a1 is the length from the top of the forearm to the center of mass of the forearm under load.

[0028] The embodiment of the present invention provides a relationship expression between the load and the stiffness of the driving rope and the change in the length of the driving rope, which can adjust the change in the length of the driving rope to obtain appropriate stiffness, so that the robot arm is suitable for the current application scenario.

[0029] Optionally, the relationship between the rope length change ΔL1 of the rope 1 and the elbow joint position is as follows:

[0030]

[0031] Wherein, θ is the rotation angle of the end of the small arm, and r is the radius of the rotating disk;

[0032] The relationship between the rope length change ΔL1 of the rope 2 and the elbow joint position is as follows:

[0033]

[0034] The embodiment of the present invention provides an expression for the relationship between the change in the length of the driving rope and the position of the elbow joint, and obtains the relationship between the rotation angle of the elbow joint and the change in the length of the driving rope.

[0035] Optionally, the derivation process of the overall stiffness of the elbow assembly is as follows:

[0036] When the elbow joint is in a balanced position, a small input torque ΔT is applied to the end of the joint, and the output end of the forearm will produce a very small rotation angle Δθ. The expansion and contraction of rope 1 and rope 2 is ΔL. According to the principle of virtual work, it can be obtained that:

[0037] ΔT·Δθ=T2·ΔL-T1·ΔL

[0038] Among them, T2 and T1 are the pulling forces of rope 2 and rope 1 on the arm rotating disk; after sorting, we get:

[0039]

[0040] Where J is the Jacobian matrix from elbow joint angular velocity to rope velocity, θ is the rotation angle of the end of the small arm, and r is the radius of the rotating disk;

[0041] The spring deforms, causing the tension to change. The above formula can be written as:

[0042]

[0043] Among them, T g It is the tension of the rope when the elbow joint is not subject to any external force;

[0044] The rope stiffness can be expressed as:

[0045]

[0046] available:

[0047]

[0048] The change in rope length due to the deformation of the spring is the same as the change in rope length in the joint as a whole, that is:

[0049]

[0050] Therefore, the overall stiffness of the elbow component is obtained:

[0051]

[0052] Optionally, the stiffness of the rope-driven anthropomorphic manipulator is as follows:

[0053] K=J(q) +T (K θ +K t )J(q) +

[0054] Where: J(q) + is the generalized inverse matrix of the Jacobian matrix J(q); K θ K is the stiffness corresponding to each joint; t is the compensation stiffness matrix.

[0055] The embodiment of the present invention provides an expression for the overall stiffness of a rope-driven anthropomorphic manipulator. Based on the determination of the variable stiffness of the elbow component, the overall stiffness of the manipulator can be further determined.

[0056] The embodiment of the present invention provides a stiffness control system of a variable stiffness rope-driven anthropomorphic manipulator, the rope-driven anthropomorphic manipulator comprises a shoulder assembly, an elbow assembly, and a wrist assembly, the elbow assembly comprises a driving assembly for driving a forearm to rotate relative to a large arm, the driving assembly comprises a driving rope provided with a variable stiffness module; the system comprises: a rope length variation and angle relationship determination module, for determining the relationship between the driving rope length variation and the forearm rotation angle; a rope stiffness and length variation relationship determination module, for establishing a driving rope stiffness-driving rope length variation relationship between the stiffness of the variable stiffness module and the driving rope stiffness and the driving rope length variation; an elbow stiffness determination module, for determining the overall stiffness of the elbow assembly according to the driving rope stiffness-driving rope length variation relationship, the relationship between the driving rope length variation and the forearm rotation angle, and the current driving rope length variation; and a manipulator arm stiffness determination module, for determining the stiffness of the rope-driven anthropomorphic manipulator according to the shoulder assembly stiffness, the overall stiffness of the elbow assembly, and the wrist assembly stiffness.

[0057] Optionally, the system further comprises a module for determining the relationship between load and length change, which is used to: establish a load-drive rope stiffness relationship between the load and the drive rope stiffness and the drive rope length change; and determine a load-drive rope length change relationship by combining the drive rope stiffness-drive rope length change relationship and the load-drive rope stiffness relationship.

[0058] The stiffness control system of the variable stiffness rope-driven anthropomorphic manipulator provided in the embodiment of the present invention can achieve the same technical effect as the stiffness control algorithm of the variable stiffness rope-driven anthropomorphic manipulator mentioned above. BRIEF DESCRIPTION OF THE DRAWINGS

[0059] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on the provided drawings without paying creative work.

[0060] Figure 1 A schematic flow chart of a stiffness control algorithm for a variable stiffness rope-driven anthropomorphic manipulator provided in an embodiment of the present invention;

[0061] Figure 2 A schematic diagram showing the effect of a change in the length of a drive rope on the position of an elbow joint in an elbow assembly provided by an embodiment of the present invention;

[0062] Figure 3 An exemplary structural diagram of the stiffness of a variable stiffness module provided in an embodiment of the present invention;

[0063] Figure 4 A schematic diagram of the forces acting on the elbow assembly provided by an embodiment of the present invention;

[0064] Figure 5 A schematic diagram of the lever arm of the forearm in an unloaded state and a loaded state provided by an embodiment of the present invention;

[0065] Figure 6 A schematic structural diagram of a stiffness control system of a variable stiffness rope-driven anthropomorphic manipulator provided in an embodiment of the present invention. DETAILED DESCRIPTION

[0066] In order to make the above-mentioned objects, features and advantages of the present invention more clearly understood, the specific embodiments of the present invention are described in detail below in conjunction with the accompanying drawings. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention.

[0067] Nowadays, rope-driven manipulators are favored by researchers for their light weight, small inertia, and fast movement speed. Compared with traditional rigid manipulators, rope-driven manipulators are less rigid and more flexible, which is also an important feature. They have application value in some scenarios that require safe interaction. In practical applications, if the rigidity of a rope-driven manipulator is too high, it may cause unpredictable harm to the operator; if the rigidity of a rope-driven manipulator is too low, it is not conducive to its application in scenarios that require precise control and protection.

[0068] The embodiment of the present invention provides a rope-driven humanoid manipulator, whose elbow has a variable stiffness structure, which can actively adjust the stiffness, is suitable for a wider range of application scenarios, and can adjust the appropriate stiffness according to its different task scenarios. For the rope-driven humanoid manipulator, the embodiment of the present invention establishes a stiffness control algorithm model.

[0069] The embodiment of the present invention provides an algorithm model for stiffness control of a rope-driven anthropomorphic manipulator with variable stiffness, aiming to actively adjust the stiffness of the manipulator to make it suitable for a wider range of application scenarios and provide safer and more reliable services in human-computer interaction.

[0070] Exemplarily, the rope-driven anthropomorphic manipulator includes a shoulder assembly, an elbow assembly, and a wrist assembly. The shoulder assembly is connected to the wrist assembly through the elbow assembly. The elbow assembly includes a rotatably connected upper arm and lower arm and a driving assembly for driving the lower arm to rotate relative to the upper arm. The shoulder assembly is connected to the upper arm, and the wrist assembly is rotatably connected to the lower arm. The driving assembly includes a driving rope provided with a variable stiffness module. The driving assembly is transmission-connected to the upper arm, lower arm, and wrist assembly through the driving rope.

[0071] The two ends of the driving rope are respectively fixed and wound around two power shafts (driven by a motor), and are wound around the rotating disk of the small arm and the rotating disk of the large arm, so that when the power shaft rotates, the driving rope drives the above two rotating disks to rotate. Among them, the variable stiffness module can be connected in the middle of the driving rope, and its deformation can adjust the stiffness of the driving rope.

[0072] Figure 1 A schematic flow chart of a stiffness control algorithm for a variable stiffness rope-driven anthropomorphic manipulator provided in an embodiment of the present invention, the algorithm comprising:

[0073] S102, determining the relationship between the change in the length of the drive rope and the rotation angle of the forearm.

[0074] Taking the elbow joint control of the robotic arm as an example, the driving rope is driven by the power shaft to drive the rotating disk in the robotic arm to rotate, thereby driving the forearm to rotate.

[0075] When the arm moves from the original position to the target position, the length of the driving rope changes accordingly. The relationship between the arm rotation angle and the change in the driving rope length can be determined through geometric relationships.

[0076] S104, establishing a driving rope stiffness-driving rope length variation relationship between the stiffness of the variable stiffness module and the driving rope stiffness and driving rope length variation.

[0077] The variable stiffness module is connected to the middle of the driving rope, and the stiffness of the variable stiffness module can be changed by adjusting the length of the driving rope. Assuming that the driving rope is rigid, the change in the length of the driving rope causes the deformation of the variable stiffness module, thereby changing the stiffness of the driving rope as a whole. Here, the relationship between the stiffness of the variable stiffness module, the stiffness of the driving rope, and the change in the length of the driving rope can be established, that is, when the stiffness of the variable stiffness module and the change in the length of the driving rope are known, the stiffness of the driving rope can be solved.

[0078] Exemplarily, a spring may be provided in the variable stiffness module, and the spring combined with the auxiliary components realizes the nonlinearity of the variable stiffness module.

[0079] S106, determining the overall stiffness of the elbow assembly according to the relationship between the stiffness of the driving rope and the change in the length of the driving rope, the relationship between the change in the length of the driving rope and the rotation angle of the forearm, and the current change in the length of the driving rope.

[0080] After determining the above relationship between the stiffness of the driving rope and the change in the length of the driving rope, combined with the relationship between the change in the length of the driving rope and the rotation angle of the forearm, the overall stiffness of the elbow assembly can be derived when the forearm rotates at a very small angle. The overall stiffness of the elbow assembly is an expression based on the current change in the length of the driving rope. By adjusting the current change in the length of the driving rope, the overall stiffness of the elbow assembly can be changed, and the stiffness of the manipulator can be actively adjusted to make it applicable to a wider range of application scenarios.

[0081] Based on the above expression of the overall stiffness of the elbow assembly and the change in the length of the drive rope, the stiffness of the elbow joint can be changed by adjusting the length of the drive rope to achieve variable stiffness control of the robotic arm.

[0082] S108, determining the stiffness of the rope-driven anthropomorphic manipulator according to the stiffness of the shoulder assembly, the overall stiffness of the elbow assembly, and the stiffness of the wrist assembly.

[0083] After obtaining the overall stiffness of the elbow assembly, the stiffness of the rope-driven anthropomorphic manipulator can be obtained by combining the stiffness of the shoulder assembly and the stiffness of the wrist assembly. Based on the above-mentioned change in the stiffness of the elbow joint, the stiffness of the overall manipulator can be adjusted. For example, a series manipulator stiffness model can be used.

[0084] The stiffness control algorithm of the variable stiffness rope-driven anthropomorphic manipulator provided in the embodiment of the present invention proposes a stiffness modeling method for the rope-driven anthropomorphic manipulator with a variable stiffness elbow component, and derives and determines the overall stiffness of the elbow component, which is affected by the change in the length of the driving rope. The stiffness of the elbow component can be changed by adjusting the length of the driving rope, so that the stiffness of the manipulator can be actively adjusted to an appropriate stiffness according to different task scenarios, and is suitable for a wider range of application scenarios.

[0085] In view of the fact that there is usually a load at the end of the robotic arm during use, this embodiment also establishes the relationship between the load and the stiffness of the drive rope and the change in the length of the drive rope. If the robotic arm is to maintain a balanced state when picking up a load, the relationship between the load and the stiffness of the drive rope and the change in the length of the drive rope needs to be satisfied.

[0086] Based on this, the above algorithm includes: establishing a load-drive rope stiffness relationship between the load and the drive rope stiffness and the change in drive rope length; and determining the load-drive rope length change relationship by combining the above drive rope stiffness-drive rope length change relationship and load-drive rope stiffness relationship.

[0087] Considering that the relationship between the stiffness of the driving rope and the change in the length of the driving rope has been derived, combined with the above-mentioned load-driving rope stiffness relationship, the relationship between the load and the change in the length of the driving rope can be derived. Therefore, in this embodiment, if the robot arm wants to maintain a balanced state when picking up a certain load, it needs to satisfy the relationship between the load and the change in the length of the driving rope.

[0088] A certain load can have multiple drive rope length changes that satisfy the above relationship, all of which can keep the robotic arm balanced. However, the elbow joint stiffness corresponding to different drive rope length changes is different. The drive rope length change can be adjusted to obtain appropriate stiffness to make the robotic arm suitable for the current application scenario.

[0089] Figure 2 A schematic diagram showing the effect of changes in the length of the drive rope in the elbow assembly on the position of the elbow joint. Figure 2 The figure shows the rotating disk of the upper arm and the rotating disk of the lower arm in the elbow assembly. The driving rope includes rope 1 (thick solid line) and rope 2 (thick dotted line). Rope 1 and rope 2 are both wound around the rotating disk of the lower arm and the rotating disk of the upper arm. The change in the length of rope 1 and rope 2 drives the rotating disk to rotate. The driving rope is rigid by default.

[0090] like Figure 2 As shown in the figure, after the forearm of the robot moves from the original position M to the target position N, it can be known from the geometric relationship that β = α. As the radius of the two rotating disks is equal, it can be known that the rotation angle θ at the end of the forearm is twice the rotation angle α of the rotating disk (planetary gear train structure), that is:

[0091] θ=2α (1.1)

[0092] Rope length variation:

[0093] ΔL=2×δL (1.2)

[0094] δL=βπr / 180° (1.3)

[0095] Where r is the radius of the elbow rotating disk, δL is the arc length corresponding to the angle θ;

[0096] therefore:

[0097] The relationship between the movement angle of the robot arm elbow joint and the change in the length of the two ropes is obtained as follows:

[0098] Elongation of rope 1:

[0099]

[0100] Elongation of rope 2:

[0101]

[0102] The relationship between the speed of the rope and the angular velocity of the elbow joint is derived as follows:

[0103] because:

[0104] in, is the speed of the rope, is the joint angular velocity, J is arrive The Jacobian matrix of ;

[0105] Combined with (1.4), we can know that:

[0106]

[0107] Figure 3 FIG. 1 shows an exemplary structural diagram of the stiffness of a variable stiffness module provided by an embodiment of the present invention. Figure 3 The medium variable stiffness module stiffness takes a quadrilateral structure including a spring as an example. Specifically, the variable stiffness module stiffness includes four connecting rods, and the four connecting rods are rotatably connected in pairs to form a quadrilateral structure. A spring is connected between two opposite vertices of the quadrilateral structure, and the other two vertices are connected to a driving rope.

[0108] The above quadrilateral structure is deformed by the tension of the driving ropes on both sides. The stiffness K(X) of the driving rope including the influence of the quadrilateral structure is expressed as follows:

[0109]

[0110] Among them, X0 is the initial length of the quadrilateral structure, that is, the distance between the left and right vertices, X is the length of the quadrilateral structure after being subjected to force, F is the tensile force on the quadrilateral structure, R is the length of the connecting rod, and K is the elastic coefficient of the spring;

[0111] Taking rope 1 and rope 2 as examples, the relationship between the driving rope stiffness and the change in driving rope length affected by the variable stiffness module is as follows:

[0112]

[0113] Among them, x is the initial length of the quadrilateral structure, K is the elastic coefficient of the spring, k1 is the stiffness of rope 1, k2 is the stiffness of rope 2, Δl1 is the change in rope length of rope 1 caused by the deformation of the variable stiffness module, and Δl2 is the change in rope length of rope 2 caused by the deformation of the variable stiffness module.

[0114] Based on the above relationship, for different Δl1 and Δl2, the stiffness of rope 1 and the stiffness of rope 2 can be solved.

[0115] Elbow assembly overall stiffness k e The relationship between the stiffness k1 and k2 of the two ropes is derived as follows:

[0116] When the elbow joint is in a balanced position, a small input torque ΔT is applied to the end of the joint, and the end of the elbow joint forearm will produce a very small rotation angle Δθ. The expansion and contraction of rope 1 and rope 2 is ΔL. According to the principle of virtual work, it can be obtained that:

[0117] ΔT·Δθ=T2·ΔL-T1·ΔL (1.12)

[0118] Among them, T2 and T1 are the tensions exerted by rope 2 and rope 1 on the arm rotating disk, that is, the output tension of the rope in the variable stiffness module;

[0119] Arranging (1.12) yields:

[0120]

[0121] Among them, J is the above arrive The Jacobian matrix of ;

[0122] Since the joint stiffness is variable, when a preload torque ΔT is applied to the end of the joint, the joint rotates Δθ, the spring deforms, and the tension changes. Equation (1.13) can be written as:

[0123]

[0124] Among them, T g It is the tension of the rope when the joint is not subjected to any external force;

[0125] The rope stiffness can be expressed as:

[0126]

[0127] Therefore (1.14) can be further organized as:

[0128]

[0129] Among them, k1 and k2 are the stiffness of rope 1 and rope 2 respectively, and Δl1 and Δl2 are the rope changes caused by spring deformation. Because the joint is only subjected to external torque, the change in the length of each group of ropes caused by the deformation of the spring is the same as the change in the length of each group of ropes in the joint as a whole, that is:

[0130]

[0131] Therefore, the overall stiffness of the elbow joint is K e as follows:

[0132]

[0133] The relationship between load Fs and stiffness k1, k2 is established as follows.

[0134] According to the way the rope tension acts on the forearm, the Lagrangian mechanical equilibrium equation for the elbow joint at any position within the rotation range can be derived as follows:

[0135]

[0136] Among them, T i is the tension of the driving rope on the arm rotating disk, that is, the output tension of the rope in the variable stiffness module, r i is the instantaneous force arm of the driving rope tension, F is the mechanical arm's own gravity G and load F s The combined force, r F is the instantaneous moment arm of the resultant force F;

[0137] F=F S +G (1.20)

[0138] Figure 4 FIG. 4 shows a schematic diagram of the forces acting on the elbow assembly. Figure 4 As shown:

[0139] The resultant force of the two ropes is:

[0140] T=k2Δl2-k1Δl1 (1.21)

[0141] The component of the resultant force of the rope tension on the lever arm is:

[0142]

[0143] The moment arms of the two rope tensions are equal:

[0144]

[0145] Combining (1.19)-(1.23), we can get the following mechanical equilibrium equation:

[0146]

[0147] Among them, α is the rotation angle of the line connecting the centers of the two rotating disks, and R(θ) is the force arm of F, which is also a function of θ. The solution process of R(θ) is as follows:

[0148] The forearm is simplified to a rod of uniform mass. Figure 5 The figure shows the arm diagram of the forearm in the no-load state and the loaded state. Figure 5 As shown, the center of mass of the forearm in the unloaded state divides its length into A1 and A2, and their respective weights are F1 and F2. The center of mass of the forearm in the loaded state divides its length into a1 and a2, and their respective weights are f1 and f2. There is the following relationship:

[0149] F1A1=F2A2

[0150] A1+A2=a1+a2

[0151] f1a1=(f2+F S )a2

[0152]

[0153] f1+f2=F1+F2

[0154] According to the above conditions, the relationship between a1 and R(θ) can be obtained:

[0155] R(θ)=a1·sin(|90°-θ|)

[0156] Formula (1.24) can be written as:

[0157]

[0158] Among them, θ is the rotation angle of the end of the forearm, and a1 is the length from the top of the forearm to the center of mass of the forearm under load.

[0159] Exemplarily, the overall stiffness of the anthropomorphic robotic arm is as follows:

[0160] Serial robot arm stiffness: K = J (q) +T (K θ +K t )J(q) +

[0161] Among them, J(q) + is the generalized inverse matrix of the Jacobian matrix J(q); K θ is the stiffness corresponding to each joint, which is a diagonal matrix; K t is the compensation stiffness matrix. For the stiffness compensation matrix K t Introduce and two evaluation indicators, v p With v r and represent the stiffness compensation matrix K t The effect on the end displacement and end angle. By applying joint stiffness values ​​and giving different load forces at the end of the robot, v is tested. p With v r The value of K t The impact on the overall stiffness of the robot.

[0162] Taking the 7-DOF rope-driven anthropomorphic arm as an example, the rotation and pitch degrees of freedom of the wrist are parallel degrees of freedom, so it cannot be modeled according to the above series robot arm stiffness model.

[0163] Since the elbow stiffness is independent of the rotation angle and only related to the pitch angle, the 7-DOF anthropomorphic arm stiffness is equivalent to the 6-DOF stiffness (excluding the wrist rotation degree of freedom).

[0164] For the equivalent 6-DOF rope-driven anthropomorphic arm:

[0165]

[0166] The Jacobian matrix of the equivalent 6-DOF manipulator is: D (q)

[0167] Therefore: K = J D (q) +T (K θ +K t )J D (q) +

[0168] Each joint of the shoulder assembly is directly connected by a cable, so the stiffness of each joint is the stiffness k of the cable. cable , that is: k shoulder_1 =k shoulder_2 =k shoulder_3 =k cable ; Pitch stiffness k of wrist assembly wrist_pitch , flip stiffness k wrist_ro The calculation method in existing literature can be used and will not be described in detail here.

[0169] In summary, the stiffness of the anthropomorphic robotic arm is as follows:

[0170] K NRB =J D (q)+T (K θ +K t )J D (q) + .

[0171] In the embodiment of the present invention, the rope-driven anthropomorphic manipulator, whose elbow has a variable stiffness structure, can actively adjust the stiffness, so it can be applied to a wider range of application scenarios, and can adjust the appropriate stiffness according to its different task scenarios. This embodiment establishes a stiffness control algorithm model for the manipulator, derives an expression of the overall stiffness of the elbow assembly based on the change in the length of the drive rope, and a relationship between the load and the stiffness of the drive rope and the change in the length of the drive rope, and can actively adjust the stiffness of the manipulator so that it can operate with a suitable overall stiffness, which can be applied to a wider range of application scenarios and provide safer and more reliable services in human-computer interaction.

[0172] Figure 6 The present invention shows a structural schematic diagram of a stiffness control system of a variable stiffness rope-driven anthropomorphic manipulator provided by an embodiment of the present invention. The rope-driven anthropomorphic manipulator comprises a shoulder assembly, an elbow assembly, and a wrist assembly. The elbow assembly comprises a driving assembly for driving a forearm to rotate relative to a large arm. The driving assembly comprises a driving rope provided with a variable stiffness module. The system comprises:

[0173] The module 601 for determining the relationship between the change in the length of the driving rope and the rotation angle of the forearm is used to determine the relationship between the change in the length of the driving rope and the rotation angle of the forearm;

[0174] A rope stiffness and length variation relationship determination module 602, used to establish a driving rope stiffness-driving rope length variation relationship between the stiffness of the variable stiffness module and the driving rope stiffness and the driving rope length variation;

[0175] The elbow stiffness determination module 603 is used to determine the overall stiffness of the elbow assembly according to the relationship between the driving rope stiffness and the driving rope length change, the relationship between the driving rope length change and the forearm rotation angle, and the current driving rope length change;

[0176] The robot arm stiffness determination module 604 is used to determine the stiffness of the rope-driven anthropomorphic robot arm according to the stiffness of the shoulder component, the overall stiffness of the elbow component, and the stiffness of the wrist component.

[0177] As a feasible approach, the system further includes a module for determining the relationship between load and length variation, which is used to:

[0178] Establishing a load-drive rope stiffness relationship between the load, the drive rope stiffness, and the change in the drive rope length;

[0179] The load-drive rope length change relationship is determined by combining the drive rope stiffness-drive rope length change relationship and the load-drive rope stiffness relationship.

[0180] The stiffness control system of the variable stiffness rope-driven anthropomorphic manipulator provided in the embodiment of the present invention can achieve the same technical effect as the stiffness control algorithm of the variable stiffness rope-driven anthropomorphic manipulator mentioned above.

[0181] Although the present invention is disclosed as above, the present invention is not limited thereto. Any person skilled in the art can make various changes and modifications without departing from the spirit and scope of the present invention. Therefore, the protection scope of the present invention shall be subject to the scope defined by the claims.

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

[0183] In this specification, each embodiment is described in a progressive manner, and each embodiment focuses on the differences from other embodiments. The same or similar parts between the embodiments can be referred to each other. For the system disclosed in the embodiment, since it corresponds to the method disclosed in the above embodiment, the description is relatively simple, and the relevant parts can be referred to the method part.

[0184] The above description of the disclosed embodiments enables one skilled in the art to implement or use the present invention. Various modifications to these embodiments will be apparent to one skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention will not be limited to the embodiments shown herein, but rather to the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A stiffness control algorithm for a variable stiffness rope-driven anthropomorphic manipulator, characterized in that: The rope-driven anthropomorphic manipulator comprises a shoulder assembly, an elbow assembly, and a wrist assembly, wherein the elbow assembly comprises a driving assembly for driving a forearm to rotate relative to a large arm, and the driving assembly comprises a driving rope provided with a variable stiffness module; the algorithm comprises: Determine the relationship between the change in the length of the drive rope and the rotation angle of the forearm; Establishing a driving rope stiffness-driving rope length variation relationship between the stiffness of the variable stiffness module and the driving rope stiffness and the driving rope length variation; Determine the overall stiffness of the elbow assembly according to the relationship between the stiffness of the drive rope and the change in the length of the drive rope, the relationship between the change in the length of the drive rope and the rotation angle of the forearm, and the current change in the length of the drive rope; The stiffness of the rope-driven anthropomorphic manipulator is determined according to the stiffness of the shoulder component, the overall stiffness of the elbow component, and the stiffness of the wrist component.

2. The algorithm according to claim 1, characterized in that The algorithm also includes: Establishing a load-drive rope stiffness relationship between the load, the drive rope stiffness, and the change in the drive rope length; The load-drive rope length change relationship is determined by combining the drive rope stiffness-drive rope length change relationship and the load-drive rope stiffness relationship.

3. The algorithm according to claim 2, characterized in that The variable stiffness module includes four connecting rods, and the four connecting rods are rotatably connected in pairs to form a quadrilateral structure. A spring is connected between two opposite vertices of the quadrilateral structure, and the other two vertices are connected to a driving rope; the driving rope includes rope 1 and rope 2; The relationship between the driving rope stiffness and the driving rope length change is as follows: Among them, R is the length of the connecting rod, x is the distance between the other two vertices, K is the elastic coefficient of the spring, k1 is the stiffness of rope 1, k2 is the stiffness of rope 2, Δl1 is the change in rope length of rope 1 caused by the deformation of the variable stiffness module, and Δl2 is the change in rope length of rope 2 caused by the deformation of the variable stiffness module.

4. The algorithm according to claim 3, characterized in that The rope 1 and the rope 2 are both wound around the rotating disk of the forearm and the rotating disk of the upper arm; the overall stiffness of the elbow assembly is as follows: Where r is the radius of the rotating disk, K e is the overall stiffness of the elbow joint.

5. The algorithm according to claim 3, characterized in that The establishing of the load-rope stiffness relationship between the load and the driving rope stiffness and the driving rope length change comprises: The Lagrangian mechanical equilibrium equation for the elbow joint at any position within the rotation range is: Among them, T i is the pulling force of the driving rope on the arm rotating disk, r i is the instantaneous force arm of the driving rope tension, F is the mechanical arm's own gravity G and load F s The combined force, r F is the instantaneous moment arm of the resultant force F; F=F S +G The following mechanical equilibrium equation is obtained: Among them, α is the rotation angle of the line connecting the centers of the two rotating disks, and R(θ) is the force arm of the resultant force F; R(θ)=a1·sin(|90°-θ|) Wherein, θ is the rotation angle of the end of the forearm, and a1 is the length from the top of the forearm to the center of mass of the forearm under load.

6. The algorithm according to claim 4, characterized in that The relationship between the rope length change ΔL1 of the rope 1 and the elbow joint position is as follows: Wherein, θ is the rotation angle of the end of the small arm, and r is the radius of the rotating disk; The relationship between the rope length change ΔL1 of the rope 2 and the elbow joint position is as follows:

7. The algorithm according to claim 4, characterized in that The derivation process of the overall stiffness of the elbow assembly is as follows: When the elbow joint is in a balanced position, a small input torque ΔT is applied to the end of the joint, and the output end of the forearm will produce a very small rotation angle Δθ. The expansion and contraction of rope 1 and rope 2 is ΔL. According to the principle of virtual work, it can be obtained that: ΔT·Δθ=T2·ΔL-T1·ΔL Among them, T2 and T1 are the pulling forces of rope 2 and rope 1 on the arm rotating disk; after sorting, we get: Where J is the Jacobian matrix from elbow joint angular velocity to rope velocity, θ is the rotation angle of the end of the small arm, and r is the radius of the rotating disk; The spring deforms, causing the tension to change. The above formula can be written as: Among them, T g It is the tension of the rope when the elbow joint is not subject to any external force; The rope stiffness can be expressed as: available: The change in rope length due to the deformation of the spring is the same as the change in rope length in the joint as a whole, that is: Therefore, the overall stiffness of the elbow component is obtained:

8. The algorithm according to claim 4, characterized in that The stiffness of the rope-driven anthropomorphic manipulator is as follows: K=J(q) +T (K θ +K t )J(q) + Where: J(q) + is the generalized inverse matrix of the Jacobian matrix J(q); K θ K is the stiffness corresponding to each joint; t is the compensation stiffness matrix.

9. A stiffness control system for a variable stiffness rope-driven anthropomorphic manipulator, characterized in that: The rope-driven anthropomorphic manipulator comprises a shoulder assembly, an elbow assembly, and a wrist assembly, wherein the elbow assembly comprises a driving assembly for driving a forearm to rotate relative to a large arm, and the driving assembly comprises a driving rope provided with a variable stiffness module; the system comprises: A module for determining the relationship between the change in length of the rope and the angle, used to determine the relationship between the change in length of the driving rope and the rotation angle of the forearm; A module for determining the relationship between rope stiffness and length variation, used for establishing a driving rope stiffness-driving rope length variation relationship between the stiffness of the variable stiffness module and the driving rope stiffness and the driving rope length variation; An elbow stiffness determination module, used to determine the overall stiffness of the elbow assembly according to the relationship between the stiffness of the driving rope and the change in the length of the driving rope, the relationship between the change in the length of the driving rope and the rotation angle of the forearm, and the current change in the length of the driving rope; The robot arm stiffness determination module is used to determine the stiffness of the rope-driven anthropomorphic robot arm according to the stiffness of the shoulder component, the overall stiffness of the elbow component, and the stiffness of the wrist component.

10. The system according to claim 9, characterized in that The system further includes a load and length variation relationship determination module, which is used to: Establishing a load-drive rope stiffness relationship between the load, the drive rope stiffness, and the change in the drive rope length; The load-drive rope length change relationship is determined by combining the drive rope stiffness-drive rope length change relationship and the load-drive rope stiffness relationship.

Images