CN109760051B - Rope length change determination method for rope-driven super-redundancy degree of freedom robot - Google Patents

Abstract

The invention provides a method for determining the change of rope length of a rope-driven super-redundant degree-of-freedom robot, which belongs to the technical field of robot control. The invention first establishes a coordinate system on the rope-driven series-parallel mechanism of the rope-driven super-redundant degree-of-freedom robot; defines the rope segment vector and obtains the force acting on the rope guide plate by the rope force from the rope segment vector; and then uses the rope force to act on the rope guide plate. The equivalent screw of the rope force is obtained from the force on the rope guide plate; the Jacobian matrix is obtained through the mapping relationship between the rope force and the equivalent moment; the rate of change of the rope length is obtained by combining the angular velocity of the rotational degree of freedom and the Jacobian matrix, and finally Integrate the resulting rate of change in rope length to obtain the change in rope length. The invention solves the problem that the control technology error of the existing rope-driven super-redundant degree-of-freedom robot is relatively large. The invention can be used for the control technology of the rope-driven super-redundant degree-of-freedom robot.

Description

A Rope Length Variation Determination Method for Rope-Driven Super-Redundant Degree-of-Freedom Robots

technical field

The invention relates to a method for determining the change of rope length of a rope-driven super-redundant degree-of-freedom robot, which belongs to the technical field of robot control.

Background technique

Rope-driven super-redundant degrees of freedom robot:

The rope-driven super-redundant degree-of-freedom robot is a series-parallel mechanism using rope as the transmission medium. This kind of robot is composed of dense kinematic joints connected in series and driven by ropes in parallel, with a large number of degrees of freedom, flexible movement, and strong movement ability in a narrow and restricted environment. Since the rope can only bear one-way force and can only restrain the mechanism in one direction, the number of ropes is generally greater than the number of degrees of freedom. Different from the joint motion control method of traditional robots, such robots need to control the length and speed of the rope. The premise of this control is to find that the rope needs to meet the motion state (rope length and rope speed) to complete the desired motion. The super-redundant degree-of-freedom robot described here is a robot composed of two-degree-of-freedom universal joints connected in series.

Inverse kinematics of a rope-driven robot:

The inverse kinematics of the rope-driven robot consists of two parts: solving the joint space motion state from the operation space motion state (end motion) and solving the rope motion state from the joint motion state. Among them, the method of solving the joint space motion state from the operation space motion state is the same as the inverse kinematics solution method of the traditional redundant robot, that is, the solution by the Jacobian matrix mapping of the velocity space or the numerical solution by the forward kinematics equation.

For the part of solving the rope motion state from the joint motion state, the traditional solution method is the rope multi-segment accumulation method. The mapping relationship between the rope length and the joint angle at a single joint is established, and the numerical method is used to solve it. Then all rope segments are superimposed to find The length of the rope is obtained. The advantage of this method is that the modeling is simple, but the solution is complicated. It involves solving nonlinear equations, the phenomenon of multiple solutions, and the accumulation of errors in the multi-segment stacking process.

SUMMARY OF THE INVENTION

In order to solve the problem that the control technology error of the existing rope-driven super-redundant degree-of-freedom robot is relatively large, the invention provides a method for determining the change of the rope length of the rope-driven super-redundant degree-of-freedom robot.

The method for determining the change of the rope length of a rope-driven super-redundant degree-of-freedom robot according to the present invention is realized by the following technical solutions:

Step 1. Establish a coordinate system on the rope-driven series-parallel mechanism of the rope-driven super-redundant degree-of-freedom robot; define the rope segment vector and obtain the force of the rope force acting on the rope guide plate from the rope segment vector;

Step 2: Obtain the equivalent screw of the rope force by using the force acting on the rope guide plate by the rope force;

Step 3: Obtain the Jacobian matrix through the mapping relationship between the rope force and the joint equivalent moment;

Step 4: Combining the angular velocity of the rotational degrees of freedom and the Jacobian matrix to obtain the rate of change of the rope length;

Step 5: Integrate the rate of change of the rope length obtained in the step 4 to obtain the change of the rope length.

The most prominent feature and significant beneficial effect of the present invention are:

The invention relates to a method for determining the change of rope length of a rope-driven super-redundant degree-of-freedom robot. Inverse kinematics is solved from the speed space to obtain the Jacobian matrix corresponding to the rope length and the rotational degree of freedom (joint) speed, and then The integral obtains the variation of the rope length, and then the rope-driven super-redundant degree-of-freedom robot can be controlled. The method of the invention is simple in operation, the obtained rope length has a unique change value and the error is small, so the control accuracy of the rope-driven super-redundant degree-of-freedom robot can also be improved. Compared with the traditional method, the method of the invention can effectively improve the rope drive The control accuracy of the hyper-redundant degree-of-freedom robot is about 20%.

Description of drawings

Figure 1 is a schematic structural diagram of a rope-driven series-parallel mechanism of a rope-driven super-redundant degree-of-freedom robot;

Figure 2 is a schematic diagram of each coordinate system established by the rope-driven series-parallel mechanism of the rope-driven super-redundant degree-of-freedom robot;

3 is a schematic diagram of a vertebral segment structure in the present invention;

Fig. 4 is the flow chart of the present invention;

1. Base, 2. Rope-driven robotic arm, 21. Vertebral segment, 211. Universal joint, 212. Upper plate, 213. Lower plate, 22. Rope.

Detailed ways

Embodiment 1: This embodiment will be described with reference to FIG. 1 and FIG. 4. A method for determining the change of rope length of a rope-driven super-redundant degree of freedom robot provided in this embodiment specifically includes the following steps:

Step 1. Establish a coordinate system on the rope-driven series-parallel mechanism of the rope-driven super-redundant degree-of-freedom robot; define the rope segment vector and obtain the force of the rope force acting on the rope guide plate from the rope segment vector;

Step 2: Obtain the equivalent screw of the rope force by using the force acting on the rope guide plate by the rope force;

Step 3: Obtain the Jacobian matrix through the mapping relationship between the rope force and the joint equivalent moment;

Step 4. Combine the angular velocity of the rotational degrees of freedom (joints) and the Jacobian matrix to obtain the rope length change rate;

Step 5: Integrate the rate of change of the rope length obtained in the step 4 to obtain the change of the rope length.

Embodiment 2: The difference between this embodiment and Embodiment 1 is that the step 1 specifically includes the following processes:

As shown in Figures 1 and 2, the rope-driven series-parallel mechanism of the rope-driven super-redundant degree-of-freedom robot includes: a base 1 and a rope-driven mechanical arm 2 arranged on the base 1; 2. Any one of the above universal joints 211 and the part between the universal joint 211 and the next universal joint 211 are defined as a vertebral segment 21; in any vertebral segment 21, the rope guide plate close to the universal joint 211 is defined as the lower one. Plate 213, the rope guide plate away from the universal joint 211 is the upper plate 212; the edges of the upper plate 212 and the lower plate 213 are evenly provided with rope holes equal to the number of the ropes 22, and each rope passes through all the vertebral segments 21 in turn. Lower plate 213 and upper plate 212 .

其在椎节坐标系{C_i-x_iy_iz_i}中的矢量为

j表示绳索的序号，椎节上j绳索通过的绳孔的序号也为j，j＝1,2,...,M；M为绳索的总数，N＝3I；位于椎节i的下板上的绳孔在惯性坐标系{O-XYZ}中的矢量记为

其在椎节坐标系{C_i-x_iy_iz_i}中的矢量为

As shown in Figure 2 and Figure 3, the vertebral segment coordinate system {C _i -x _i y _i z _i } is established with the center C _i of the lower plate of the vertebral segment as the origin, i represents the serial number of the vertebral segment, i=1,2, ...,I; I represents the total number of vertebral segments; the direction from the center C _i of the lower plate to the first rope hole in the lower plate is defined as the _xi axis direction, the direction perpendicular to the lower plate is the z _i axis direction, The y _i axis is perpendicular to both the x _i axis and the z _i axis; the DH (a general method proposed by Denavit and Hartenberg Hardenberg in 1955) is established at the center of the universal joint. The coordinate system {O ⁿ -x ⁿ y ⁿ z ⁿ }, n represents the serial number of rotational degrees of freedom, n=1,2,...,N; N represents the total number of rotational degrees of freedom, N=2I; has rotational degrees of freedom in two directions; the vector of the rope hole located on the upper plate of vertebral segment i in the inertial coordinate system {O-XYZ} is denoted as

Its vector in the vertebral coordinate system {C _i -x _i y _i z _i } is

j represents the serial number of the rope, and the serial number of the rope hole through which the j rope passes through the vertebral segment is also j, j=1,2,...,M; M is the total number of ropes, N=3I; located on the lower plate of the vertebral segment i The vector of the rope hole on the inertial coordinate system {O-XYZ} is denoted as

Its vector in the vertebral coordinate system {C _i -x _i y _i z _i } is

其计算公式为：The vector from the lower plate rope hole j of vertebral segment i+1 to the upper plate rope hole j of vertebral segment i is the rope segment vector

Its calculation formula is:

其计算公式为：The vector from the lower plate rope hole j of vertebral segment i to the upper plate rope hole j of vertebral segment i-1 is the rope segment vector

Its calculation formula is:

Then the rope force acting on the lower plate and the upper plate of the vertebral segment i is:

为绳索j作用在椎节i的下板上的作用力；为绳索j作用在椎节i的上板上的作用力；

为绳段矢量

的单位矢量；

为绳段矢量

的单位矢量；f_j表示绳索j的拉力大小。in,

is the force of the rope j acting on the lower plate of the vertebral segment i; is the force of the rope j acting on the upper plate of the vertebral segment i;

for rope segments vector

the unit vector of ;

for rope segments vector

The unit vector of ; f _j represents the pulling force of the rope j.

Other steps and parameters are the same as in the first embodiment.

绳段矢量的单位矢量

Embodiment 3: The difference between this embodiment and Embodiment 2 is that the rope segment vector unit vector of

rope segment vector unit vector of

Other steps and parameters are the same as in the first or second embodiment.

Embodiment 4: The difference between this embodiment and Embodiment 2 is that the equivalent screw of the rope force described in step 2 is specifically:

为绳索j作用在椎节i上的力在椎节i的椎节坐标系原点处产生的力旋量；f＝[f₁ f₂ … f_M]^T；

表示由绳索力向第i个椎节上力旋量映射的雅克比矩阵。Among them, S _i is the equivalent screw of the rope force generated by the rope on the vertebral segment i,

is the force screw produced by the force of the rope j acting on the vertebral segment i at the origin of the vertebral segment coordinate system of the vertebral segment i; f=[f ₁ f ₂ ... f _M ] ^T ;

Represents the Jacobian matrix that maps from the rope force to the force screw on the ith vertebral segment.

Other steps and parameters are the same as in the second embodiment.

的具体计算过程包括：Embodiment 5: This embodiment differs from Embodiment 4 in that the

The specific calculation process includes:

The rope forces on the vertebral segments are divided into two categories:

A. The ropes that drive the movement of the vertebral segment i are numbered as: j=i, I+i, 2I+i;

B. The ropes that interfere with the movement of vertebral segments i+1, i+2,...,I are numbered j=i+1,i+2,...I,I+i+1,I+i+2,...2I ,2I+i+1,2I+i+2,…3I;

When the rope j is the driving rope of the vertebral segment i, the force acting on the vertebral segment i is only the force acting on the lower plate of the vertebral segment i, namely:

在椎节i的椎节坐标系原点处产生的力旋量为：at this time,

The force screw generated at the origin of the vertebral segment coordinate system of vertebral segment i is:

When the rope j is the interference force of the vertebral segment i, the force acting on the vertebral segment i is the vector sum of the forces acting on the lower plate and the lower plate of the vertebral segment i:

在椎节i的椎节坐标系原点处产生的力旋量为：at this time,

The force screw generated at the origin of the vertebral segment coordinate system of vertebral segment i is:

Other steps and parameters are the same as in the first, second or third embodiment.

Embodiment 6: The difference between this embodiment and Embodiment 5 is that the process of obtaining the Jacobian matrix described in step 3 is:

Obtained from formulas (7) and (9) for:

The calculation method for converting Si to _τ is:

Among them, τ ⁱ is the joint equivalent moment caused by Si; J _i is the Jacobian matrix that maps the force vector on the vertebral segment _i to the joint equivalent moment, and its composition is:

Among them, z ₀ , z ₁ ,...z _2i-1 is the unit vector of the Z axis of the DH coordinate system in the inertial coordinate system, and p ₀ , p ₁ ,...p _2i is the origin of the DH coordinate system in the inertial coordinate system. vector coordinates;

The torque of all vertebral segments converted into the equivalent moment of the joint is:

Combining formula (14):

τ=J _t f (14)

Get _Jt as:

Among them, J _t is the Jacobian matrix that maps the rope force to the joint equivalent moment.

Other steps and parameters are the same as in the first, second, third, fourth or fifth embodiment.

Embodiment 7: The difference between this embodiment and Embodiments 2, 3, 4, 5 or 6 is that the specific process of obtaining the rate of change of the rope length described in step 4 includes:

The rope is replaced by force, and the principle of virtual work can be used to obtain:

并将其与式(3)带入式(16)得到：From formula (4), we get

And put it and formula (3) into formula (16) to get:

δ is a variational operator, τ _n represents the magnitude of the equivalent moment of the n-th rotational degree of freedom (joint) caused by the rope; q _n represents the angular velocity of the n-th rotational degree of freedom;

得到

则式(17)能够改写为：By formula (2) and

get

Then formula (17) can be rewritten as:

l_j为绳索j的长度，则式(18)能够转化为：because

l _j is the length of rope j, then equation (18) can be transformed into:

f ^T δl=τ ^T δq (19)

Wherein, δl=[δl 1 δl 2 … δl _M ] ^T ; τ=[τ ₁ τ ₂ … τ _N ] ^T _; δq=[δq _{1 δq 2} … δq _N ] ^T ;

Taking equation (14) into (19), we get:

f ^T δl=f ^T J _t ^T δq (20)

From formula (20), we get:

δl=J _t ^T δq (21)

与转动自由度的角速度之间的关系为：Since all constraints are steady constraints, the rate of change of rope length

The relationship with the angular velocity of the rotational degrees of freedom is:

in, The superscript "·" indicates derivation with respect to time.

When solving the inverse kinematics relationship (22), the key is to obtain the Jacobian matrix J _t . From equation (14), it can be known that J _t can be obtained through the mapping relationship between the rope force and the equivalent moment.

Other steps and parameters are the same as those in the first to sixth embodiments.

The present invention can also have other various embodiments. Without departing from the spirit and essence of the present invention, those skilled in the art can make various corresponding changes and deformations according to the present invention, but these corresponding changes and deformations are all It should belong to the protection scope of the appended claims of the present invention.

Claims (1)

1. A method for determining the length change of a rope-driven super-redundancy freedom robot is characterized by comprising the following steps:

firstly, establishing a coordinate system on a rope drive serial-parallel mechanism of a rope drive super-redundancy freedom degree robot; defining a rope segment vector and obtaining the force of the rope force acting on the rope guide plate by the rope segment vector, wherein the specific process comprises the following steps:

defining any universal joint and the part between the universal joint and the next universal joint as a vertebral joint; in any vertebral segment, the rope guide plate close to the universal joint is a lower plate, and the rope guide plate far away from the universal joint is an upper plate;

with the center C of the lower plate of the vertebral segment_iEstablishing a vertebral level coordinate system for the origin { C_i-x_iy_iz_iI represents the serial number of the vertebral segment, I is 1, 2. I represents the total number of vertebral segments; is defined by the center C of the lower plate_iThe direction pointing to the 1 st rope hole on the lower plate is x_iThe axial direction, the direction perpendicular to the lower plate, is z_iAxial direction, y_iThe axis being perpendicular to x_iAxis and z_iA shaft; establishing a D-H coordinate system { O } at the center of the gimbalⁿ-xⁿyⁿzⁿN represents a serial number of a rotational degree of freedom, and N is 1, 2. N represents the total number of rotational degrees of freedom, N ═ 2I; the vector of the rope hole on the upper plate of the vertebral segment i in the inertial coordinate system { O-XYZ } is recorded as

It is in the vertebral level coordinate system { C_i-x_iy_iz_iThe vector in (f) isj represents the serial number of the rope, and the serial number of a rope hole for the j rope to pass through on the vertebral segment is also j, j is 1, 2. M is the total number of ropes, and M is 3I; the vector of the rope hole on the lower plate of the vertebral segment i in the inertial coordinate system (O-XYZ) is recorded as

It is in the vertebral level coordinate system { C_i-x_iy_iz_iThe vector in (f) is

The vector from the lower plate rope hole j of the vertebral segment i +1 to the upper plate rope hole j of the vertebral segment i is a rope segment vector

The calculation formula is as follows:

the vector from the lower plate rope hole j of the vertebral segment i to the upper plate rope hole j of the vertebral segment i-1 is a rope segment vector

The calculation formula is as follows:

the rope forces acting on the lower and upper plates of the vertebral segment i are then:

wherein,

the force of cord j on the lower plate of vertebra segment i;

the force of the cord j on the upper plate of the vertebral segment i;

is a vector of rope segment

A unit vector of (a);

is a vector of rope segment

Unit vector of, rope segment vector

Unit vector ofRope segment vectorUnit vector of

f_jRepresenting the magnitude of the tension of the rope j;

step two, utilize the rope force to act on the equivalent momentum of the rope force of the rope deflector, the concrete process includes:

wherein S is_iThe equivalent amount of rotation of the rope force generated by the rope on the vertebral segment i,

a force momentum generated at the origin of the vertebral level coordinate system of the vertebral level i for the force of the cord j acting on the vertebral level i; f ═ f₁f₂… f_M]^T；A Jacobian matrix representing a mapping of the force momentum from the cable force to the ith vertebral level;

the above-mentioned

The specific calculation process comprises the following steps:

the cable force on the vertebral level is divided into two categories:

A. rope for driving vertebral segment i to move is numbered as follows: j ═ I, I + I,2I + I;

B. the number of the ropes disturbing the movement of the vertebral segments I +1, I +2, … I, I +1, I +2, … 2I,2I +1,2I +2, … 3I;

when cord j is the drive cord for vertebra i, the forces acting on vertebra i are:

at this time, the process of the present invention,

the amount of torque generated at the origin of the vertebral level coordinate system for vertebra i is:

when the cord j is the distracting force of the vertebra segment i, the forces acting on the vertebra segment i are:

at this time, the process of the present invention,

the amount of torque generated at the origin of the vertebral level coordinate system for vertebra i is:

step three, obtaining a Jacobian matrix through a mapping relation between the rope force and the joint equivalent moment, wherein the specific process comprises the following steps:

obtained from the formulae (7) and (9)Comprises the following steps:

S_ithe calculation method for the conversion to tau is as follows:

wherein, tauⁱIs S_iInduced joint equivalent moment; j. the design is a square_iThe Jacobian matrix which is mapped from the force vector on the vertebral level i to the joint equivalent moment comprises the following components:

wherein z is₀,z₁,…z_2i-1Z in D-H coordinate systemUnit vector of axis in inertial frame, p₀,p₁,...p_2iVector coordinates of an origin of the D-H coordinate system in an inertial coordinate system;

the force rotation quantity borne by all vertebral segments is converted into joint equivalent torque as follows:

combined formula (14):

τ＝J_tf (14)

to obtain J_tComprises the following steps:

wherein, J_tA Jacobian matrix for mapping the rope force to the joint equivalent moment;

step four, combining the angular speed of the rotational freedom degree and the Jacobian matrix to obtain the rope length change rate, and the concrete process comprises the following steps:

the rope force is replaced, and the principle of virtual work is utilized to obtain the following result:

delta is a variation operator, tau_nThe magnitude of the equivalent moment representing the nth rotational degree of freedom caused by the rope; q. q.s_nAn angular velocity representing the nth rotational degree of freedom;

is represented by the formula (2) andto obtain

Equation (17) can be rewritten as:

due to the fact that

l_jFor the length of rope j, equation (18) can be converted into:

f^Tδl＝τ^Tδq (19)

wherein δ l ═ δ l [ δ l ═ δ l₁δl₂… δl_M]^T；τ＝[τ₁τ₂… τ_N]^T；δq＝[δq₁δq₂… δq_N]^T(ii) a Substituting formula (14) into (19) yields:

δl＝J_t ^Tδq (21)

since all constraints are constant constraints, the rate of change of the rope length

The relationship with the angular velocity of the rotational degree of freedom is:

wherein,

superscript "·" denotes derivation over time;

and step five, integrating the rope length change rate obtained in the step four to obtain the change of the rope length.

Images