CN112975925A - Rope-driven snakelike mechanical arm motion data processing method containing rope hole gaps - Google Patents

Rope-driven snakelike mechanical arm motion data processing method containing rope hole gaps Download PDF

Info

Publication number
CN112975925A
CN112975925A CN202110179166.4A CN202110179166A CN112975925A CN 112975925 A CN112975925 A CN 112975925A CN 202110179166 A CN202110179166 A CN 202110179166A CN 112975925 A CN112975925 A CN 112975925A
Authority
CN
China
Prior art keywords
rope
joint
length
mechanical arm
solving
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202110179166.4A
Other languages
Chinese (zh)
Other versions
CN112975925B (en
Inventor
张越
谷海宇
王东辉
李文博
张海博
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Harbin Institute of Technology
Xidian University
Original Assignee
Harbin Institute of Technology
Xidian University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Harbin Institute of Technology, Xidian University filed Critical Harbin Institute of Technology
Priority to CN202110179166.4A priority Critical patent/CN112975925B/en
Publication of CN112975925A publication Critical patent/CN112975925A/en
Application granted granted Critical
Publication of CN112975925B publication Critical patent/CN112975925B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/06—Programme-controlled manipulators characterised by multi-articulated arms
    • B25J9/065—Snake robots
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/10—Programme-controlled manipulators characterised by positioning means for manipulator elements
    • B25J9/104—Programme-controlled manipulators characterised by positioning means for manipulator elements with cables, chains or ribbons
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Manipulator (AREA)

Abstract

The invention belongs to the technical field of robot control, and discloses a method for processing motion data of a rope-driven snake-shaped mechanical arm with a rope hole gap, which classifies rope holes and ropes on the snake-shaped mechanical arm and defines a gap coefficient by the relation between the actual rope length and the ideal rope length; fitting the clearance coefficient by utilizing a Chebyshev polynomial; solving the theoretical rope length by using the clearance coefficient and the actual length of the rope, solving the joint motion state by using the theoretical rope length, and solving the working space state by using the joint space to carry out forward kinematics solution; and then solving the joint state by the working space state, calculating the theoretical rope length by the joint angle, and calculating the actual rope length by utilizing the clearance coefficient to carry out inverse kinematics solution. The method is simple in operation, and can improve the control precision of the rope-driving serpentine arm with the rope hole gap, and compared with the traditional method without considering the rope hole gap, the method can effectively improve the control precision of the rope-driving serpentine arm with the rope hole gap by 20%.

Description

Rope-driven snakelike mechanical arm motion data processing method containing rope hole gaps
Technical Field
The invention belongs to the technical field of robot control, and particularly relates to a motion data processing method of a rope-driven snakelike mechanical arm with a rope hole gap.
Background
At present, the rope containing the rope hole gap drives the snake-shaped mechanical arm: rope drives snakelike arm is a continuous type robot, compares in traditional rigid robot, and rope drives snakelike arm and has good environmental suitability, especially can nimble motion in unstructured environment and narrow and small space, is used widely in fields such as medical treatment operation, disaster rescue, space exploration, narrow and small space interior maintenance equipment at present. The rope-driven snake-shaped mechanical arm coordinately controls the lengths of the plurality of ropes to adjust the joint angles, so that the working space pose of the rope-driven snake-shaped mechanical arm is controlled. The kinematic model is used for describing the mapping relation of rope motion, joint motion and Cartesian space motion and is the basis for motion control of the rope-driven snake-shaped mechanical arm.
The degree of freedom and the number of driving ropes of the rope-driven snake-shaped mechanical arm are large, the working pose corresponds to various joint motions due to the zero-space motion of the joints, the joint motions and the rope motions are mutually coupled nonlinear mapping, and the difficulty of modeling and solving kinematics is increased. In addition, the difference between the actual rope length and the nominal rope length is caused by the rope-hole clearance, and the method has important influence on the pose calibration and control precision of the serpentine mechanical arm.
Rope-driven serpentine manipulator kinematics: the rope-driven serpentine mechanical arm kinematics comprises two parts: positive kinematics and inverse kinematics. Wherein, the positive kinematics part solves the joint motion state by the rope motion state and solves the operation space motion state by the joint space motion state; the inverse kinematics part is to solve the joint space motion state from the operation space motion state and solve the rope motion state from the joint motion state.
At present, some methods for solving the kinematics of the rope-driven snake-shaped mechanical arm exist, but the methods do not consider the kinematics solution error caused by the rope hole gap, and do not eliminate the influence of the rope hole gap on the control technology, so that the control precision of the rope-driven snake-shaped mechanical arm is poor.
Through the above analysis, the problems and defects of the prior art are as follows:
(1) in the prior art, the rope containing the rope hole gap drives the snake-shaped mechanical arm to move, and the error of controlling the accurate data of the snake-shaped mechanical arm driven by the rope is large.
(2) In the prior art, the numerical solution accuracy of an overconstrained nonlinear equation set containing measurement disturbance is greatly influenced by a termination condition.
(3) In the prior art, a complex rope driving mechanism does not have a modeling and calculation model related to a rope hole.
The difficulty in solving the above problems and defects is:
(1) the precision of the joint angle of the rope-driven snake-shaped mechanical arm is sensitive to the rope length value, and the influence of the change of the length of the rope on the joint angle and the tail end posture positioning precision is large.
(2) When the setting error of the termination condition is large, the calculation result is inaccurate, and when the setting error of the termination condition is small, the solution of 'dead loop' is easy to fall into the extreme point.
(3) The modeling and calculation of the rope hole model relate to multiple factors such as the material property of the rope, the geometric property of the rope hole and the like, and are difficult to consider comprehensively.
The significance of solving the problems and the defects is as follows: under the condition of no photoelectric measurement of an encoder, the joint angle positioning precision of the rope-driven snake-shaped mechanical arm can be improved by one order of magnitude.
Disclosure of Invention
Aiming at the problems in the prior art, the invention provides a motion data processing method of a rope-driven snakelike mechanical arm with a rope hole gap.
The invention is realized in such a way that a rope-driven snakelike mechanical arm motion data processing method with rope hole gaps comprises the following steps:
step one, classifying rope holes and ropes on the snake-shaped mechanical arm; defining a clearance coefficient according to the relation between the actual rope length and the ideal rope length;
step two, fitting the clearance coefficient by utilizing Chebyshev polynomial
Figure BDA0002941647910000021
With respect to q2i、q2i-1The unknown function of (2);
step three, solving the working space state by the joint space state;
step four, solving the joint state according to the working space state;
calculating the theoretical rope length by using the joint angle, and calculating the actual rope length by using the clearance coefficient;
solving the theoretical rope length according to the actual rope length by utilizing the clearance coefficient, and solving the joint motion state according to the theoretical rope length;
and step seven, performing kinematics solution on the rope-driven snake-shaped mechanical arm with the rope hole gap, combining the step three and the step six to solve the positive kinematics, and combining the step four and the step five to solve the inverse kinematics.
Further, the first step is to classify the rope holes and the ropes on the snake-shaped mechanical arm; the clearance coefficient is defined by the relation of the actual rope length and the ideal rope length, and the specific process comprises the following steps:
rope holes in the snake-shaped mechanical arm are divided into two types: a rope locking hole and a rope threading hole. The rope locking hole is positioned at the tail end of the rope, so that the rope is fixedly connected with the vertebral segment plate; the rope threading hole is used for limiting the position of the rope on the mechanical arm and guiding the rope to the fixing hole. A rope-hole gap is formed at the contact part of the guide hole and the rope. The rope hole clearance makes the difference between the actual length of rope and the theoretical length that calculates according to the rope hole center.
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;
depending on the spatial position of the ropes, each rope can be divided into three parts: joint rope section, vertebra festival rope section and base rope section. The joint rope section is a rope part distributed among different vertebral segments, and the length of the joint rope section is determined by the corresponding joint corner; the vertebral segment rope section is a rope part positioned between the upper plate and the lower plate of the same vertebral segment, and the length of the rope part is fixed; the base rope section is a rope portion on the base, and the length variation of the rope portion is equal to the sum of the length variations of the joint rope sections. The rope length error caused by the rope hole clearance is mainly caused by the accumulation of the length error of the joint rope section.
The theoretical rope length is calculated by taking the center of each rope hole as the end point of each rope section, and the sum of the distances between the hollow points of the adjacent rope holes is the theoretical rope length. The actual rope length is the length of the rope center line, and because the rope is bent along the hole contour at the rope hole contact part, and the bending center of the rope does not coincide with the center of the rope hole according to the rope continuous contact via hole model, an error exists between the actual length and the theoretical length of the joint rope section. And introducing a clearance coefficient to represent the relationship between the actual length of the rope segment and the calculated length:
Figure BDA0002941647910000031
wherein the content of the first and second substances,
Figure BDA0002941647910000041
calculating the length of the corresponding rope k at the joint i;
Figure BDA0002941647910000042
is the actual length of the corresponding rope k at joint i.
Calculating the length of the rope segment in the formula (1)
Figure BDA0002941647910000043
Can be turned by the joint angle q of the universal joint i2iAnd q is2i-1And (4) calculating. Actual length of rope segment
Figure BDA0002941647910000044
Is composed of straight line part outside the rope hole contact area and curved line part in the rope hole contact area, and is influenced by joint angle, rope hole gap, rope position in hole and other factorsInfluence, it is difficult to obtain its analytical expression.
Further, fitting the clearance coefficient by utilizing Chebyshev polynomial in the second step
Figure BDA0002941647910000045
With respect to q2i、q2i-1The specific process of the unknown function of (2) is as follows:
the recursion relationship of the chebyshev polynomial is as follows:
Figure BDA0002941647910000046
wherein x ∈ [ -1,1]，n＝1、2、3……N，Tn(x) Is an nth order polynomial on x.
Since x ∈ [ -1,1] in chebyshev polynomial (2), the joint angle can be normalized:
Figure BDA0002941647910000047
wherein the content of the first and second substances,
Figure BDA0002941647910000048
is the upper bound of the joint angle j,
Figure BDA0002941647910000049
the lower bound of the joint angle j.
Figure BDA00029416479100000410
Angle q of rotation with joint2iAnd q is2i-1The relationship (2) is regarded as a recursion relationship, and a Chebyshev polynomial is adopted for fitting each joint corner. Thus can obtain
Figure BDA00029416479100000411
With respect to q2iAnd q is2i-1The fitting function of the binary chebyshev polynomial of (a) is:
Figure BDA00029416479100000412
wherein the content of the first and second substances,
Figure BDA00029416479100000413
and
Figure BDA00029416479100000414
are each q2i-1And q is2iThe normalized joint angle is obtained after calculation through the process shown in the formula (3);
Figure BDA00029416479100000415
is composed of
Figure BDA00029416479100000416
About
Figure BDA00029416479100000417
And
Figure BDA00029416479100000418
the chebyshev fit function of (a).
The weight coefficient C (n) in the formula (4) can be solved through finite measurement data by using a least square method2i-1,n2i). When defining
Figure BDA00029416479100000419
The square error of the fit is:
Figure BDA0002941647910000051
wherein p and q are each independently
Figure BDA0002941647910000052
And
Figure BDA0002941647910000053
sample numbers of (1), P and Q are respectively
Figure BDA0002941647910000054
And
Figure BDA0002941647910000055
the number of samples.
And (3) calculating partial derivatives of two ends of the formula (5), and obtaining the partial derivatives according to the orthogonality principle of the Chebyshev polynomial:
Figure BDA0002941647910000056
wherein the content of the first and second substances,
Figure BDA0002941647910000057
according to extreme requirements, i.e.
Figure BDA0002941647910000058
Can ask for
Figure BDA0002941647910000059
The value of (A) is as follows:
Figure BDA00029416479100000510
further, in the third step, the working space state is solved from the joint space state, and the specific process is as follows:
the pose of the end is represented by the pose of the tool coordinate system in the task space, and the homogeneous transformation matrix between adjacent D-H coordinate systems can be represented as follows according to the definition mode of the D-H parameters:
Figure BDA00029416479100000511
from equation (8), the end pose can be calculated from the joint angle:
TE＝T0·0T1·1T2…J-2TJ-1·J-1TJ·JTE (9)
wherein, T0Is a homogeneous transformation matrix of the base coordinate system relative to the global coordinate system,JTEis a tool coordinate system relative to a D-H coordinate system { CJ-xJyJzJThe homogeneous transformation matrix of.
The processes shown in the formulas (8) and (9) are processes for calculating the terminal pose from the joint angle. The recursion algorithm of the speed of each moving body of the mechanical arm can be expressed by a three-dimensional vector method as follows:
Figure BDA0002941647910000061
wherein the content of the first and second substances,
Figure BDA0002941647910000062
respectively, are the representative vectors of the angular velocity and the origin velocity of the D-H coordinate system j in the coordinate system j.
Figure BDA0002941647910000063
Is the attitude rotation matrix of coordinate system j relative to coordinate system j-1,
Figure BDA0002941647910000064
is the location vector of the origin of coordinate system j in coordinate system j-1.
The recursive algorithm of the acceleration of each moving body of the mechanical arm can be expressed by a three-dimensional vector method as follows:
Figure BDA0002941647910000065
wherein the content of the first and second substances,
Figure BDA0002941647910000066
the angular acceleration and the origin acceleration of the D-H coordinate system j are respectively represented vectors in the coordinate system j.
The processes shown in equations (8) - (11) are positive kinematics processes that solve the working space state transitions from the joint space states.
Further, the joint state is solved by the working space state in the fourth step, and the specific process is as follows:
the inverse kinematics process of the transition from the working space state to the joint state involves a plurality of redundant degrees of freedom, and the inverse kinematics solution is performed by using a pseudo-inverse jacobian matrix of the velocity space.
Working space velocity v and joint angular velocity
Figure BDA0002941647910000067
The mapping relationship between the two is as follows:
Figure BDA0002941647910000068
wherein the content of the first and second substances,
Figure BDA0002941647910000069
is a joint jacobian matrix.
Since the joint space dimension is larger than the working space dimension, the solution is based on equation (12)
Figure BDA00029416479100000610
The process comprises the following steps:
Figure BDA00029416479100000611
wherein the content of the first and second substances,
Figure BDA0002941647910000071
is JqIs inverse to M-P, eta is dimension and
Figure BDA0002941647910000072
the same vector, η, is generally determined by a motion optimization objective function.
The angular velocity of the joint can be obtained from the equation (13), and the angle of the joint and the angular acceleration are respectively obtained by
Figure BDA0002941647910000073
Is obtained by time integration and time differentiation.
Further, the fifth step calculates the theoretical rope length by the joint angle, and calculates the actual rope length by utilizing the clearance coefficient
Cord segment vector with cord k positioned between the inferior lamina of vertebra segment i and the superior lamina of vertebra segment i-1
Figure BDA0002941647910000074
Can be expressed as:
Figure BDA0002941647910000075
wherein the content of the first and second substances,
Figure BDA0002941647910000076
is the vector of the central position of the lower plate rope hole k of the vertebra segment i,
Figure BDA0002941647910000077
is the vector of the central position of the superior plate cord hole k of the vertebra segment i-1.
In formula (1)
Figure BDA0002941647910000078
Can be obtained from the formula (14), i.e.
Figure BDA0002941647910000079
From equation (14) the calculated length l of the rope k can be determinedkComprises the following steps:
Figure BDA00029416479100000710
wherein the content of the first and second substances,
Figure BDA00029416479100000711
the length of the fixed rope segment on the vertebral segment.
The relationship between the rope motion state and the articulation state is given by equation (15):
Figure BDA00029416479100000712
wherein f isR(q) determining the implicit function of the length of the cord with respect to the angle of the joint according to equation (15), JRIs a rope-joint jacobian matrix.
Since the constraint between the rope and the joint is a geometric constraint, JRCan be expressed as:
Figure BDA00029416479100000713
wherein the content of the first and second substances,
Figure BDA00029416479100000714
is the joint jacobian matrix of the vertebral level i,
Figure BDA00029416479100000715
is the cord jacobian matrix of vertebra segment i.
Figure BDA00029416479100000716
And
Figure BDA00029416479100000717
the mechanism configuration is completely determined, and the calculation formula is as follows:
Figure BDA0002941647910000081
Figure BDA0002941647910000082
wherein the content of the first and second substances,
Figure BDA0002941647910000083
from the formula (1), the actual length of the rope segment is
Figure BDA0002941647910000084
The actual rope length is then:
Figure BDA0002941647910000085
further, the sixth step of solving the theoretical rope length from the actual rope length by using the clearance coefficient, and solving the joint motion state from the theoretical rope length comprises the following specific processes:
using the above-determined gap coefficient
Figure BDA0002941647910000086
The theoretical rope length is solved from the actual rope length:
Figure BDA0002941647910000087
wherein the content of the first and second substances,
Figure BDA0002941647910000088
is the segment vector of cord k between the inferior lamina of vertebra segment p and the superior lamina of vertebra segment p-1.
The process of solving the joint angle by the length of the joint rope section is the inverse process of the formula (16):
Figure BDA0002941647910000089
wherein the content of the first and second substances,
Figure BDA00029416479100000810
is fR(ii) an inverse function of (c.),
Figure BDA00029416479100000811
is JRM-P inverse of (1).
The lengths of the rope sections at the universal joints between the vertebral segments i and the vertebral segments i +1 are respectively as follows:
Figure BDA0002941647910000091
wherein, g1(q2i-1,q2i)、g2(q2i-1,q2i)、g3(q2i-1,q2i) Can be determined by equation (14).
Rewriting formula (23) to obtain:
Figure BDA0002941647910000092
numerical solution of (24) by Newton iteration using g1、g2、g3With respect to q2i-1、q2iThe Hessian matrix and the Jacobin matrix can calculate the joint angle q2i-1And q is2i。
Further, the seventh step is to perform kinematic solution on the rope-driven snake-shaped mechanical arm with the rope hole gap, the sixth step is combined with the sixth step to solve the positive kinematics, and the fifth step is combined with the fourth step to solve the inverse kinematics.
Another object of the present invention is to provide a rope-driven serpentine robot motion data processing system including a rope hole gap for implementing the data processing method, the rope-driven serpentine robot motion data processing system including:
the rope hole and rope classifying module is used for classifying the rope holes and the ropes on the snake-shaped mechanical arm; defining a clearance coefficient according to the relation between the actual rope length and the ideal rope length;
the joint corner unknown function fitting module is used for fitting the unknown function of the clearance coefficient relative to the joint corner by utilizing a Chebyshev polynomial;
the working space state solving module is used for solving the working space state by using the acquired unknown function of the joint corner;
the joint state solving module is used for solving the joint state according to the working space state;
the joint motion state solving module is used for calculating the theoretical rope length by using the joint angle and calculating the actual rope length by using the clearance coefficient; the clearance coefficient is used for solving the theoretical rope length according to the actual rope length, and the joint motion state is solved according to the theoretical rope length;
and the mechanical arm kinematics solving module is used for carrying out kinematics solving on the rope-driven snake-shaped mechanical arm with the rope hole gap to solve the positive rope kinematics data and the inverse rope kinematics data.
The invention also aims to provide a rope-driven snake-shaped mechanical arm, which is used for implementing the rope-driven snake-shaped mechanical arm movement data processing method with the rope hole gap.
Another object of the present invention is to provide a robot carrying the rope driven serpentine manipulator.
It is another object of the present invention to provide a program storage medium for receiving user input, the stored computer program causing an electronic device to execute the method for processing data of motion of a rope-driven serpentine robotic arm having a rope hole gap.
By combining all the technical schemes, the invention has the advantages and positive effects that: the method and the system for processing the motion data of the rope-driving snake-shaped mechanical arm with the rope hole gap analyze the influence of the rope hole gap on the calculation of the rope length, apply Chebyshev polynomial fitting gap coefficient (the relation between the theoretical rope length and the actual rope length) on an unknown function of a joint angle, further perform kinematic solution on the rope-driving snake-shaped mechanical arm with the rope hole gap, and further control the rope-driving snake-shaped arm with the rope hole gap. The method is simple in operation, and after errors caused by space-saving gaps are analyzed, the obtained rope length and the obtained rope length change value are unique and the errors are very small, so that the control precision of the rope-driving serpentine arm with the rope hole gaps can be improved, and compared with a traditional method without considering the rope hole gaps, the method can effectively improve the control precision of the rope-driving serpentine arm with the rope hole gaps by 20%.
The invention provides an inverse kinematics algorithm of the snake-shaped mechanical arm under the condition of measurement error, which can ensure that the numerical calculation result has higher stability and calculation precision. The calculation method has strong robustness and can adapt to the measurement noise which is 3 times higher than that of the common sensor.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present application, the drawings needed to be used in the embodiments of the present application will be briefly described below, and it is obvious that the drawings described below are only some embodiments of the present application, and it is obvious for those skilled in the art that other drawings can be obtained from the drawings without creative efforts.
FIG. 1 is a diagram illustrating a method for processing motion data of a rope-driven serpentine manipulator with a rope hole gap according to an embodiment of the present invention
FIG. 2 is a schematic structural diagram of a rope-driven serpentine manipulator arm body mechanism with a rope hole gap according to an embodiment of the present invention;
FIG. 3 is a schematic diagram of a single cone structure provided by an embodiment of the present invention;
FIG. 4 is a schematic diagram of a rope driven serpentine manipulator base structure with rope hole gaps according to an embodiment of the present invention;
FIG. 5 is a schematic diagram of an actual cord position versus an ideal cord position provided by an embodiment of the present invention;
in fig. 2-4: 1. a vertebral segment; 2. a rope; 3. a universal joint shaft; 4. a rope locking mechanism; 5. a vision sensor; 6. a joint; 7. a vertebral upper plate; 8. a vertebral segment support column; 9. a lower lamina of the vertebral segment; 10. a bearing seat; 11. a rope locking hole; 12. a stringing hole; 13. tightening the screw; 14. a DC servo motor; 15. an inner ring module; 16. an outer ring module; 17. a transition rope pulley group; 18. fixing the cross beam; 19. a rope bending center; 20. the center of the rope hole; 21. a rope hole profile.
In fig. 5: -. practical rope segment center line; -calculating the centreline for the rope portion.
FIG. 6 is a schematic view of a cone coordinate system of a rope driven serpentine robotic arm having rope hole gaps according to an embodiment of the present invention;
fig. 7 is a schematic diagram of a method for processing motion data of a rope-driven serpentine manipulator with a rope hole gap according to an embodiment of the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention more apparent, the present invention is further described in detail with reference to the following embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention.
Aiming at the problems in the prior art, the invention provides a method for processing the motion data of a rope-driven snake-shaped mechanical arm with a rope hole gap, and the invention is described in detail below with reference to the attached drawings.
The method for processing the motion data of the rope-driven snakelike mechanical arm with the rope hole clearance estimates the error between the actual rope length and the calculated rope length by introducing the rope length clearance coefficient; evaluating the rope length clearance coefficient through a Chebyshev polynomial; a stable kinematics solving algorithm of the rope-driven snake-shaped robot is designed based on the rope hole gap model. Specifically, firstly, rope holes and ropes on the snake-shaped mechanical arm are classified, and a clearance coefficient is defined according to the relation between the actual rope length and the ideal rope length; fitting the clearance coefficient by utilizing a Chebyshev polynomial; solving the theoretical rope length by using the clearance coefficient and the actual length of the rope, solving the joint motion state by using the theoretical rope length, and solving the working space state by using the joint space to carry out forward kinematics solution; and then solving the joint state by the working space state, calculating the theoretical rope length by the joint angle, and calculating the actual rope length by utilizing the clearance coefficient to carry out inverse kinematics solution. The invention solves the problem that the kinematics resolving and control technology error of the existing rope-driven snake-shaped mechanical arm with the rope hole gap is larger. The invention can be used for the control technology of the rope-driven snake-shaped mechanical arm with the rope hole gap.
Preferably, as shown in fig. 1, a method for processing motion data of a rope-driven serpentine manipulator with a rope hole gap according to an embodiment of the present invention includes the following steps:
s101, classifying rope holes and ropes on the snake-shaped mechanical arm; the clearance factor is defined by the relation of the actual rope length to the ideal rope length.
And S102, fitting an unknown function of the clearance coefficient relative to the joint rotation angle by utilizing a Chebyshev polynomial.
And S103, solving the working space state by the joint space state.
And S104, solving the joint state according to the working space state.
And S105, calculating the theoretical rope length by using the joint angle, and calculating the actual rope length by using the clearance coefficient.
And S106, solving the theoretical rope length according to the actual length of the rope by utilizing the clearance coefficient, and solving the joint motion state according to the theoretical rope length.
S107, solving the kinematics of the rope-driven snake-shaped mechanical arm with the rope hole gap, combining the step S103 with the step S106 to solve the positive kinematics, and combining the step S104 with the step S105 to solve the inverse kinematics.
The method for searching for a transcription factor binding site provided by the present invention can be implemented by other steps, and the method for searching for a transcription factor binding site provided by the present invention of fig. 1 is only one specific example.
The technical solution of the present invention will be further described with reference to the following embodiments and accompanying drawings.
Example 1
The method for processing the motion data of the rope-driven snakelike mechanical arm with the rope hole gaps comprises the following steps:
step one, classifying rope holes and ropes on the snake-shaped mechanical arm; the clearance factor is defined by the relation of the actual rope length to the ideal rope length.
Step two, fitting clearance coefficient c by utilizing Chebyshev polynomiali kWith respect to q2i、q2i-1Is unknown.
And step three, solving the working space state according to the joint space state.
And step four, solving the joint state according to the working space state.
And step five, calculating the theoretical rope length by using the joint angle, and calculating the actual rope length by using the clearance coefficient.
And step six, solving the theoretical rope length according to the actual length of the rope by utilizing the clearance coefficient, and solving the joint motion state according to the theoretical rope length.
And seventhly, performing kinematics solution on the rope-driven snake-shaped mechanical arm with the rope hole gap.
The first step specifically comprises the following steps:
the rope holes on the snake-shaped mechanical arm are divided into two types as shown in figure 3: a rope locking hole and a rope threading hole. The rope locking hole is positioned at the tail end of the rope, so that the rope is fixedly connected with the vertebral segment plate; the rope threading hole is used for limiting the position of the rope on the mechanical arm and guiding the rope to the fixing hole. A rope-hole gap is formed at the contact part of the guide hole and the rope. The rope hole clearance makes the difference between the actual length of rope and the theoretical length that calculates according to the rope hole center.
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.
Fig. 2 (in the figure, 1 is a vertebral segment; 2 is a rope; 3 is a universal joint rotating shaft; 4 is a rope locking mechanism; 5 is a vision sensor; 6 is a joint; 7 is an upper vertebral segment plate; 8 is a vertebral segment supporting column; 9 is a lower vertebral segment plate), fig. 3 (in the figure, 2 is a rope; 3 is a universal joint rotating shaft; 7 is an upper vertebral segment plate; 9 is a lower vertebral segment plate; 2 is a rope; 10 is a bearing seat; 11 is a rope locking hole; 12 is a rope through hole; 13 is a set screw), and fig. 4 (in the figure, 2 is a rope; 14 is a DC servo motor; 15 is an inner ring module; 16 is an outer ring module; 17 is a transition rope wheel set; 18 is a fixed beam).
Depending on the spatial position of the ropes, each rope can be divided into three parts: joint rope section, vertebra festival rope section and base rope section. The joint rope section is a rope part distributed among different vertebral segments, and the length of the joint rope section is determined by the corresponding joint corner; the vertebral segment rope section is a rope part positioned between the upper plate and the lower plate of the same vertebral segment, and the length of the rope part is fixed; the base rope section is a rope portion on the base, and the length variation of the rope portion is equal to the sum of the length variations of the joint rope sections. The rope length error caused by the rope hole clearance is mainly caused by the accumulation of the length error of the joint rope section.
As shown in figure 5 (2, rope, 19, rope bending center, 20, rope hole center, 21 and rope hole outline in the figure), the theoretical rope length is calculated by taking the rope hole center as the end point of each rope segment, and the sum of the distances between the hollow points of the adjacent rope holes is the theoretical rope length. The actual rope length is the length of the rope center line, and because the rope is bent along the hole contour at the rope hole contact part, and the bending center of the rope does not coincide with the center of the rope hole according to the rope continuous contact via hole model, an error exists between the actual length and the theoretical length of the joint rope section. And introducing a clearance coefficient to represent the relationship between the actual length of the rope segment and the calculated length:
Figure BDA0002941647910000141
wherein the content of the first and second substances,
Figure BDA0002941647910000142
calculating the length of the corresponding rope k at the joint i;
Figure BDA0002941647910000143
is the actual length of the corresponding rope k at joint i.
Calculating the length of the rope segment in the formula (1)
Figure BDA0002941647910000144
Can be turned by the joint angle q of the universal joint i2iAnd q is2i-1And (4) calculating. Actual length of rope segment
Figure BDA0002941647910000145
The linear part outside the rope hole contact area and the curve part of the rope hole contact area are influenced by multiple factors such as joint angles, rope hole gaps, positions of ropes in holes and the like, and an analytical expression of the linear part and the curve part is difficult to obtain.
In the second step provided by the invention, the Chebyshev polynomial is used for fitting the clearance coefficient
Figure BDA0002941647910000146
With respect to q2i、q2i-1The specific procedure of the unknown function of (1) is as follows.
The recursion relationship of the chebyshev polynomial is as follows:
Figure BDA0002941647910000151
wherein x ∈ [ -1,1]，n＝1、2、3……N，Tn(x) Is an nth order polynomial on x.
Since x ∈ [ -1,1] in chebyshev polynomial (2), the joint angle can be normalized:
Figure BDA0002941647910000152
wherein the content of the first and second substances,
Figure BDA0002941647910000153
is the upper bound of the joint angle j,
Figure BDA0002941647910000154
the lower bound of the joint angle j.
Figure BDA0002941647910000155
Angle q of rotation with joint2iAnd q is2i-1The relationship (2) is regarded as a recursion relationship, and a Chebyshev polynomial is adopted for fitting each joint corner. Thus can obtain
Figure BDA0002941647910000156
With respect to q2iAnd q is2i-1The fitting function of the binary chebyshev polynomial of (a) is:
Figure BDA0002941647910000157
wherein the content of the first and second substances,
Figure BDA0002941647910000158
and
Figure BDA0002941647910000159
are each q2i-1And q is2iObtaining a normalized joint angle after calculation through the process shown in the formula;
Figure BDA00029416479100001510
is composed of
Figure BDA00029416479100001511
About
Figure BDA00029416479100001512
And
Figure BDA00029416479100001513
the chebyshev fit function of (a).
Weight coefficient C (n) in formula can be solved by finite measurement data by using least square method2i-1,n2i). When defining
Figure BDA00029416479100001514
The square error of the fit is:
Figure BDA00029416479100001515
wherein p and q are each independently
Figure BDA00029416479100001516
And
Figure BDA00029416479100001517
sample numbers of (1), P and Q are respectively
Figure BDA00029416479100001518
And
Figure BDA00029416479100001519
the number of samples.
And (3) calculating partial derivatives of two ends of the formula (5), and obtaining the partial derivatives according to the orthogonality principle of the Chebyshev polynomial:
Figure BDA00029416479100001520
wherein the content of the first and second substances,
Figure BDA00029416479100001521
according to extreme requirements, i.e.
Figure BDA0002941647910000161
Can ask for
Figure BDA0002941647910000162
The value of (A) is as follows:
Figure BDA0002941647910000163
in the third step provided by the invention, the concrete solving process for solving the working space state by the joint space state is as follows:
the pose of the end is represented by the pose of the tool coordinate system in the task space, and the homogeneous transformation matrix between adjacent D-H coordinate systems can be represented as follows according to the definition mode of the D-H parameters:
Figure BDA0002941647910000164
from the formula, the end pose can be calculated from the joint angle:
TE＝T0·0T1·1T2…J-2TJ-1·J-1TJ·JTE (9)
wherein, T0Is a homogeneous transformation matrix of the base coordinate system relative to the global coordinate system,JTEis a tool coordinate system relative to a D-H coordinate system { CJ-xJyJzJThe homogeneous transformation matrix of.
Equation (2-11) -the procedure shown is a procedure of calculating the tip pose from the joint angle. The recursion algorithm of the speed of each moving body of the mechanical arm can be expressed by a three-dimensional vector method as follows:
Figure BDA0002941647910000165
wherein the content of the first and second substances,
Figure BDA0002941647910000166
respectively, are the representative vectors of the angular velocity and the origin velocity of the D-H coordinate system j in the coordinate system j.
Figure BDA0002941647910000167
Is the attitude rotation matrix of coordinate system j relative to coordinate system j-1,
Figure BDA0002941647910000168
is the location vector of the origin of coordinate system j in coordinate system j-1.
The recursive algorithm of the acceleration of each moving body of the mechanical arm can be expressed by a three-dimensional vector method as follows:
Figure BDA0002941647910000171
wherein the content of the first and second substances,
Figure BDA0002941647910000172
the angular acceleration and the origin acceleration of the D-H coordinate system j are respectively represented vectors in the coordinate system j.
The processes shown in equations (8) - (11) are positive kinematics processes that solve the working space state transitions from the joint space states.
In the fourth step provided by the invention, the resolving process for resolving the joint state by the working space state is as follows:
the inverse kinematics process of the transition from the working space state to the joint state involves a plurality of redundant degrees of freedom, and the inverse kinematics solution is performed by using a pseudo-inverse jacobian matrix of the velocity space.
Working space velocity v and joint angleSpeed of rotation
Figure BDA0002941647910000173
The mapping relationship between the two is as follows:
Figure BDA0002941647910000174
wherein the content of the first and second substances,
Figure BDA0002941647910000175
is a joint jacobian matrix.
Since the joint space dimension is larger than the working space dimension, the solution is based on equation (12)
Figure BDA0002941647910000176
The process comprises the following steps:
Figure BDA0002941647910000177
wherein the content of the first and second substances,
Figure BDA0002941647910000178
is JqIs inverse to M-P, eta is dimension and
Figure BDA0002941647910000179
the same vector, η, is generally determined by a motion optimization objective function.
The angular velocity of the joint can be obtained from the equation (13), and the angle of the joint and the angular acceleration are respectively obtained by
Figure BDA00029416479100001710
Is obtained by time integration and time differentiation.
In the fifth step provided by the invention, the theoretical rope length is calculated by the joint angle, and the process of calculating the actual rope length by utilizing the clearance coefficient is as follows:
as shown in fig. 6, cord k is located at the cord segment vector between the inferior lamina of vertebra segment i and the superior lamina of vertebra segment i-1
Figure BDA0002941647910000181
Can be expressed as:
Figure BDA0002941647910000182
wherein the content of the first and second substances,
Figure BDA0002941647910000183
is the vector of the central position of the lower plate rope hole k of the vertebra segment i,
Figure BDA0002941647910000184
is the vector of the central position of the superior plate cord hole k of the vertebra segment i-1.
In formula (1)
Figure BDA0002941647910000185
Can be obtained from the formula (14), i.e.
Figure BDA0002941647910000186
From equation (14) the calculated length l of the rope k can be determinedkComprises the following steps:
Figure BDA0002941647910000187
wherein the content of the first and second substances,
Figure BDA0002941647910000188
the length of the fixed rope segment on the vertebral segment.
The relationship between the rope motion state and the articulation state is given by equation (15):
Figure BDA0002941647910000189
wherein f isR(q) determining the implicit function of the length of the cord with respect to the angle of the joint according to equation (15), JRIs a rope-joint jacobian matrix.
Since the constraint between the rope and the joint is a geometric constraint, JRCan be expressed as:
Figure BDA00029416479100001810
wherein the content of the first and second substances,
Figure BDA00029416479100001811
is the joint jacobian matrix of the vertebral level i,
Figure BDA00029416479100001812
is the cord jacobian matrix of vertebra segment i.
Figure BDA00029416479100001813
And
Figure BDA00029416479100001814
the mechanism configuration is completely determined, and the calculation formula is as follows:
Figure BDA00029416479100001815
Figure BDA0002941647910000191
wherein the content of the first and second substances,
Figure BDA0002941647910000192
Figure BDA0002941647910000193
from the formula (1), the actual length of the rope segment is
Figure BDA0002941647910000194
The actual rope length is then:
Figure BDA0002941647910000195
in the sixth step provided by the invention, the theoretical rope length is solved from the actual length of the rope by utilizing the clearance coefficient, and the process of solving the joint motion state from the theoretical rope length is as follows:
using the above-determined gap coefficient
Figure BDA0002941647910000196
The theoretical rope length is solved from the actual rope length:
Figure BDA0002941647910000197
wherein the content of the first and second substances,
Figure BDA0002941647910000198
is the segment vector of cord k between the inferior lamina of vertebra segment p and the superior lamina of vertebra segment p-1.
The process of solving the joint angle by the length of the joint rope section is the inverse process of the formula (16):
Figure BDA0002941647910000199
wherein the content of the first and second substances,
Figure BDA00029416479100001910
is fR(ii) an inverse function of (c.),
Figure BDA00029416479100001911
is JRM-P inverse of (1).
The lengths of the rope sections at the universal joints between the vertebral segments i and the vertebral segments i +1 are respectively as follows:
Figure BDA0002941647910000201
wherein, g1(q2i-1,q2i)、g2(q2i-1,q2i)、g3(q2i-1,q2i) Can be determined by equation (14).
Rewriting formula (23) to obtain:
Figure BDA0002941647910000202
numerical solution of (24) by Newton iteration using g1、g2、g3With respect to q2i-1、q2iThe Hessian matrix and the Jacobin matrix can calculate the joint angle q2i-1And q is2i。
Example 2
The present embodiment is described with reference to fig. 7, and the present embodiment differs from the present embodiment in that kinematics solution is performed on the rope-driven serpentine manipulator including the rope hole gap, positive kinematics is obtained by combining the three steps and six steps, and inverse kinematics is obtained by combining the four steps and five steps.
The other steps are the same as those of the first, second, third, fourth, fifth, sixth and seventh embodiments.
In the description of the present invention, "a plurality" means two or more unless otherwise specified; the terms "upper", "lower", "left", "right", "inner", "outer", "front", "rear", "head", "tail", and the like, indicate orientations or positional relationships based on the orientations or positional relationships shown in the drawings, are only for convenience in describing and simplifying the description, and do not indicate or imply that the device or element referred to must have a particular orientation, be constructed in a particular orientation, and be operated, and thus, should not be construed as limiting the invention. Furthermore, the terms "first," "second," "third," and the like are used for descriptive purposes only and are not to be construed as indicating or implying relative importance.
It should be noted that the embodiments of the present invention can be realized by hardware, software, or a combination of software and hardware. The hardware portion may be implemented using dedicated logic; the software portions may be stored in a memory and executed by a suitable instruction execution system, such as a microprocessor or specially designed hardware. Those skilled in the art will appreciate that the apparatus and methods described above may be implemented using computer executable instructions and/or embodied in processor control code, such code being provided on a carrier medium such as a disk, CD-or DVD-ROM, programmable memory such as read only memory (firmware), or a data carrier such as an optical or electronic signal carrier, for example. The apparatus and its modules of the present invention may be implemented by hardware circuits such as very large scale integrated circuits or gate arrays, semiconductors such as logic chips, transistors, or programmable hardware devices such as field programmable gate arrays, programmable logic devices, etc., or by software executed by various types of processors, or by a combination of hardware circuits and software, e.g., firmware.
The above description is only for the purpose of illustrating the present invention and the appended claims are not to be construed as limiting the scope of the invention, which is intended to cover all modifications, equivalents and improvements that are within the spirit and scope of the invention as defined by the appended claims.

Claims (10)

1. A method for processing motion data of a rope-driven snake-shaped mechanical arm with a rope hole gap is characterized by comprising the following steps:
classifying the rope holes and the ropes on the snake-shaped mechanical arm; defining a clearance coefficient according to the relation between the actual rope length and the ideal rope length;
fitting the unknown function of the clearance coefficient relative to the joint rotation angle by utilizing a Chebyshev polynomial;
solving the working space state of the joint space state by using the acquired unknown function of the joint corner;
solving joint states from the working space states;
calculating the theoretical rope length by using the joint angle, and calculating the actual rope length by using the clearance coefficient;
solving the theoretical rope length according to the actual rope length by utilizing the clearance coefficient, and solving the joint motion state according to the theoretical rope length;
and performing kinematic solution on the rope-driven snake-shaped mechanical arm with the rope hole gap to solve the joint positive kinematic data and the joint inverse kinematic data.
2. The method for processing the motion data of the rope-driving serpentine mechanical arm with the rope hole gaps as claimed in claim 1, wherein the rope holes and the ropes on the serpentine mechanical arm are classified; the clearance coefficient is defined by the relation of the actual rope length and the ideal rope length, and the specific process comprises the following steps:
dividing rope holes in the snake-shaped mechanical arm into rope locking holes and rope threading holes;
dividing each rope into a joint rope section, a vertebral segment rope section and a base rope section;
and introducing a clearance coefficient to represent the relationship between the actual length of the rope segment and the calculated length:
Figure FDA0002941647900000011
wherein the content of the first and second substances,
Figure FDA0002941647900000012
calculating the length of the corresponding rope k at the joint i;
Figure FDA0002941647900000013
the actual length of the corresponding rope k at the joint i;
calculating the length of the rope portion in the formula
Figure FDA0002941647900000014
By joint angle q of universal joint i2iAnd q is2i-1Calculating to obtain;
fitting the clearance coefficient by utilizing Chebyshev polynomial
Figure FDA0002941647900000015
With respect to q2i、q2i-1The specific process of the unknown function of (2) is as follows:
the recursion relationship of the chebyshev polynomial is as follows:
Figure FDA0002941647900000021
wherein x ∈ [ -1,1]，n＝1、2、3……N，Tn(x) Is an nth order polynomial on x;
since x ∈ [ -1,1] in the chebyshev polynomial, the joint angle is normalized:
Figure FDA0002941647900000022
wherein the content of the first and second substances,
Figure FDA0002941647900000023
is the upper bound of the joint angle j,
Figure FDA0002941647900000024
is the lower bound of the joint angle j;
Figure FDA0002941647900000025
angle q of rotation with joint2iAnd q is2i-1The relationships are recursion relationships which meet the recursion relationship of the Chebyshev polynomial, and the fitting of each joint corner adopts the Chebyshev polynomial; to obtain
Figure FDA0002941647900000026
With respect to q2iAnd q is2i-1The fitting function of the binary chebyshev polynomial of (a) is:
Figure FDA0002941647900000027
wherein the content of the first and second substances,
Figure FDA0002941647900000028
and
Figure FDA0002941647900000029
are each q2i-1And q is2iThe normalized joint angle is obtained after calculation through the process shown in the formula (3);
Figure FDA00029416479000000210
is composed of
Figure FDA00029416479000000211
About
Figure FDA00029416479000000212
And
Figure FDA00029416479000000213
the chebyshev fit function of (a);
solving weight coefficient C (n) in fitting function of binary Chebyshev polynomial by using least square method through finite measurement data2i-1,n2i)；
Figure FDA00029416479000000214
The square error of the fit is:
Figure FDA00029416479000000215
wherein p and q are each independently
Figure FDA00029416479000000216
And
Figure FDA00029416479000000217
sample numbers of (1), P and Q are respectively
Figure FDA00029416479000000218
And
Figure FDA00029416479000000219
the number of samples of (a);
to pair
Figure FDA00029416479000000220
And (3) solving the partial derivatives at two ends of the fitted square error difference type, and obtaining the square error difference type according to the Chebyshev polynomial orthogonality principle:
Figure FDA00029416479000000221
wherein the content of the first and second substances,
Figure FDA00029416479000000222
according to extreme requirements, i.e.
Figure FDA0002941647900000031
To find
Figure FDA0002941647900000032
The value of (A) is as follows:
Figure FDA0002941647900000033
3. the method for processing the motion data of the rope-driven snake-shaped mechanical arm with the rope hole gap as claimed in claim 1, wherein the working space state is solved by the joint space state, and the concrete process is as follows:
the end pose is represented by the pose of the tool coordinate system in the task space, and the homogeneous transformation matrix between the adjacent D-H coordinate systems is represented as follows according to the definition mode of the D-H parameters:
Figure FDA0002941647900000034
according to the homogeneous transformation matrix, calculating the terminal pose by the joint angle:
TE＝T0·0T1·1T2…J-2TJ-1·J-1TJ·JTE
wherein, T0Is a homogeneous transformation matrix of the base coordinate system relative to the global coordinate system,JTEis a tool coordinate system relative to a D-H coordinate system { CJ-xJyJzJ-a homogeneous transformation matrix of;
the recursion algorithm of the speed of each moving body of the mechanical arm is expressed by a three-dimensional vector method as follows:
Figure FDA0002941647900000035
wherein the content of the first and second substances,
Figure FDA0002941647900000036
respectively representing vectors of the angular velocity and the origin point velocity of the D-H coordinate system j in the coordinate system j;
Figure FDA0002941647900000037
is the attitude rotation matrix of coordinate system j relative to coordinate system j-1,
Figure FDA0002941647900000038
is the position vector of the origin of the coordinate system j in the coordinate system j-1;
the recursion algorithm of the acceleration of each moving body of the mechanical arm is expressed by a three-dimensional vector method as follows:
wherein the content of the first and second substances,
Figure FDA0002941647900000041
angular acceleration and origin acceleration in D-H coordinate system j respectivelyThe vector is represented in coordinate system j.
4. The method for processing the motion data of the rope-driven snake-shaped mechanical arm with the rope hole gap as claimed in claim 1, wherein the joint state is solved by the working space state, and the concrete process is as follows:
working space velocity v and joint angular velocity
Figure FDA0002941647900000042
The mapping relationship between the two is as follows:
Figure FDA0002941647900000043
wherein the content of the first and second substances,
Figure FDA0002941647900000044
is a joint jacobian matrix;
solving according to the mapping relation, because the joint space dimension is larger than the working space dimension
Figure FDA0002941647900000045
The process comprises the following steps:
Figure FDA0002941647900000046
wherein the content of the first and second substances,
Figure FDA0002941647900000047
is JqIs inverse to M-P, eta is dimension and
Figure FDA0002941647900000048
the same vector, η, is generally determined by the motion optimization objective function;
by
Figure FDA0002941647900000049
The angular velocity of the joint, the angle of the joint and the angular acceleration are respectively obtained through the pair
Figure FDA00029416479000000410
Is obtained by time integration and time differentiation.
5. The method for processing the motion data of the rope-driven serpentine mechanical arm with the rope hole gap as claimed in claim 1, wherein the calculating the theoretical rope length from the joint angle and the calculating the actual rope length by using the gap coefficient comprises:
cord segment vector with cord k positioned between the inferior lamina of vertebra segment i and the superior lamina of vertebra segment i-1
Figure FDA00029416479000000411
Expressed as:
Figure FDA00029416479000000412
wherein the content of the first and second substances,
Figure FDA00029416479000000413
is the vector of the central position of the lower plate rope hole k of the vertebra segment i,
Figure FDA00029416479000000414
is the vector of the central position of the upper plate rope hole k of the vertebral segment i-1;
Figure FDA00029416479000000415
in the formula
Figure FDA00029416479000000416
By
Figure FDA00029416479000000417
Is obtained by
Figure FDA00029416479000000418
By
Figure FDA00029416479000000419
Formula determines the calculated length l of the rope kkComprises the following steps:
Figure FDA0002941647900000051
wherein the content of the first and second substances,
Figure FDA0002941647900000052
the length of the fixed rope segment on the vertebral segment;
by
Figure FDA0002941647900000053
The relationship between the rope motion state and the joint motion state is as follows:
Figure FDA0002941647900000054
wherein f isR(q) is based on
Figure FDA0002941647900000055
Formula determination of the implicit function of the cord length with respect to the joint angle, JRIs a rope-joint jacobian matrix;
since the constraint between the rope and the joint is a geometric constraint, JRExpressed as:
Figure FDA0002941647900000056
wherein the content of the first and second substances,
Figure FDA0002941647900000057
is the joint jacobian matrix of the vertebral level i,
Figure FDA0002941647900000058
a cord jacobian matrix for vertebra segment i;
Figure FDA0002941647900000059
and
Figure FDA00029416479000000510
completely determined by the configuration of the mechanism, the calculation formula is as follows:
Figure FDA00029416479000000511
Figure FDA00029416479000000512
wherein the content of the first and second substances,
Figure FDA00029416479000000513
by
Figure FDA00029416479000000514
The actual length of the rope section is
Figure FDA00029416479000000515
The actual rope length is then:
Figure FDA00029416479000000516
6. the method for processing the motion data of the rope-driven snakelike mechanical arm with the rope hole gap as claimed in claim 1, wherein the clearance coefficient is used for solving the theoretical rope length according to the actual rope length, and the joint motion state is solved according to the theoretical rope length, and the specific process is as follows:
using determined gap coefficients
Figure FDA0002941647900000061
The theoretical rope length is solved from the actual rope length:
Figure FDA0002941647900000062
wherein the content of the first and second substances,
Figure FDA0002941647900000063
is the cord segment vector for cord k between the inferior lamina of vertebra segment p and the superior lamina of vertebra segment p-1;
the process of solving the joint angle by using the length of the joint rope section comprises
Figure FDA0002941647900000064
The reverse process of formula:
Figure FDA0002941647900000065
wherein the content of the first and second substances,
Figure FDA0002941647900000066
is fR(ii) an inverse function of (c.),
Figure FDA0002941647900000067
is JRM-P inverse of (1);
the lengths of the rope sections at the universal joints between the vertebral segments i and the vertebral segments i +1 are respectively as follows:
Figure FDA0002941647900000068
wherein, g1(q2i-1,q2i)、g2(q2i-1,q2i)、g3(q2i-1,q2i) In the analytic form of
Figure FDA0002941647900000069
Determining a formula;
will be provided with
Figure FDA00029416479000000610
Rewriting the formula:
Figure FDA0002941647900000071
numerical solution of the rewritten above formula by Newton iteration using g1、g2、g3With respect to q2i-1、q2iCalculating the joint angle q by the Hessian matrix and the Jacobin matrix2i-1And q is2i。
7. A rope-driven serpentine mechanical arm motion data processing system containing a rope hole gap for implementing the data processing method as claimed in any one of claims 1 to 6, wherein the rope-driven serpentine mechanical arm motion data processing system containing the rope hole gap comprises:
the rope hole and rope classifying module is used for classifying the rope holes and the ropes on the snake-shaped mechanical arm; defining a clearance coefficient according to the relation between the actual rope length and the ideal rope length;
the joint corner unknown function fitting module is used for fitting the unknown function of the clearance coefficient relative to the joint corner by utilizing a Chebyshev polynomial;
the working space state solving module is used for solving the working space state by using the acquired unknown function of the joint corner;
the joint state solving module is used for solving the joint state according to the working space state;
the joint motion state solving module is used for calculating the theoretical rope length by using the joint angle and calculating the actual rope length by using the clearance coefficient; the clearance coefficient is used for solving the theoretical rope length according to the actual rope length, and the joint motion state is solved according to the theoretical rope length;
and the mechanical arm kinematics solving module is used for carrying out kinematics solving on the rope-driven snake-shaped mechanical arm with the rope hole gap to solve the positive rope kinematics data and the inverse rope kinematics data.
8. The rope-driven serpentine mechanical arm is characterized by being used for implementing the rope-driven serpentine mechanical arm motion data processing method containing the rope hole gap according to any one of claims 1 to 6.
9. A robot carrying the rope driven serpentine robotic arm of claim 8.
10. A program storage medium for receiving user input, the stored computer program causing an electronic device to perform the method for processing data on the movement of a rope-driven serpentine robotic arm having a rope hole gap according to any one of claims 1 to 6.
CN202110179166.4A 2021-02-08 2021-02-08 Rope-driven snakelike mechanical arm motion data processing method containing rope hole gaps Active CN112975925B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110179166.4A CN112975925B (en) 2021-02-08 2021-02-08 Rope-driven snakelike mechanical arm motion data processing method containing rope hole gaps

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110179166.4A CN112975925B (en) 2021-02-08 2021-02-08 Rope-driven snakelike mechanical arm motion data processing method containing rope hole gaps

Publications (2)

Publication Number Publication Date
CN112975925A true CN112975925A (en) 2021-06-18
CN112975925B CN112975925B (en) 2021-10-26

Family

ID=76392744

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110179166.4A Active CN112975925B (en) 2021-02-08 2021-02-08 Rope-driven snakelike mechanical arm motion data processing method containing rope hole gaps

Country Status (1)

Country Link
CN (1) CN112975925B (en)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113977562A (en) * 2021-10-26 2022-01-28 浙江大学 Compact snakelike arm for adverse circumstances
CN114055448A (en) * 2021-11-18 2022-02-18 中国科学院自动化研究所 Rope-driven snakelike mechanical arm control method, device and equipment

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102990676A (en) * 2012-11-23 2013-03-27 中国航空工业集团公司北京航空制造工程研究所 Snakelike mechanical arm
EP3072642A2 (en) * 2015-03-23 2016-09-28 Rolls-Royce plc Flexible tools and apparatus for machining objects
CN106926223A (en) * 2015-12-30 2017-07-07 中国科学院沈阳自动化研究所 A kind of snake-shaped robot
CN109176494A (en) * 2018-09-28 2019-01-11 哈尔滨工业大学（深圳） Rope drives Arm Flexible machine people self-calibrating method and system, storage medium
CN109760051A (en) * 2019-01-16 2019-05-17 哈尔滨工业大学 A kind of rope drives the determining method of rope lengths variation of ultra-redundant degree of freedom robot
CN110154010A (en) * 2019-07-04 2019-08-23 哈尔滨迅动科技有限责任公司 A kind of rope drive Snakelike mechanical arm carrying out rope pull measurement
CN111906762A (en) * 2020-06-10 2020-11-10 哈尔滨工业大学 Joint angle determination method for snake-shaped mechanical arm

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102990676A (en) * 2012-11-23 2013-03-27 中国航空工业集团公司北京航空制造工程研究所 Snakelike mechanical arm
EP3072642A2 (en) * 2015-03-23 2016-09-28 Rolls-Royce plc Flexible tools and apparatus for machining objects
CN106926223A (en) * 2015-12-30 2017-07-07 中国科学院沈阳自动化研究所 A kind of snake-shaped robot
CN109176494A (en) * 2018-09-28 2019-01-11 哈尔滨工业大学（深圳） Rope drives Arm Flexible machine people self-calibrating method and system, storage medium
CN109760051A (en) * 2019-01-16 2019-05-17 哈尔滨工业大学 A kind of rope drives the determining method of rope lengths variation of ultra-redundant degree of freedom robot
CN110154010A (en) * 2019-07-04 2019-08-23 哈尔滨迅动科技有限责任公司 A kind of rope drive Snakelike mechanical arm carrying out rope pull measurement
CN111906762A (en) * 2020-06-10 2020-11-10 哈尔滨工业大学 Joint angle determination method for snake-shaped mechanical arm

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
顿向明等: "蛇形臂机器人研究与发展综述", 《飞控与探测》 *

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113977562A (en) * 2021-10-26 2022-01-28 浙江大学 Compact snakelike arm for adverse circumstances
CN113977562B (en) * 2021-10-26 2023-08-15 浙江大学 Compact snake-shaped mechanical arm for severe environment
CN114055448A (en) * 2021-11-18 2022-02-18 中国科学院自动化研究所 Rope-driven snakelike mechanical arm control method, device and equipment
CN114055448B (en) * 2021-11-18 2023-03-31 中国科学院自动化研究所 Rope-driven snakelike mechanical arm control method, device and equipment

Also Published As

Publication number Publication date
CN112975925B (en) 2021-10-26

Similar Documents

Publication Publication Date Title
CN112975925B (en) Rope-driven snakelike mechanical arm motion data processing method containing rope hole gaps
US7756608B2 (en) System for calibration of an industrial robot and a method thereof
US4698572A (en) Kinematic parameter identification for robotic manipulators
US6526373B1 (en) Optimization tool for robot placement
CN109176505B (en) Ball arm instrument-based six-axis joint industrial robot spatial error calibration method
US7756606B2 (en) Method of controlling redundant manipulator
Wang et al. Parameter optimization of a four-legged robot to improve motion trajectory accuracy using signal-to-noise ratio theory
Rauf et al. Experimental results on kinematic calibration of parallel manipulators using a partial pose measurement device
CN110883774B (en) Robot joint angle zero calibration system, method and storage medium
CN107718050A (en) SCARA robots brachium and Zero calibration method, device, medium and computer equipment
WO2021003928A1 (en) Constant linear velocity control method for detecting contour of workpiece
Wang et al. Identification of strut and assembly errors of a 3-PRS serial–parallel machine tool
CN112318498B (en) Industrial robot calibration method considering parameter coupling
CN112975987B (en) Orthopedic surgery robot control method based on dynamic model
US11673275B2 (en) Through-beam auto teaching
Andersson Discretization of a continuous curve
Xiao et al. A new fixed axis-invariant based calibration approach to improve absolute positioning accuracy of manipulators
Lu et al. Experimental determination of dynamic parameters of robotic arms
Fabritius et al. A framework for analyzing the accuracy, complexity, and long-term performance of cable-driven parallel robot models
CN114734436B (en) Robot encoder calibration method and device and robot
Shahamiri et al. Uncalibrated visual servoing using a biased newton method for on-line singularity detection and avoidance
CN115723127A (en) Grating encoder-based profile error prediction method for hybrid robot
CN114952858A (en) Industrial robot trajectory tracking method and system based on friction compensation control
CN108742840B (en) Calibration method and device for robot
Gu et al. Dexterous obstacle-avoidance motion control of Rope Driven Snake Manipulator based on the bionic path following

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant