CN112936273B

CN112936273B - Speed stage kinematics modeling method for rope-driven flexible mechanical arm

Info

Publication number: CN112936273B
Application number: CN202110156259.5A
Authority: CN
Inventors: 梁斌; 王学谦; 杨皓强; 孟得山
Original assignee: Shenzhen International Graduate School of Tsinghua University
Current assignee: Shenzhen International Graduate School of Tsinghua University
Priority date: 2021-02-04
Filing date: 2021-02-04
Publication date: 2023-07-25
Anticipated expiration: 2041-02-04
Also published as: CN112936273A

Abstract

The invention discloses a speed stage kinematics modeling method of a rope-driven flexible mechanical arm, which comprises the following steps: a1: establishing a relational expression for solving the speed of the rope according to the joint angular speed according to the relational expression of the length change rate of the rope in each joint and the joint angular speed; a2: solving the joint angular velocity according to the given velocity of the rope by solving a relational expression of the velocity of the rope according to the joint angular velocity; a3: based on a DH coordinate system of the mechanical arm, establishing a relational expression for solving the linear speed and the angular speed of the end effector according to the joint angular speed; a4: the joint angular velocity is solved for a given end effector linear velocity and angular velocity by solving the end effector linear velocity and angular velocity relationship from the joint angular velocity. The invention can be widely used in numerical solution of mechanical arm position level kinematics inverse solution, and can completely meet the actual engineering requirement.

Description

Speed stage kinematics modeling method for rope-driven flexible mechanical arm

Technical Field

The invention relates to the technical field of robots, in particular to a speed stage kinematics modeling method of a rope-driven flexible mechanical arm.

Background

In recent years, with the gradual progress of the technology level, complicated machinery and equipment are endlessly layered, and the problems of detection, maintenance, manufacturing and assembly and the like brought along with the complicated machinery and equipment are increasingly prominent. High-end manufacturing equipment often has an extremely complex structure inside, while also having very little cavity volume, which makes the environment inside the whole equipment very limited. In such a limited environment, most of the detection and maintenance methods adopted at present are manual intervention, for example, the maintenance method of an aircraft engine is to disassemble the engine from a wing, disassemble the engine, and assemble the engine back, so that the economic loss caused by the whole process is about 40 ten thousand dollars. For extreme environments of ultralow temperature, weightlessness and strong radiation, manual intervention can greatly increase maintenance cost, and can seriously threaten personal safety of operators, such as on-orbit maintenance and maintenance of space satellites, detection and maintenance of pipeline leakage of a nuclear power station and the like. There are many occasions where the operation space is limited, for example, in the medical field, many surgical operations and minimally invasive examinations go deep into the human body to perform fine operations, and during the operation, it is required that the inner wall cannot be touched. Conventional industrial robots are required to operate in open places, and it is difficult to perform tasks in complex limited environments, so it is particularly important to study robots capable of working in complex limited environments. Flexible rope-driven mechanical arms oriented to narrow working spaces are widely focused in academia and industry.

The rope drives the arm to have extensive application prospect, have traditional industry arm does not possess characteristics, like: good bending characteristics, flexibility, and rapid response to control. The characteristics enable the device to operate in a narrow space and complete space exploration, underwater exploration, nuclear waste treatment and other high-difficulty tasks.

The rope-driven mechanical arm has more degrees of freedom and the joints are mutually coupled, so that the speed-stage kinematics modeling of the rope-driven mechanical arm is complex, and generally, the speed of a plurality of joints can be changed simultaneously due to the fact that the driving speed of one rope is changed. The speed level kinematics of the rope-driven super-redundant mechanical arm has important application in the aspects of moving target grabbing, speed level path planning, position level numerical inverse solution and the like. In the physical sense, the speed stage kinematics are the result of position stage kinematics derivation, but the rope drive super-redundant mechanical arm is characterized by joint coupling and the number of driving ropes is larger than the number of degrees of freedom, so that the analysis of the speed stage kinematics needs to pay attention to a lot of details.

Chinese patent document CN110576438A discloses a simplified kinematic solution method, device and system for a linkage flexible mechanical arm, wherein the method comprises: the structure of the flexible mechanical arm is analyzed to obtain joint variable parameters of joint segments, an equivalent motion equation and a joint segment jacobian matrix of the mechanical arm are calculated and deduced according to the joint variable parameters, the mechanical arm jacobian matrix is further obtained through the joint segment jacobian matrix, the angle of each current joint segment is further solved through inverse kinematics of the mechanical arm jacobian matrix to obtain an optimal solution of the angle of the joint segment, and finally the mechanical arm is driven to move by combining the optimal solution and the equivalent motion equation. Chinese patent document CN106844951a discloses a method for solving the inverse kinematics of a super-redundant robot based on a piecewise geometry method, comprising: the system segments the mechanical arm of the robot with n degrees of freedom into a shoulder, an elbow and a wrist, and then solves and determines the positions of a head end node So of the shoulder, a crossing node Eo of the shoulder and the elbow and each node W of the crossing node W of the elbow and the wrist according to a wrist end node T, solves the angles of each joint of the shoulder, solves the angles of each joint of the elbow, and solves the angles of each joint of the wrist. However, none of the above disclosed solutions is universally applicable to numerical solutions of mechanical arms in position-level kinematic inverse solutions, and is not enough to meet the needs of practical engineering.

The foregoing background is only for the purpose of facilitating an understanding of the principles and concepts of the invention and is not necessarily in the prior art to the present application and is not intended to be used as an admission that such background is not entitled to antedate such novelty and creativity by the present application without undue evidence prior to the present application.

Disclosure of Invention

In order to solve the technical problems, the invention provides a speed-stage kinematic modeling method for a rope-driven flexible mechanical arm, which can be widely used in a numerical solution of a position-stage kinematic inverse solution of the mechanical arm and can completely meet the actual engineering requirements.

In order to achieve the above purpose, the present invention adopts the following technical scheme:

the embodiment of the invention discloses a speed stage kinematics modeling method of a rope-driven flexible mechanical arm, which comprises the following steps:

a1: establishing a relational expression for solving the speed of the rope according to the joint angular speed according to the relational expression of the length change rate of the rope in each joint and the joint angular speed;

a2: solving the joint angular velocity according to the given velocity of the rope by solving a relational expression of the velocity of the rope according to the joint angular velocity;

a3: based on a DH coordinate system of the mechanical arm, establishing a relational expression for solving the linear speed and the angular speed of the end effector according to the joint angular speed;

a4: the joint angular velocity is solved for a given end effector linear velocity and angular velocity by solving the end effector linear velocity and angular velocity relationship from the joint angular velocity.

Preferably, if the solution of the joint angular velocity obtained in step A2 is not unique, the solution of least square with the second norm of the given rope velocity is used as the optimal solution of the joint angular velocity; if the solution of the joint angular velocity obtained by the solution in the step A4 is not unique, a least square solution with the minimum distance from the given end effector speed and angular velocity binary norms is used as the optimal solution of the joint angular velocity.

Preferably, the mechanical arm comprises a driving base, n arm rods, n joints, 3n ropes and an end effector, wherein the n joints are respectively connected between every two adjacent arm rods and between the most end part of the arm rods and the end effector, the first ends of the 3n ropes are driven by the driving base, the second ends of the 3n ropes sequentially penetrate through rope passing holes in discs arranged at two ends of the arm rods to be respectively connected with the end parts of the n joints, each joint is driven by 3 ropes, rotating brackets are respectively arranged at two ends of each arm rod and one end of the end effector, rotating shafts of the rotating brackets respectively arranged at two ends of each arm rod are mutually perpendicular, and each rotating bracket between every two adjacent arm rods and between the most end part of each arm rod and the two rotating brackets of the end effector are respectively connected on a universal joint through pin shafts to form the joint, and each joint is mutually perpendicular.

Preferably, the relation between the length change rate of the rope in each joint and the joint angular velocity in the step A1 is:

in the formula, v _i，i For the length change rate of the ith driving rope in the ith joint, v _i，i，+n For the length change rate of the i+n-th driving rope in the i-th joint, v _i，i+2n For the length change rate of the i+2n-th driving rope in the i-th joint, l _i，i For variable length of the ith rope in the ith joint, l _i，i+n Variable length, l, for the i+n rope in the i-th joint _i，i+2n Variable length, alpha, for the i+2n rope in the i-th joint _i Is the pitch angle of the ith joint, beta _i Is the yaw angle of the ith joint, w _iα Pitch rate for the ith joint; w (w) _iβ And n is the number of joints of the mechanical arm, wherein the yaw rate is the ith joint.

Preferably, the relation established in step A1 for solving the speed of the rope from the joint angular speed is:

wherein, the liquid crystal display device comprises a liquid crystal display device,v _i to drive the speed of the cord caused by the base driving the cord No. i,

preferably, in step A2, the following formula is obtained to solve the joint angular velocity according to the given velocity of the rope by solving the relational expression of the velocity of the rope according to the joint angular velocity:

wherein, the liquid crystal display device comprises a liquid crystal display device,( ⁱ J _vcj ) ⁺ is that ⁱ J _vjc Is a pseudo-inverse of the matrix of (a).

Preferably, the relational expression established in the step A3 for solving the linear velocity and the angular velocity of the end effector according to the joint angular velocity is:

in the formula, v _e V, the velocity vector of the end effector _e ＝[v _ex v _ey v _ez ] ^T ；w _e Is the angular velocity vector, w, of the end effector _e ＝[w _ex w _ey w _ez ] ^T ；w _iα Pitch rate for the ith joint; w (w) _iβ For the yaw rate of the ith joint, ⁰ J _i，vejα ∈R ⁶ represented is a joint space to working space velocity jacobian matrix of pitch angle alpha for joint i, ⁰ J _i，vejβ ∈R ⁶ represented is a joint space to working space velocity jacobian of the yaw angle beta of joint i, ⁰ J _i，vej ＝[ ⁰ J _i，vejα ⁰ J _i，vejβ ]∈R ^6×2 。

preferably, in step A4, the following equation is obtained by solving the relational expression of the linear velocity and the angular velocity of the end effector according to the joint angular velocity to solve the joint angular velocity according to the linear velocity and the angular velocity of the given end effector:

in the middle of (a) ⁰ J _vej ) ⁺ For jacobian matrix ⁰ J _vej Is a pseudo-inverse of the matrix of (a).

The embodiment of the invention discloses a numerical inverse solution method from a working space to a joint space of a rope-driven flexible mechanical arm, which comprises the following steps of:

b1: given a desired pose T _d Initializing the iteration times k;

b2: calculating the current pose T _c ；

B3: according to a given desired pose T _d And the current pose T _c Calculating a position error deltap and an attitude error deltao;

b4: determining whether the position error Δp is smaller than the maximum position tolerance error ε ₁ Whether the attitude error Deltao is smaller than the maximum attitude tolerance error epsilon ₂ If the position error Δp is less than the maximum position tolerance error ε ₁ And the attitude error delta o is smaller than the maximum attitude tolerance error epsilon ₂ Ending the iteration, otherwise, executing the step B5;

b5: solving both sides of the relation of the joint angular velocity according to the linear velocity and the angular velocity of the given end effector by the velocity-stage kinematic modeling method according to any one of claims 1 to 8 while multiplying by a unit time difference Δt to calculate a joint angle difference Δq according to the position error Δp and the attitude error Δo; then obtaining a rope length difference Deltal according to calculation by multiplying the two sides of the relation of the speed of the rope according to the joint angular speed by a unit time difference Deltat and Deltaq according to the speed stage kinematic modeling method of any one of claims 1 to 8;

b6: and B2, respectively calculating the joint angle value and the rope length value at the next iteration according to the joint angle difference delta q and the rope length difference delta l, and adding 1 to the iteration number k and returning to the step B2.

One embodiment of the invention discloses a computer-readable storage medium storing computer-executable instructions that, when invoked and executed by a processor, cause the processor to implement the above-described method of numerical inverse solution of the working space to joint space of a rope-driven flexible mechanical arm.

Compared with the prior art, the invention has the beneficial effects that: the speed stage kinematic modeling method for the rope-driven flexible mechanical arm establishes a complete speed stage kinematic model, can further solve the problem of inverse solution of the position stage kinematics of the rope-driven mechanical arm in a driving space and a joint space and a working space in a numerical integration mode based on the complete speed stage kinematic model, has universality and can be used for rope-driven mechanical arms in any configuration.

In a further scheme, the mechanical arm rod type is a non-linkage type rope driven mechanical arm connected by a vertical universal joint, and the design of the vertical arm rod can reduce the assembly difficulty and simplify the mechanical arm kinematic model.

Drawings

FIG. 1 is a flow chart of a velocity-stage kinematic modeling method of a rope-driven flexible mechanical arm according to an embodiment of the invention;

FIG. 2 is a schematic diagram of the structure of a rope-driven flexible mechanical arm according to a preferred embodiment of the present invention;

FIG. 3 is a diagram comparing the lever structure of the robotic arm of FIG. 2 with the lever structure of the prior art;

FIG. 4 is a schematic illustration of the joint structure of the robotic arm of FIG. 2;

FIG. 5 is a schematic diagram of the establishment of a DH coordinate system of the robotic arm of FIG. 2;

FIG. 6a is a schematic illustration of the structure of the 5 th joint of the mechanical arm of FIG. 2;

FIG. 6b is a schematic diagram of the establishment of an arm lever coordinate system of the robotic arm of FIG. 2;

FIG. 7 is a velocity-stage kinematic analysis chart;

fig. 8 is a flow chart of a numerical inverse solution method from a working space to a joint space of a rope-driven flexible mechanical arm according to an embodiment of the invention.

Detailed Description

In order to make the technical problems, technical schemes and beneficial effects to be solved by the embodiments of the present invention more clear, the present invention is further described in detail below with reference to the accompanying drawings and the embodiments. It should be understood that the specific embodiments described herein are for purposes of illustration only and are not intended to limit the scope of the invention.

It will be understood that when an element is referred to as being "mounted" or "disposed" on another element, it can be directly on the other element or be indirectly on the other element. When an element is referred to as being "connected to" another element, it can be directly connected to the other element or be indirectly connected to the other element. In addition, the connection may be for both the fixing action and the circuit communication action.

It is to be understood that the terms "length," "width," "upper," "lower," "front," "rear," "left," "right," "vertical," "horizontal," "top," "bottom," "inner," "outer," and the like are merely for convenience in describing embodiments of the invention and to simplify the description by referring to the figures, rather than to indicate or imply that the devices or elements referred to must have a particular orientation, be constructed and operated in a particular orientation, and thus are not to be construed as limiting the invention.

Furthermore, the terms "first," "second," and the like, are used for descriptive purposes only and are not to be construed as indicating or implying a relative importance or implicitly indicating the number of technical features indicated. Thus, a feature defining "a first" or "a second" may explicitly or implicitly include one or more such feature. In the description of the embodiments of the present invention, the meaning of "plurality" is two or more, unless explicitly defined otherwise.

Through analytical studies, the inventors found that the following disadvantages exist in chinese patent document CN 110576438A: (1) The linkage flexible arm keeps continuous in the linkage large section and presents an arc line, and the flexibility of the linkage flexible arm is inferior to that of the non-linkage mechanical arm. The arm rods of the non-linkage mechanical arm move independently, so that the device can adapt to various complex environments; (2) only the modeling process of location level kinematics is completed. The jacobian matrix of the linkage type mechanical arm is solved through DH parameters and is used for analysis and inverse solution of position-level kinematics, and the relation of velocity vectors of a driving space, a joint space and a working space is not systematically considered. The following disadvantages exist in chinese patent document CN106844951 a: (1) The arm type is a parallel type, i.e. the movement of the joint is pitch-yaw-pitch (PYYP), which increases the modeling complexity and assembly difficulty of the mechanical arm. (2) The inverse solution of the position-level kinematics is simply researched, and the solution method is very special and has no universality in practical use. Aiming at the defect that the linkage type rope-driven mechanical arm is inflexible, the invention uses the rope-driven mechanical arm with each section connected by the rigid universal joint and independently moving, so that the mechanical arm can adapt to various complex and changeable environments. Aiming at the fact that no complete speed stage kinematic analysis and modeling process exists at present, the speed stage kinematics of the non-linkage type rope driven mechanical arm are systematically analyzed, and the speed stage kinematics relation among a driving space, a joint space and a working space is included. Aiming at the difficulty in modeling and assembly caused by a parallel mechanical arm, the invention has the advantages that the analysis object is a vertical arm, the assembly can be simplified, and the modeling calculation amount can be reduced. Aiming at special position level kinematic inverse solutions, the analysis of the invention on the speed level can be universally used for numerical solutions of the position level kinematic inverse solutions of the mechanical arms, and can completely meet the actual engineering requirements.

Based on the defects existing in the research, the invention provides a novel mechanical arm type-vertical type arm lever, a speed stage kinematic model is built aiming at the arm type, and the speed stage kinematic relation among the driving space, the joint space and the working space is analyzed in detail.

As shown in fig. 1, an embodiment of the present invention discloses a speed stage kinematic modeling method of a rope-driven flexible mechanical arm, including:

a2: solving the joint angular velocity according to the velocity of a given rope by solving a relational expression of the velocity of the rope according to the joint angular velocity;

a4: the joint angular velocity is solved for a given linear and angular velocity of the end effector by solving a relationship between the linear and angular velocities of the end effector from the joint angular velocity.

For ease of understanding, the following preferred embodiment takes a 5-joint rope-driven mechanical arm as an example, and teaches how to perform velocity-level kinematic modeling of a rope-driven flexible mechanical arm.

The following mentioned driving space refers to the physical parameters related to the ropes in the rope driving mechanical arm, such as: the invention relates to a rope driving tension, a rope driving speed, a rope changing length and the like, and the rope changing length and the rope driving speed are discussed in the invention; the joint space refers to physical parameters related to joints in the rope-driven mechanical arm, such as: joint moment, joint angle, joint angular velocity, etc., discussed herein are joint angle and joint angular velocity; working space refers to physical parameters related to the tail end in the rope driven mechanical arm, such as external torque applied to the tail end, external force applied to the tail end, tail end speed and the like, and the tail end speed and the angular speed are discussed in the invention.

1. Rope drives super redundant arm's structure

Referring to fig. 2, a robotic arm including five joints is illustrated, including a drive base 30,5 arms 11, 12, 13, 14, 15,5 joints 21, 22, 23, 24, 25, 15 cables, and an end effector 16 (also arm 6), wherein the end effector 16 of fig. 2 is configured to clamp a weight 40. Referring to fig. 3 and 4,5 joints are respectively connected between every two adjacent arm rods and between the arm rod 15 and the end effector 16 at the end, the first ends of the 15 ropes are driven by the driving base 30, the second ends of the 15 ropes sequentially pass through rope passing holes 61 on discs arranged at two ends of the arm rods to be respectively connected at the end parts of 5 joints, wherein each joint is driven by 3 ropes, two end parts of the arm rods and one end of the end effector are respectively provided with rotating brackets, the rotating shafts 101 and 102 of the rotating brackets respectively arranged at the two end parts of each arm rod are mutually perpendicular, and the two rotating brackets of every two adjacent arm rods and the two rotating brackets of the arm rod and the end effector at the end are respectively connected on the universal joint 51 through pin shafts 52 to form joints, and the two rotating brackets forming the joints are mutually perpendicular.

The pitch and yaw motions of each gimbal require three ropes to drive so that the maximum of 15 ropes passes through arm 11, the minimum of 3 passes through arm 15, and the three passes through arm 15 ultimately controls the end effector. In this embodiment, the derivation of the present invention is applicable to any number of joints, each arm is connected by means of a universal joint, one universal joint is one joint, the driving box drives the rope to move, the rope drives the arm to move, and the arm can perform pitching motion and yawing motion around the joint, so that one joint has two degrees of freedom. The arm lever of this arm is shown on the right side of fig. 3, and the first rotation axis 101 and the second rotation axis 102 of the arm lever are vertical, not parallel like the left side, which can increase the success rate in the actual assembly of the arm lever of the arm and simplify the derived model. The arms are connected by a universal joint 51, as shown in fig. 4.

For the robotic arm model shown in fig. 2, the arm bars are vertical, following the classical DH modeling method in robotics. The DH coordinate system shown in FIG. 5 can be established, and the DH parameter table shown in Table 1, in which θ, can be obtained by the rule of variation between DH coordinate systems _l Is the included angle of the common vertical line of two adjacent connecting rods, d _l Is the distance between the common vertical lines of two adjacent connecting rods connected by the first joint angle (pitch angle or yaw angle), a _l Is the length of the connecting rod alpha _{DH_l} Is the torsion angle of the connecting rod, L ₀ L is the length between the centers of adjacent joints _g Distance α from the 5 th joint (the joint at the end adjacent to the end effector) to the center of the end effector _i Is the pitch angle of the ith joint, beta _i Is the yaw angle of the ith joint, and the i of the ith joint can take values of 1,2,3,4 and 5.

TABLE 1 parameter Table of DH coordinate System of rope-driven mechanical arm

2. Position-level kinematic modeling of rope-driven super-redundant mechanical arm

Taking the example of the 5 th joint in which the 5 th arm is connected to the end effector, the relationship between the joint inner rope length and the joint pitch angle and yaw angle is analyzed as shown in fig. 6a and 6b, in which the end effector 16 is connected and driven by the 5 th rope 65, the 10 th rope 610 and the 15 th rope 615 in fig. 6a, in which the 5 th rope 65 is fixed by the fixed end 651.

As can be seen from FIG. 6b, point A ₁ And point A ₃ The two ropes passing through the two points are the same rope. Coordinate system {3} midpoint A ₃ Coordinates of {1} midpoint a of coordinate system ₁ The coordinates of (c) are the same:

wherein r is ₀ -distance from the round hole of the cloth rope to the central axis of the arm lever;

-angle between the origin of the coordinate system and the line connecting the point to be solved and the positive direction of the X-axis, +.>Hereinafter, unless otherwise indicated,/>And->All means this.

From the geometrical meaning of the homogeneous coordinate matrix, the point A ₃ The coordinates in the coordinate system {1} are:

in the formula, h is the lifting lug height of the single-section arm lever.

Pass through point A ₁ And point A ₃ The rope length of (2) is:

since no special properties are used in the derivation, equation (3) holds true for any corresponding point on the roping disk. Thus, there are:

wherein:representing the included angle between the connecting line of the origin of the coordinate system and the round hole of the mth rope and the positive direction of the X axis, and the whole rope-driven super-redundancy mechanical arm is driven by 15 ropes. l (L) _i，m Representing the variable length of the mth rope in the ith joint.

The three ropes driving the joint i are numbered i, i+5, i+10, so that the total length of the three ropes is:

wherein, I ₀ -a fixed length of rope in each arm;

l _k，i -variable length of the ith rope in the kth joint;

l _i -total length of the i-th rope.

Because the ropes are distributed uniformly, there areIf all joint angles alpha of the mechanical arm are known _i ，β _i (i=1, 2,3,4, 5), the length of 15 ropes can be obtained from the formulas (4) and (5).

3. Rope-driven super-redundant mechanical arm speed stage kinematics modeling

The velocity-stage kinematic modeling can provide guidance for the velocity motion planning of the mechanical arm, and can obtain the numerical inverse solution of the position-stage control by simultaneously carrying out numerical integration (namely, simultaneously multiplying delta t) on two sides of the velocity-stage model, which has important application in the end track planning.

In the speed-stage kinematic analysis, three spaces, namely a driving space, a joint space and a working space, are needed to be considered for the rope-driven mechanical arm. The drive space contains a matrix of the rate of change of length in the joints of the individual ropes, the concept of which is used to distinguish from the commonly understood rope speeds. In the research of the super-redundant rope drive mechanical arm, the rope speed refers to the rope motion speed of the whole rope driven by a motor, and the analysis shows that the joints are coupled, so that the analysis needs to be conducted by introducing the length change rate in the joints of the rope in order to research the relation between the angular speed of the joints and the rope speed. The joint space contains a matrix of joint angular velocities of the various joints of the robotic arm. The working space comprises a matrix formed by the linear velocity and the angular velocity of the tail end of the mechanical arm. The relationship between the three is shown in fig. 7.

3.1 mapping of joint space to drive space

v _i，m The calculation formula of (2) is formula (6), and the physical meaning is: the rate of change of length of the drive rope at the mth in the ith joint.

In the formula, v _i，m -the rate of change of length of the drive rope number m in the ith joint;

Δl _i，m -the variation length of the mth drive rope in the ith joint in Δt time.

Attention is paid to distinguishing rope speed from the rate of change of length in rope joints, rope speed v _m Is the driving motor is directAnd the speed of the whole rope is connected with the driving rope. Obviously v _i，m And v _m Are not equal and satisfy (7)

In the formula, v _m -the rope speed caused by the drive motor driving the rope No. m;

wherein i takes all joints through which the m-th rope passes. The value of i in the formula (7) can be obtained by looking up table 2.

Table 2 corresponding values of m and i

The mapping relation between the joint space and the driving space is known joint angular velocity, and the length change rate and the rope velocity in the rope joint are solved. From the analysis of the position level kinematics, it is known that the pitch angle speed and yaw angle speed of the ith joint are directly influenced by the length change rate v in the rope joint _i，i ，v _i，i+5 ，v _i，i+10 . From formula (4), it can be seen that:

wherein w is _iα Pitch rate of joint i;

w _iβ yaw rate of joint i.

Note that since the joint angle in the rope length calculation formula and the joint angle in the D-H coordinate system are somewhat different, l in formula (8) _i，m Unlike formula (4), it is necessary to add α in formula (4) _i By-alpha _i Alternatively, the pitch rate in formula (8)Both the degree and yaw rate are for the D-H coordinate system. Then, m in the formula (8) is equal to i, i+5, i+10, and the mapping relation (6) in fig. 7 can be obtained, and the mapping relation is written into a matrix form, namely:

from the analytical expression of equation (4), it can be deduced

In the middle of

The coefficient matrix of the angular velocity in the formula (9) is recorded as ⁱ J _vcj The jacobian matrix representing the velocity from the joint space to the driving space in the joint i can be solved by combining the formula (10) (11) as shown in the formula (13) ⁱ J _vcj Is included in the set of parameters.

Further, the speed v of any ith rope can be solved by combining the combined type (7), (8), (10), (11), (12) and combining the table 2 _i 。v _i Satisfying the formula (14).

Wherein:

/>

3.2 mapping of drive space to joint space

The mapping relation from the driving space to the joint space is the length change rate in the known rope joint, and the joint angular velocity is solved.

In practice, if the rope speed is known, it is possible to first decouple the rate of change of the intra-articular length of the rope in each segment of the joint and then solve for the angular joint speed. The decoupling process is to solve the pitch and yaw angular velocities of the 1 st joint by controlling the three rope velocities of the 1 st joint, then solve the length change rate in the 2 nd joint by utilizing the pitch and yaw angular velocities of the 1 st joint and the three rope velocities of the 1 st joint, and then solve the pitch and yaw angular velocities of the 2 nd joint by utilizing the length change rate in the rope joint, thus solving the angular velocities of all joints according to the rope velocities.

The process of solving the joint angular velocity from the rate of change of length in the rope joint is described in detail below.

Given the rate of change of length in the rope joint by equation (9), solving the joint angular velocity is a solution of a typical non-homogeneous system of linear equations.

Order the ⁱ v＝[v _i，i v _i，i+5 v _i，i+10 ] ^T The condition for the exact solution of equation (9) is rank ([ A ] ⁱ J _vcj ， ⁱ v])＝rank( ⁱ J _vcj ) From an objective point of view, the situation where the equation is not solved exactly is not present, since the speeds of the three ropes are not coordinated, and then a certain rope break must occur.But in practice, due to the presence of measurement errors, the measurement results ⁱ v may result in an inaccurate solution to the equation, which is analyzed later.

As can be seen from the formula (13), ⁱ J _vcj always of full rank, i.e. rank ⁱ J _vcj ) =2, therefore, [ w ] with an exact solution _iα w _iβ ] ^T Only the only solution is provided, and the solution structure is as follows:

[w _iα w _iβ ] ^T ＝( ⁱ J _vcj ) ^- ·[v _i，i v _i，i+5 v _i，i+10 ] ^T (18)

the middle part of (18) ⁱ J _vcj ) ^- Is that ⁱ J _vcj Is a generalized inverse matrix of (a). ⁱ J _vcj The first row transformation P and the first column transformation Q can be converted into the simplest shape, namely:

generalized inverse matrix [ ] ⁱ J _vcj ) ^- Can be written as:

( ⁱ J _vcj ) ^- ＝Q[I ₂ x _2×1 ]P (20)

wherein x is _2×1 Is an arbitrary 2-dimensional column vector. Obviously, the generalized inverse matrix is not unique, and one special generalized inverse matrix is called pseudo inverse matrix ⁱ J _vcj ) ⁺ (also referred to as the left inverse matrix at this time), the expression is:

( ⁱ J _vcj ) ⁺ ＝(( ⁱ J _vcj ) ^T ( ⁱ J _vcj )) ^-1 ( ⁱ J _vcj ) ^T (21)

when the filling condition with the solution of equation (9) is not satisfied, namely rank ([ ⁱ J _vcj ， ⁱ v])≠rank( ⁱ J _vcj ) When the incompatible equation sets have no exact solution, only the least square solution can be obtained, and among a plurality of least square solutions, the incompatible equation sets can be used forTo select a least squares solution with a minimum 2 norms, which is called the best least squares solution.

The solution structure of the best least squares solution is:

[w _iα w _iβ ] ^T ＝( ⁱ J _vcj ) ⁺ ·[v _i，i v _i，i+5 v _i，i+10 ] ^T (22)

in (22), the process is as follows ⁱ J _vcj ) ⁺ Is that ⁱ J _vjc The expression of the pseudo-inverse matrix of (2) is the expression (21).

Thus, in combination with the above two cases, whether the equations are compatible or not, whether the solution is an exact solution or not, can be solved by equation (19), and this conclusion can be directly used in the subsequent solution of the non-homogeneous linear system of equations.

3.3 mapping of joint space to working space

The joint space to working space mapping is in fact known joint angular velocity and solves for the linear and angular velocities of the end tool. Under a classical D-H coordinate system, a velocity Jacobian (Jacobian) matrix from joint space to working space can be solved constructively.

In the case of the rope-driven super-redundant robot arm, all joints are rotary joints, so the analysis of Jacobian matrix is performed below by taking rotary joints as an example. Taking the coordinate system {0} in fig. 5 as the base coordinate system, then:

⁰ z _l-1 ＝ ⁰ T _l-1 (1：3，3) (24)

⁰ T _l-1 a homogeneous transformation matrix representing the coordinate system { l-1} relative to the coordinate system {0 }; ⁰ z _l-1 representative of ⁰ T _i-1 A sub-matrix located in column 3 (i.e., a representative row preceding the comma in brackets, a representative column following the comma, a: B representing from row a or column to row B or column);

wherein the method comprises the steps of ⁰ T _l-1 (1:3, 3) by the formula (25) and combining the parameter table of the DH coordinate system of the rope-driven mechanical arm in the table 1, the right multiplication of the homogeneous transformation matrix is used for calculation.

⁰ ρ _l→2n ＝ ⁰ p _2n - ⁰ p _l-1 ＝ ⁰ T _2n (1：3，4)- ⁰ T _l-1 (1：3，4) (26)

Wherein c represents cos, s represents sin, θ _l 、α _{DH_l} 、a _l 、d _l DH parameters of the rope-driven mechanical arm are respectively adopted, ⁰ ρ _l→2n is a representation of the position vector under the coordinate system {0} with the origin of the coordinate system { l-1} pointing to the origin of the coordinate system {2n }, ⁰ p _2n is a representation of the origin of the coordinate system {2n } under the coordinate system {0}, ⁰ p _l-1 is a representation of the origin of the coordinate system { l-1} under the coordinate system {0 }.

n represents the total number of joints or levers of the mechanical arm, in this embodiment 2n=10, in particular, for joint 1 there is:

the first column of the Jacobian matrix can be found by combining equations (24) and (26), and the calculation formula is equation (28):

the relationship between the velocity of the tip and the angular velocity of the joint is therefore:

in the method, in the process of the invention,v _e terminal velocity vector, v _e ＝[v _ex v _ey v _ez ] ^T ；

w _e -tip angular velocity vector, w _e ＝[w _ex w _ey w _ez ] ^T 。

w _l Represents the rotational angular velocity of the coordinate system { l }, where l is the pitch angular velocity of the joint when it is odd and the yaw angular velocity of the joint when it is even.

As a result of: v _e ＝[v _ex v _ey v _ez ] ^T ，w _e ＝[w _ex w _ey w _ez ] ^T In order to compare the equation (29) with the pitch angle rate w described previously _iα And yaw rate w _iβ In this embodiment, the expression (29) is rewritten as:

wherein, the liquid crystal display device comprises a liquid crystal display device, ⁰ J _i，vejα ∈R ⁶ represented is a joint space to working space velocity jacobian matrix of pitch angle alpha for joint i, ⁰ J _i，vejβ ∈R ⁶ represented is a joint space to working space velocity jacobian of the yaw angle beta of joint i, ⁰ J _i，vej ＝[ ⁰ J _i，vejα ⁰ J _i，vejβ ]∈R ^6×2 。

3.4 mapping of working space to Joint space

The mapping relationship from the working space to the joint space is actually that the linear velocity and the angular velocity of the end tool are known, and the joint angular velocity is solved, which is a problem of solving a non-homogeneous linear equation.

Abbreviated as (30)

In the method, in the process of the invention, ⁰ J _vej -a velocity jacobian matrix of the manipulator joint space to the working space.

As can be seen from the formula (31), ⁰ J _vej ＝[ ⁰ J _1，vej ⁰ J _2，vej ⁰ J _3，vej ⁰ J _4，vej ⁰ J _5，vej ]∈R ^6×10 . When rank (J) _vej ) Not equal to 6, i.e. when the row is full, the mechanical arm is in a singular state, and at this time, the terminal speed of any given mechanical arm cannot be achieved in most cases, and only a few specific terminal speeds exist, so that the mechanical arm can be achieved through the driving joint speed.

As the conclusion from the analysis of the drive space to joint space mapping described above, whether or not the non-homogeneous linear equation has an exact solution, can be represented by a pseudo-inverse matrix. However, the jacobian matrix analyzed in this section has two points, one is that the number of rows of the matrix is larger than the number of columns, and the other is that the matrix does not have to be full of rows. This results in different forms of pseudo-inverse and different construction of the solution space for the exact solution.

(1) Jacobian matrix ⁰ J _vej When the row is full, i.e. the mechanical arm does not generate singular, the matrix analysis theory shows that the jacobian matrix ⁰ J _vej Pseudo inverse matrix ⁰ J _vej ) ⁺ (also referred to as the right inverse matrix at this time) satisfies:

( ⁰ J _vej ) ⁺ ＝( ⁰ J _vej ) ^T ( ⁰ J _vej ( ⁰ J _vej ) ^T ) ^-1 (32)

the solution space is as follows:

where z is an arbitrary 10-dimensional column vector. Wherein the first term is a special solution part of a non-homogeneous system of linear equations and the second term is a general solution part of a homogeneous system of linear equations. The dimension of the solution space w is 10-rank% ⁰ J _vej ) Of course, the dimension of the solution space can also be determined by rank (I ₁₀ -( ⁰ J _vej ) ⁺ · ⁰ J _vej ) Direct observation of =4.

Equation (33) illustrates how, for a given tip speed, an infinite set of joint angular velocities is involved in choosing a set of joint angular velocities, which is an optimization problem. When all joints of the mechanical arm work normally, a minimum norm solution should be selected; when a certain joint of the mechanical arm cannot work normally, a group of solutions should be selected so that the angular velocity of the joint is as small as possible, and the driving speed is reduced to protect the joint.

(2) Jacobian matrix ⁰ J _vej When the rank is not full, i.e. when the mechanical arm is singular, only a specific end speed can make the equation (31) have an accurate solution, and only the general case of no accurate solution is discussed here. Without an exact solution, only the solution with the smallest distance from the given terminal velocity two norms can be found, which may not be unique, but if it is also defined that the norms of the solution are smallest, then the solution is unique. The solution structure is as follows:

wherein, pseudo inverse matrix ⁰ J _vej ) ⁺ Cannot be calculated by equation (32) because ⁰ J _vej ( ⁰ J _uej ) ^T And no longer a full rank matrix. At this time, calculating pseudo-inverse matrix ⁰ J _vej ) ⁺ Can be matrix-formed ⁰ J _vej Full rank decomposition is done. Let us assume rank @ ⁰ J _vej ) R, then there must be a matrix M _6×r Sum matrix N _r×10 So that ⁰ J _vej ＝M _6×r N _r×10 Wherein M is _6×r Is a full rank matrix, N _r×10 Is a full rank matrix. The two matrixes obtained by full rank decomposition are not unique, and any group is selected. Pseudo inverse matrix ⁱ J _vcj ) ⁺ The expression of (2) is:

( ⁰ J _vcj ) ⁺ ＝N ^T (NN ^T ) ^-1 (M ^T M) ^-1 M ^T (35)

it is notable that the decomposition mode of full rank decomposition is not unique, but the calculated pseudo-inverse matrix ⁱ J _vcj ) ⁺ Is unique.

To sum up:

the two sides of the formula (36) are multiplied by Δt to obtain the numerical inverse solution of the working space and the joint space, and although the vector z in the formula (36) can take any vector, from the viewpoint of minimum driving cost, z should be a zero vector, namely:

the inverse solution of the values from the working space to the joint space is shown in fig. 8, and specifically comprises the following steps:

b1: for a rope-driven flexible mechanical arm, the initial pose (the pose includes both the position and the pose) of the rope-driven flexible mechanical arm can be arbitrary, and then the pose matrix T which is expected to be reached of the mechanical arm is given _d The manipulator arm is to be brought from an initial pose to a desired pose (including both the tip position and the tip pose). Initializing the iteration number k=0;

b2: then, according to the joint angle of the current mechanical arm, calculating a current pose matrix T _c ；

B3: then, the difference is made, the position error deltap and the attitude error deltao are calculated,

b4: if the position error is less than the maximum position tolerance error epsilon ₁ And the attitude error is smaller than the maximum attitude tolerance error epsilon ₂ Then the procedure ends. If one or both errors are greater than the maximum tolerated error, then the next iteration is performed, step B5 is performed.

B5: by (37) (velocity step for working space to joint space)Kinematics) are multiplied by the difference deltat in unit time at the same time to obtainThe same applies to both sides of equation (14) (velocity-level kinematics from joint space to rope space) multiplied by the difference Δt per unit time to give Δl=j _vcj Δq，

B6: the rope length value at the next moment in time (i.e. at the next iteration) is then: l (k+1) =l (k) +Δl, l (k) is a rope length value at the current time (i.e. when referring to the current iteration), and the mechanical arm joint angle value obtained after the rope is pulled is: q (k+1) =q (k) +Δq, q (k) being the joint angle value at the current time. Then the iteration number k is increased by 1, because the joint angle of the mechanical arm moves to q (k+1) at the next moment, and then the pose matrix T is calculated continuously _c I.e. back to step B2. And the method is repeated until the position error and the attitude error are simultaneously smaller than the maximum tolerance error.

Wherein J in FIG. 8 _vcj The expression of (2) is:

the speed stage kinematic modeling method of the rope-driven flexible mechanical arm provided by the preferred embodiment of the invention has the following advantages:

(1) The assembly success rate is increased, and the modeling complexity is reduced. The rope-driven mechanical arm provided by the invention has the advantages that the arm rod is vertical, so that the success rate in actual assembly can be increased, and the universal joint always takes pitching-yawing as a motion module, so that the modeling complexity can be reduced.

(2) And establishing a complete speed stage kinematic model of the rope-driven mechanical arm. The invention provides a complete speed stage kinematic model aiming at a rope-driven mechanical arm model connected by a universal joint, which comprises bidirectional mapping of a driving space and a joint space and bidirectional mapping of the joint space and a working space. And the rate of change of the length in the rope joint is proposed and the relation between this physical quantity and the rope speed is found.

(3) A generic solution to the inverse solution of position-level kinematics is proposed. According to the complete speed level kinematic model established by the invention, the inverse solution problem of the position level kinematics of the rope driven mechanical arm in the driving space and the joint space and the working space can be solved by a numerical integration mode, and the method has universality and can be used for rope driven mechanical arms with any configuration.

The foregoing is a further detailed description of the invention in connection with the preferred embodiments, and it is not intended that the invention be limited to the specific embodiments described. It will be apparent to those skilled in the art that several equivalent substitutions and obvious modifications can be made without departing from the spirit of the invention, and the same should be considered to be within the scope of the invention.

Claims

1. A speed stage kinematics modeling method of a rope-driven flexible mechanical arm is characterized in that,

the mechanical arm comprises a driving base, n arm rods, n joints, 3n ropes and an end effector, wherein the n joints are respectively connected between every two adjacent arm rods and between the arm rod at the most end and the end effector, the first ends of the 3n ropes are driven by the driving base, the second ends of the 3n ropes sequentially penetrate through rope passing holes in discs arranged at two ends of the arm rods to be respectively connected with the end parts of the n joints, each joint is driven by 3 ropes, rotating supports are respectively arranged at two ends of the arm rods and one end of the end effector, the rotating shafts of the rotating supports respectively arranged at two ends of each arm rod are mutually perpendicular, the rotating supports between every two adjacent arm rods and between the arm rod at the most end and the rotating supports of the end effector are respectively connected on a universal joint through pin shafts to form the joints, and the rotating supports between the rotating supports of the joints are mutually perpendicular;

the speed stage kinematic modeling method comprises the following steps:

2. The method according to claim 1, wherein if the solution of the joint angular velocity obtained by the solution in step A2 is not unique, a least square solution with the smallest two norms of the velocity of the given rope is used as the optimal solution of the joint angular velocity; if the solution of the joint angular velocity obtained by the solution in the step A4 is not unique, a least square solution with the minimum distance from the given end effector speed and angular velocity binary norms is used as the optimal solution of the joint angular velocity.

3. The speed stage kinematic modeling method according to claim 1, wherein the relation between the length change rate of the rope in each joint and the joint angular velocity in step A1 is:

in the formula, v _i，i For the length change rate of the ith drive rope in the ith joint, v _i，i+n For the length change rate of the i+n-th driving rope in the i-th joint, v _i，i+2n For the length change rate of the i+2n-th driving rope in the i-th joint, l _i，i For variable length of the ith rope in the ith joint, l _i，i+n Variable length, l, for the i+n rope in the i-th joint _i，i+2n Variable length, alpha, for the i+2n rope in the i-th joint _i Is the pitch angle of the ith joint, beta _i Is the yaw angle of the ith joint, w _iα Pitch rate for the ith joint; w (w) _iβ And n is the number of joints of the mechanical arm, wherein the yaw rate is the ith joint.

4. A speed stage kinematic modeling method according to claim 3, characterized in that the relation established in step A1 to solve the speed of the rope from the joint angular speed is:

5. a speed stage kinematic modeling method according to claim 4, characterized in that in step A2, by solving the relation of the speed of the rope from the joint angular speed, the following formula is obtained to solve the joint angular speed from the given speed of the rope:

6. The method of modeling speed stage kinematics according to claim 1, wherein the relationship established in step A3 for solving the linear speed and the angular speed of the end effector according to the joint angular speed is:

in the formula, v _e V, the velocity vector of the end effector _e ＝[v _ex v _ey v _ez ] ^T ；w _e Is the angular velocity vector, w, of the end effector _e ＝[w _ex w _ey w _ez ] ^T ；w _iα Pitch rate for the ith joint; w (w) _iβ For the yaw rate of the ith joint, ⁰ J _i，vejα ∈R ⁶ represented is a joint space to working space velocity jacobian matrix of pitch angle alpha for joint i, ⁰ J _i，vejβ ∈R ⁶ represented is a joint space to working space velocity jacobian of the yaw angle beta of joint i, ⁰ J _i，vej ＝[ ⁰ J _i，vejα ⁰ J _i，vejβ ]∈R ⁶ ^×2 。

7. the method of speed stage kinematic modeling according to claim 6, wherein in step A4, the following equation is obtained by solving the relationship between the linear velocity and the angular velocity of the end effector from the joint angular velocity to solve the joint angular velocity from the linear velocity and the angular velocity of the given end effector:

8. The numerical inverse solution method from the working space to the joint space of the rope-driven flexible mechanical arm is characterized by comprising the following steps of:

b1: given a desired pose T _d Initializing the iteration times k;

b2: calculating the current pose T _c ；

b5: solving both sides of the relation of the joint angular velocity according to the linear velocity and the angular velocity of the given end effector by the velocity-stage kinematic modeling method according to any one of claims 1 to 7 while multiplying by a unit time difference Δt to calculate a joint angle difference Δq according to the position error Δp and the attitude error Δo; then obtaining a rope length difference Deltal according to calculation by multiplying the two sides of the relation of the speed of the rope according to the joint angular speed by a unit time difference Deltat and Deltaq according to the speed stage kinematic modeling method of any one of claims 1 to 7;

9. A computer readable storage medium storing computer executable instructions that, when invoked and executed by a processor, cause the processor to implement the method of working space to joint space numerical inverse solution of a rope driven flexible robotic arm of claim 8.