A method for kinematics modeling of a rope-driven segmented redundant manipulator

Abstract

The application provides a kind of kinematics modeling method of rope-driven segmented redundant manipulator, the rope-driven segmented redundant manipulator includes I large section, each large section is made of J small section, each small section is connected by cross shaft, including the kinematics modeling of forward kinematics joint angle to configuration and the kinematics modeling of forward kinematics joint angle to rope length, and the modeling process is carried out using exponential product formula, wherein, in the kinematics modeling of forward kinematics joint angle to configuration, linkage error is considered, and linkage error factor is introduced.The application solves the technical problems that the kinematics modeling of rope-driven segmented redundant manipulator is complex, accuracy is poor and specific arm shape cannot be represented in the prior art, and better modeling effect is achieved.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of kinematics modeling of mechanical arm, and particularly relates to a kinematics modeling method of rope-driven segmented redundant mechanical arm. BACKGROUND

[0002] In the aerospace manufacturing industry, engine maintenance and assembly, installation of internal parts of the fuselage, and overhaul of the fuselage structure all require workers to work in very small spaces, with particularly harsh working environments and extremely difficult work; in the equipment manufacturing industry, welding cabin partitions, installation of mining hydraulic devices, and welding and maintenance of cathode busbars of electrolytic cells all require workers to work continuously in high-temperature, polluted, and narrow spaces, and the working environment of the workers is extremely harsh, and the quality of the welds is difficult to guarantee; in the nuclear power industry, key equipment such as nuclear power reactor bodies and fuel rod control pipelines often intersect with each other, forming a complex environment, and the environment is highly radioactive, high-temperature, and small in space, and there is a lack of effective means for maintenance and maintenance; in post-disaster rescue tasks, narrow spaces formed by collapsed buildings or equipment make it difficult for rescue personnel to enter the rescue site and obtain information about trapped personnel; for the above working environments, rope-driven segmented super-redundant mechanical arms are very suitable for performing work tasks in small, dangerous, and unstructured spaces due to their super-multiple degrees of freedom. However, due to the large number of joints and complex mechanical structure, it is difficult to describe the kinematics model. In order to achieve motion control of the mechanical arm, accurate kinematics modeling is crucial.

[0003] In the prior art, some kinematics modeling researches for CSRM propose a kinematics modeling method based on the D-H parameter method. This method needs to establish a link coordinate system and describe each joint coordinate system, which is relatively complex in representation, cannot express the pose of each joint, can only express the pose of the end effector, and the accuracy of the kinematics model is poor. SUMMARY

[0004] In view of the deficiencies in the prior art, the present application proposes a kinematics modeling method of rope-driven segmented redundant mechanical arm, solving the problems of complex modeling and poor accuracy of current rope-driven segmented super-redundant mechanical arm.

[0005] In order to achieve the above purpose, the present application adopts the following technical solutions:

[0006] The application provides a kinematic modeling method of a rope-driven segmented redundant manipulator, the rope-driven segmented redundant manipulator comprising I large segments, each of the large segments being composed of J small segments, each of the small segments being connected by a cross shaft, comprising kinematic modeling of positive kinematics joint angles to configurations and kinematic modeling of positive kinematics joint angles to rope lengths, and the modeling process is performed by using an exponential product formula, wherein in the kinematic modeling of positive kinematics joint angles to configurations, linkage error is considered, and a linkage error factor is introduced.

[0007] In some embodiments, the application also has the following technical features:

[0008] The kinematic modeling of positive kinematics joint angles to configurations comprises the following steps:

[0009] The rope-driven segmented redundant manipulator has 2I degrees of freedom, and a linkage error factor f is introduced for each degree of freedom;

[0010] The transformation matrix from the cross shaft coordinate system of each small segment to the base coordinate system is calculated by using the product of the exponential product formula, and the kinematic model of positive kinematics joint angles to configurations is established.

[0011] The kinematic model of positive kinematics joint angles to configurations is specifically:

[0012]

[0013] Wherein, c T b represents the pose transformation from the base coordinate system to the end of the entire arm, n represents the number of large segments, θa:b represents the angle rotated from the a-th degree of freedom to the b-th degree of freedom, fa:b represents the linkage factor corresponding to the a-th degree of freedom to the b-th degree of freedom, and the transfer matrix C i (θ, f) is the exponential product formula, which represents the change of the joint pose at the end of the n-th large segment caused by the change of the cross shaft angle, M n is a known quantity, which represents the initial attitude matrix of the end of the n-th large segment relative to the base coordinate system.

[0014] The transfer matrix is specifically:

[0015]

[0016] Wherein, represents the screw matrix related to the j-th joint of the i-th segment, k j is a proportion factor.

[0017] The formula of the proportion factor is: k j = 3.5-j, j = 1~J.

[0018] The screw matrix and the corresponding coordinate is expressed as:

[0019]

[0020]

[0021]

[0022] wherein, is the joint axis direction, is the vector from an arbitrary point on the same joint axis to the base coordinate system, is the skew-symmetric matrix corresponding to ω = [ω x , ω y , ω z ], wherein ω x , ω y , ω z respectively represent the axial direction of x, y, z.

[0023] The kinematic modeling of the joint angle to the rope length of the forward kinematics comprises the following steps:

[0024] Each large section of the rope-driven segmented redundant manipulator is connected to a motor drive box located at the root of the rope-driven segmented redundant manipulator by three ropes, and the ropes are uniformly distributed at 120 degrees;

[0025] Taking the cross shaft between adjacent sections in the large section as the object, taking the proximal root end as the fixed end and the distal root end as the moving end, a transfer matrix of the moving end relative to the fixed end is established;

[0026] Based on the transfer matrix, the rope length change in the cross shaft is obtained;

[0027] Based on the rope length change in the cross shaft, a kinematic model of the joint angle to the rope length of the forward kinematics is established.

[0028] The kinematic model of the joint angle to the rope length of the forward kinematics is:

[0029]

[0030] wherein, is the rope length vector of the ith large section, is the change amount of the rope length of each section in the ith large section,

[0031]

[0032] wherein, Δl r represents the rope length change amount of the rth rope in the three ropes, Δl u = || m R f pg + m t f ||-2d,u=1,2,3, m R f and m t f Let be the rotation matrix and translation vector of the transfer matrix, 2d be the rope length between the two sections connected by the cross shaft when it is not rotated, and p be the rotation matrix and translation vector of the transfer matrix. g Let g be the coordinates of the g-th rope hole on the fixed end, where g = 1, 2, 3.

[0033] Establishing the transfer matrix of the mobile terminal relative to the fixed terminal includes the following steps:

[0034] The cross shafts rotate θ about their two movable axes respectively. 2i-1 and θ 2i The transition matrix of the mobile terminal relative to the fixed terminal can be expressed as:

[0035]

[0036] in, m T f M is the transition matrix. m S represents the attitude of the mobile device in the fixed-end coordinate system. i The spinor matrix is ​​as follows:

[0037]

[0038] S i =(ω i ,v i ), i=1,2, ω1=(0,0,1)v1=(d,0,0)ω2=(1,0,0)v2=(0,0,-d).

[0039] The coordinates of the g-th rope hole on the fixed end are:

[0040]

[0041] Where r is the distance from the rope hole to the central axis of the disk, and α is the offset angle of the first rope hole.

[0042] The beneficial effects of this invention are:

[0043] This invention employs an exponential product formula modeling method, avoiding the need to establish a link coordinate system and effectively representing the motion state of all joints throughout the arm. Furthermore, considering the linkage errors caused by factors such as gravity and friction during the modeling process, a linkage error factor is introduced to describe these errors, thereby improving the accuracy of the CSRM kinematic model.

[0044] Other benefits of embodiments of the present application will be described further below. BRIEF DESCRIPTION OF DRAWINGS

[0045] Figure 1 is a schematic diagram of joint coordinate distribution in a large segment in embodiments of the present application;

[0046] Figure 2 is a diagram of initial state joint angle and rope length relationship in embodiments of the present application;

[0047] Figure 3 is a diagram of non-initial state joint angle and rope length relationship in embodiments of the present application. DETAILED DESCRIPTION

[0048] In order to make the technical solutions and advantages of the present application clearer, the technical solutions of the embodiments of the present application will be described in detail below with reference to the drawings in the present application. Obviously, the described embodiments are part of the embodiments of the present application, rather than all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative labor fall within the scope of protection of the present application.

[0049] The embodiments of the present application will be described in detail below, and examples of the embodiments are shown in the drawings, in which the same or similar reference numerals represent the same or similar elements or elements having the same or similar functions throughout. The embodiments described below with reference to the drawings are exemplary and are only used to explain the present application, and cannot be understood as a limitation of the present application.

[0050] The cable-driven segmented redundant manipulator has the characteristics of a slender structure. When performing a task, not only does it need to control the end of the manipulator to reach a specified target pose, but it also needs to calculate the pose of all joints of the entire arm, i.e., the arm shape, so as to avoid collision between the manipulator and the working environment. The forward kinematics modeling of the CSRM can be divided into joint angle to configuration kinematics modeling and joint angle to rope length kinematics modeling. The joint angle to configuration modeling is to give the rotation angle of each joint and calculate the pose of the end of each joint or each large segment. The joint angle to rope length modeling is to calculate the length of the driving rope that needs to be pulled according to the given joint angle.

[0051] First, the concepts involved in the present application are explained and described:

[0052] Cable-driven segmented redundant manipulator (CSRM)

[0053] The CSRM is a kind of rope-driven super-redundant manipulator, which is different from the traditional rigid manipulator, and is driven by a motor to pull a rope to drive the joints of the manipulator to move. The CSRM has super-multiple redundant degrees of freedom. Taking the modeling object in the embodiment of the present application as an example, the whole manipulator is divided into five large sections, each large section has six small sections, and there are thirty cross shaft joints and sixty degrees of freedom. Each large section of the CSRM is pulled by three ropes, and each small section in each large section is connected through a linkage mechanism, while ensuring flexibility, reducing the number of driving motors, and improving the accuracy of control.

[0054] D-H parameter method (Denavit-Hartenberg parameter, D-H)

[0055] The D-H parameter method is a commonly used modeling method for manipulators, and is widely used in rigid arms, rope-driven super-redundant manipulators and the like. This method establishes a coordinate system on each link, and realizes the transformation of the coordinates on two links through homogeneous coordinate transformation. In a multi-link series system, the relationship between the first and last coordinate systems can be established by using homogeneous coordinate transformation multiple times.

[0056] Exponential product formula (Product of Exponential, PoE)

[0057] The exponential product formula is a kind of modeling method for manipulator kinematics, which does not need to establish a link coordinate system, and has a simple expression. For the CSRM, since it has super-multiple degrees of freedom, the exponential product formula can be conveniently extended and applied for modeling.

[0058] When the exponential product formula is used to establish forward kinematics, a base coordinate system {s} and an end coordinate system {b} are established, M ∈ SE(3) represents the initial pose of the end coordinate system relative to the base coordinate system, M is the initial pose transformation matrix of the end relative to the base coordinate system, SE(3) is defined in the Lie group Lie algebra, which is equivalent to the three-dimensional pose transformation matrix defined in the Lie group, but is expressed in the form of Lie algebra. It is assumed that only the n th joint rotates, and the corresponding joint variable is θ n , the displacement of the end coordinate system M and the new pose T after the end moves can be written as:

[0059]

[0060] In the formula, T ∈ SE(3) is the new pose of the joint end, S n =(ω n ,v n ) is the screw coordinate of the joint n in the base coordinate system, ω n ∈ R 3is the unit vector along the positive direction of the joint axis, v n = -ω n x q n , q n is an arbitrary point on the joint, and the coordinate value is measured in the base coordinate system. According to this reasoning, when all joints move, the new position of the end of the robot arm should satisfy:

[0061]

[0062] The above equation is the exponential product formula for the forward kinematics of an n-degree-of-freedom open-chain robot. All the spinor matrices in it are represented based on the base coordinate system.

[0063] Linkage error factor

[0064] There are six small sections in each large section of the CSRM. In order to make each small section in the large section rotate by the same angle (this is determined by the mechanical arm design structure, because the number of driving motors needs to be controlled, so there is only one set of driving ropes in each large section, but there is more than one small section in each large section, so the linkage rope is used to make them rotate by the same angle. This design can make the mechanical arm more flexible and does not require too many driving motors), a linkage mechanism is designed to make each joint rotate by the same angle. However, due to factors such as gravity and friction, the joints near the end often rotate by a larger angle than the joints near the root, and it is impossible to make each joint rotate by the same angle. This error is called linkage error. In the kinematic modeling process of the CSRM, the influence of the linkage error is considered, and a linkage error factor is introduced to describe it, which can greatly improve the accuracy of the kinematic modeling of the CSRM and is helpful for the motion control of the CSRM.

[0065] In some embodiments, the present application provides a kinematic modeling method for a rope-driven segmented redundant mechanical arm, which includes five large sections, each of which is composed of six small sections, and each of the small sections is connected by a cross shaft. The kinematic modeling includes forward kinematics of joint angles to configuration and forward kinematics of joint angles to rope length, and the modeling process adopts an exponential product formula, wherein in the forward kinematics of joint angles to configuration, a linkage error factor is introduced considering the linkage error.

[0066] In some embodiments, the forward kinematics of joint angles to configuration includes the following steps:

[0067] The present application takes a typical CRSM as the research object, and the entire arm is divided into five large sections, each of which is divided into six small sections, and each small section is connected by a cross shaft. The joint coordinate distribution in one large section is as shown in Figure 1 ;

[0068] CSRM has ten degrees of freedom θ∈R 10 where R10 represents ten degrees of freedom in the Euclidean space, and a linkage error factor vector f∈R 10 is introduced in each degree of freedom, which represents the angle error in each large section due to factors such as gravity and friction. The forward kinematics of joints to configuration, i.e. the transformation matrix from each cross-axis coordinate system to the base coordinate system, is calculated by the product of the exponential product formula to establish the kinematics model of joint angle to configuration:

[0069]

[0070] where, c T b represents the pose transformation from the base coordinate system to the end of the whole arm, n represents the number of large sections, θa:b represents the angle rotated from the a-th degree of freedom to the b-th degree of freedom, fa:b represents the linkage factor corresponding to the a-th degree of freedom to the b-th degree of freedom, and the transfer matrix C i (θ,f) is the exponential product formula, which represents the change of the end joint pose of the n-th large section caused by the change of the cross-axis angle, M n is a known quantity, which represents the initial attitude matrix of the end of the n-th large section relative to the base coordinate system, and the initial joint angles are all 0.

[0071] The mounting structure of each two small sections is Z-Y-Y-Z. The transfer matrix of the adjacent two small sections can be represented as:

[0072]

[0073] where represents the screw matrix related to the j-th joint of the i-th section, k j is a scale factor, which will be introduced in detail below.

[0074] The screw matrix and the corresponding coordinates can be represented as:

[0075]

[0076]

[0077]

[0078] where, is the joint axis direction, is the vector from an arbitrary point on the same joint axis to the base coordinate system, is the skew-symmetric matrix corresponding to ω = [ω x ,ω y ,ω z ], which is defined as:

[0079]

[0080] where ω x ,ω y ,ω z represent the axial direction of x, y, z respectively. Due to the friction between the connecting rod driving rope and the sleeve mechanism and the influence of gravity, the rotation angle of each small section in a large section is not exactly the same. The linkage force is transmitted from the rope connection end of each section in turn, and as the linkage force decreases, the linkage error is proportional to the error coefficient. The empirical formula is verified by experiments, where k j is the proportional factor, which is related to the joint sequence, as follows:

[0081] k j = 3.5-j, j = 1 ~ 6

[0082] In some embodiments, the kinematics modeling of joint angle to rope length of forward kinematics includes the following steps:

[0083] Each large section of the rope-driven hyper-redundant robot arm is connected to the motor drive box located at the root of the robot arm by three steel ropes, and the ropes are evenly distributed at 120 degrees. When the angle of the joint is zero, as shown in Figure 2 , set the three rope lengths to 0 as the initial state. Taking the cross shaft between adjacent small sections in the large section as the object, and taking the near root end as the fixed end and the far root end as the moving end, the transfer matrix of the moving end relative to the fixed end is established;

[0084] As shown in Figure 3 , assuming that the lower end is fixed and the upper end is movable, the rope length after the joint angle changes is compared with the rope length at zero angle, where d is the distance from the center of the cross shaft to the upper end or the lower end. The cross shaft between each small section rotates θ 2i-1 and θ 2i degrees around the z-axis and the x-axis respectively. The transfer matrix of the moving end O m relative to the fixed end O f can be expressed as:

[0085]

[0086] where θ 2i-1 and θ 2i are the rotation angles of the i-th section, and the pose of the moving end in the fixed end coordinate system can be expressed as:

[0087]

[0088] The screw matrix S i = (ω i , v i ) is as follows:

[0089] ω1 = (0, 0, 1) v1 = (d, 0, 0) ω2 = (1, 0, 0) v2 = (0, 0, -d) the rope length change in the cross shaft is obtained based on the transfer matrix;

[0090] The coordinates of the three rope holes of the moving end in the fixed end coordinate system are:

[0091]

[0092] wherein r is the radius of the rope hole on the disc, and α is the offset angle of the first rope hole and the x-axis, wherein any one rope hole can be specified as the first rope hole.

[0093] The rope holes in each section of a long section are in the same position, and the rope holes of the moving end in the i-th section cross shaft have the same coordinate value. The change expression of the three rope lengths relative to the moving end in a cross shaft is as follows:

[0094]

[0095] The three rope length vectors can be expressed as:

[0096] Δl i =|| m R f p i + m t f ||-2d,i = 1, 2, 3

[0097] wherein, m R f and m t f are the rotation matrix and translation vector of m T f , and 2d is the initial joint angle when the rope length is 0.

[0098] The kinematics model of the joint angle to the rope length is established based on the rope length change in the cross shaft.

[0099] Since the rope driving device is located at the root of the mechanical arm, the rope away from the base also needs to pass through all the near root ends in turn, and the rope length will also be affected by the rotation angle of the near root end. The rope length vector of the i-th section can be written as:

[0100]

[0101] wherein, is the rope length vector of the i-th long section, is the change amount of the rope length in each section of the i-th long section,

[0102] The present application has the following beneficial effects:

[0103] The accuracy of kinematics modeling of CSRM is improved: the influence of linkage error is considered in modeling, and the motion is closer to the real situation;

[0104] The complexity of kinematics modeling of CSRM is simplified: the modeling method of exponential product formula can greatly simplify the modeling process, and the linkage coordinate system does not need to be established, and the kinematics modeling efficiency is improved;

[0105] The motion of each joint on the entire arm rod of CSRM can be expressed: the modeling method of exponential product formula can describe the motion of all joint angle positions of the mechanical arm, which is convenient for the subsequent application of control, perception and planning algorithm of CSRM.

[0106] In the description of the present specification, the terms "one embodiment" and "example" and the like refer to the specific features, structures or characteristics described in connection with the embodiment or example contained in at least one embodiment or example of the present application. In the present specification, the illustrative description of the above terms does not necessarily refer to the relative embodiment or example in a suitable manner.

[0107] It must be pointed out that the above description of the embodiments is not used for limitation but only for helping to understand the core idea of the present application. Any improvement made by those skilled in the art without departing from the principles of the present application, and the alternative solutions equivalent to the present product, also belong to the protection scope of the claims of the present application.

Claims

1. A method of kinematics modeling of a segmented redundant rope-driven manipulator, the segmented redundant rope-driven manipulator comprising I macro segments, each of the macro segments consisting of J micro segments, each of the micro segments being connected by a cross shaft, characterized in that, The kinematics modeling of joint angles to configuration of forward kinematics and the kinematics modeling of joint angles to rope length of forward kinematics are both performed by using exponential product formula, wherein, in the kinematics modeling of joint angles to configuration of forward kinematics, linkage error is considered, and linkage error factor is introduced; the kinematics model of joint angles to configuration of forward kinematics is specifically as follows: wherein, represents a pose transformation from the base coordinate system to the end of the entire arm, n represents the number of the large section, θa:b indicates the angle of rotation from the a-th degree of freedom to the b-th degree of freedom, fa:b indicates the linkage factor corresponding to the a-th degree of freedom to the b-th degree of freedom, and the transfer matrix is an exponential product formula, represents the pose change of the end joint of the n-th large section caused by the cross-axis angle change, is a known quantity, and represents the initial attitude matrix of the end of the n-th large section relative to the base coordinate system;​ The transformation matrix is constructed by the screw matrix of the joint , a scale factor and a linkage error factor f The kinematics modeling of joint angles to rope length of forward kinematics includes the following steps: Each large section of the rope-driven segmented redundant manipulator is driven by a rope; Taking the cross shaft between adjacent small sections in the large section as an object, and taking the near root end as a fixed end and the far root end as a moving end, a transfer matrix of the moving end relative to the fixed end is established; Based on the transfer matrix, rope length change in the cross shaft is obtained; Based on the rope length change in the cross shaft, a kinematics model of joint angles to rope length of forward kinematics is established.

2. The method of claim 1, wherein, The kinematics modeling of joint angles to configuration of forward kinematics includes the following steps: The rope-driven segmented redundant manipulator has 2I degrees of freedom, and a linkage error factor f is introduced for each degree of freedom; The transformation matrix from the cross shaft coordinate system of each small section to the base coordinate system is calculated by using the product of the exponential product formula, and a kinematics model of joint angles to configuration of forward kinematics is established.

3. The method of claim 2, wherein, The transfer matrix is specifically as follows: wherein, denotes the screw matrix associated with the jth joint of the ith segment, is a scale factor.

4. The method of claim 3, wherein, The formula for the scale factor is: .

5. The method of claim 3, wherein, The spinor matrix and the corresponding coordinate representation is wherein, is the joint axis direction, is the vector from an arbitrary point on the same joint axis to the base coordinate system, is the corresponding anti-symmetric matrix, wherein, x, y, z direction, respectively.

6. The method of claim 1, wherein, The kinematics modeling of joint angles to rope length of forward kinematics includes the following steps: Each large section of the rope-driven segmented redundant manipulator is connected to the motor drive box located at the root of the rope-driven segmented redundant manipulator by three ropes for driving, and the ropes are uniformly distributed at 120 degrees.

7. The method of claim 6, wherein, The kinematics model of joint angles to rope length of forward kinematics is as follows: wherein, is the rope length vector of the ith segment, is the change in rope length per section in the ith segment ; wherein, denotes the change of the length of the rth rope, , and is a rotation matrix and a translation vector of the transfer matrix, 2d is the length of the rope between the two small sections connected by the cross shaft when the cross shaft does not rotate, is the coordinate of the rth rope hole on the fixed end, . .

8. The method of claim 7, wherein, Establishing the transfer matrix of the moving end relative to the fixed end includes the following steps: The cross shafts rotate about their two movable axes, respectively and The transfer matrix of the mobile end relative to the fixed end is represented as: wherein, is a translation matrix, is a pose of the mobile end in the fixed end coordinate system, is a rotation matrix, specifically: = = 。 9. The method of claim 7, wherein, The coordinates of the fixed end of the first string hole are: ​ where r is the distance from the hole to the center axis of the disc, is the offset angle for the first hole.

Images