CN115533920A - Collaborative planning method and system for solving inverse kinematics of rope-driven mechanical arm and computer storage medium - Google Patents
Collaborative planning method and system for solving inverse kinematics of rope-driven mechanical arm and computer storage medium Download PDFInfo
- Publication number
- CN115533920A CN115533920A CN202211420266.2A CN202211420266A CN115533920A CN 115533920 A CN115533920 A CN 115533920A CN 202211420266 A CN202211420266 A CN 202211420266A CN 115533920 A CN115533920 A CN 115533920A
- Authority
- CN
- China
- Prior art keywords
- rope
- mechanical arm
- driven mechanical
- configuration
- arm
- 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.)
- Pending
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/16—Programme controls
- B25J9/1628—Programme controls characterised by the control loop
-
- 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/1628—Programme controls characterised by the control loop
- B25J9/1643—Programme controls characterised by the control loop redundant control
-
- 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 discloses a collaborative planning method and a collaborative planning system for solving inverse kinematics of a rope-driven mechanical arm and a computer storage medium, wherein the method comprises the following steps: selecting the intersection point of the tail end point of the rope-driven mechanical arm and part of the middle joints as a controlled point; freely setting a corresponding control point near the controlled point; calculating Euclidean distance (Euclidean distance) between the control point and the controlled point; and combining a gradient projection method, taking the calculated Euclidean distance as an optimization objective function, and adjusting the configuration of the whole arm in the self-movement space of the rope-driven mechanical arm by adjusting parameters such as control points, scale factors and the like. The planning method comprises the following steps: when any Euclidean distance is larger than the corresponding safety threshold, the Euclidean distance approaches zero through global optimization based on a gradient projection method, so that the freedom degree of the whole arm of the rope-driven mechanical arm is optimized, the consistency of the whole arm configuration of the rope-driven mechanical arm and the configuration set by the control point reaches an optimized state, and the collaborative planning target of 'whole arm configuration-terminal pose' is realized.
Description
Technical Field
The invention relates to the field of robot control, in particular to a collaborative planning method and a collaborative planning system for solving inverse kinematics of a rope-driven mechanical arm and a computer storage medium, which are suitable for the requirement of narrow-space operation of the rope-driven mechanical arm.
Background
Different from the traditional 6 or 7-degree-of-freedom rope-driven mechanical arm, the rope-driven mechanical arm has more or infinite multiple degrees of freedom, is similar to a plurality of sections of reptiles such as noses or snakes in shape and action, and has excellent environmental adaptability, compatibility and extremely high obstacle avoidance capability. Therefore, the rope-driven mechanical arm has a large enough flexible operation space, can pass through a narrow range, has small interference on a base, has the capabilities of avoiding obstacles, avoiding singularities, optimizing joint torque and the like, and can meet the requirements of comprehensively detecting and maintaining a large-scale non-cooperative spacecraft. In order to adapt to the operation in the narrow environment of the non-cooperative spacecraft, the advantage of the smart motion of the rope-driven mechanical arm needs to be fully exerted.
Due to the existence of a large number of degrees of freedom and obstacles, the inverse kinematics solution of the rope-driven mechanical arm is considered to be very complex, so that the trajectory planning and control are always the key points and difficulties in the research of the rope-driven mechanical arm. Generally, inverse kinematics solving methods of redundant rope-driven mechanical arms can be divided into three types: algebraic method (1) iterative method (2), such as neural network method, genetic algorithm, fuzzy algorithm (3) geometric method. Nowadays, kinematic decoupling and other methods are commonly used to simplify the analysis process. When a closed solution of the special rope-driven mechanical arm configuration does not exist, a numerical method or a method of mixing multiple algorithms is usually adopted to solve the inverse kinematics solution. Rope-driven robotic arms tend to have more degrees of freedom than are needed to achieve the end position pose, in which case it becomes very efficient to use a numerical solution. In the inverse solution process, the super-redundant structure can realize the attitude tracking of the tail end position and complete various additional tasks, such as obstacle avoidance, joint limit avoidance, singularity avoidance, joint speed optimization, driving torque optimization and the like, and can meet the additional task constraints, namely the advantages of the redundant rope-driven mechanical arm compared with the traditional rope-driven mechanical arm. Freund et al propose a method for multi-robot obstacle avoidance planning in an on-line planning multi-obstacle environment. The obstacle avoidance planning problem is converted into a secondary convex optimization problem, in order to avoid obstacles, the method adopts an acceleration solving mode capable of directly controlling joint motion, and potential collision risks can be considered at the same time; because the method is a planning method based on the model, the method has certain universality. Homayoun et al propose a method of real-time planning. The method formulates a trajectory planning problem as a force-position hybrid control problem. Yoshida et al provide a planning algorithm of a rope-driven mechanical arm in a complex three-dimensional environment based on an iterative two-stage planning method. Mu and the like propose a segmental geometric method and an improved mode function method aiming at the problem of track planning of the rope-driven mechanical arm, study the problem of motion planning of the rope-driven mechanical arm, but also need to combine with the working environment to expand the research of an adaptive algorithm.
In summary, in the previous research, the problem of planning the track of the rope-driven mechanical arm has been completed. However, for narrow space operation, a collaborative planning method with obstacle avoidance, singularity avoidance, joint avoidance overrun and 'whole arm configuration-terminal pose' needs to be provided in combination with a rope-driven mechanical arm.
Disclosure of Invention
The present invention is directed to solving, at least in part, one of the technical problems in the related art. Therefore, the invention aims to provide a collaborative planning method and a collaborative planning system for solving inverse kinematics of a rope-driven mechanical arm and a computer storage medium, which are used for realizing collaborative planning processing of 'whole arm configuration-end pose' of the rope-driven mechanical arm.
The invention adopts a technical scheme that: a collaborative planning method for solving inverse kinematics of a rope-driven mechanical arm comprises the following steps:
setting i controlled points at an end effector and a middle joint of a rope-driven mechanical arm, and correspondingly setting i control points in space;
setting the controlled points as nodes between every N (N =2,4, \8230m, wherein m is the total number of the degrees of freedom) degrees of freedom on the rope-driven mechanical arm according to the characteristics of the rope-driven mechanical arm, wherein the 'proximity' between the controlled points and the corresponding control points is an object to be optimized;
step three, judging whether any Euclidean distance is larger than a corresponding safety threshold value, if so, enabling the Euclidean distance to approach zero through global optimization based on a gradient projection method, and accordingly obtaining an expected configuration of the rope-driven mechanical arm in space;
and step four, judging whether any Euclidean distance is zero, and if so, indicating that the configuration of the rope-driven mechanical arm is completely consistent with the expected configuration.
And step five, judging whether any Euclidean distance is zero, if not, optimizing the whole arm freedom degree of the rope-driven mechanical arm to enable the weighted sum of the proximity degrees to be minimum, namely enabling the consistency of the whole arm configuration of the rope-driven mechanical arm and the configuration set by the control point to reach an optimized state, and achieving a 'whole arm configuration-tail end pose' collaborative planning target.
Further, for the selection of the controlled point, the redundancy of the position is fully utilized, and the joint node between every four degrees of freedom is set as the controlled point of the rope-driven mechanical arm.
For the selection of the control points, in order to achieve the aim of flexibly adjusting the intermediate configuration of the rope-driven mechanical arm according to an operator, a variable control point-based track planning method for the rope-driven mechanical arm is provided, the position coordinates of the control points in space are obtained in an 'on-demand' grabbing point mode, the grabbing points are projected to a YZ plane of a D-H coordinate system in an algorithm through a coordinate conversion mode, and the intersection point of the connecting line of a foot and the center of a circle and a control circle is the control point.
Further, a configuration optimization criterion function of the rope-driven mechanical arm is established, when the optimization target is 0, the controlled point and the control point are completely overlapped, the rope-driven mechanical arm is in an expected configuration, and the problem is converted into that: under the condition that the pose of the tail end is not changed, the problem of optimizing the arm shape based on variable control points is achieved by adjusting the self-movement of the rope-driven mechanical arm in a zero space.
Further, the problem that the spatial configuration of the rope-driven mechanical arm has the maximum self-motion change rate in any direction under a certain combination of current joint spaces is solved by applying a gradient projection method, the joint configuration of the rope-driven mechanical arm is optimized, and meanwhile, the angular speed of the joint angle of the rope-driven mechanical arm is optimized.
Further, establishing a mapping Jacobian matrix of the joint space and the task space, and inputting the known conditions:
1) The expected pose of the end effector of the rope-driven mechanical arm in the task space;
2) A current joint angle;
3) D-H coordinate system parameters;
4) The desired position of the control point.
And further calculating the current pose difference, obtaining the relation between the pose difference and the joint variable by using a Jacobian matrix and a gradient projection method, calculating new input of the joint of the rope-driven mechanical arm, and performing motion control on the rope-driven mechanical arm to realize the motion of the rope-driven mechanical arm.
And further, realizing the collaborative planning processing of the rope-driven mechanical arm by adjusting and improving corresponding parameters.
Further, the collaborative planning parameters include an arm configuration parameter and an end pose parameter.
The other technical scheme adopted by the invention is as follows: a collaborative planning system for solving inverse kinematics of a rope-driven mechanical arm comprises
A position selection unit: the system is used for acquiring the position coordinates of the control point in the space according to the operation requirement;
a position detection unit: the device is used for detecting the current position of the rope-driven mechanical arm so as to calculate the Euclidean distance between the control point and the controlled point;
a position determination unit: the device is used for judging whether the Euclidean distance is greater than a corresponding safety threshold value or not and judging whether the Euclidean distance between the control point and the controlled point is zero or not;
a planning unit: the system is used for calculating the current pose difference, obtaining the relation between the pose difference and the joint variable through a gradient projection method by utilizing a Jacobian matrix, calculating new input of the joint of the rope-driven mechanical arm and performing motion control on the rope-driven mechanical arm;
an input unit: the rope driving mechanical arm is used for inputting the planned joint configuration and the angular speed of the joint angle of the rope driving mechanical arm to the rope driving mechanical arm, and movement of the rope driving mechanical arm is achieved.
Further, the collaborative planning processing of the rope-driven mechanical arm is realized by adjusting and improving corresponding parameters. .
Further, the collaborative planning parameters include an arm configuration parameter and an end pose parameter.
The invention adopts another technical scheme that: a computer storage medium having a computer program stored thereon, which when executed by a processor, performs the steps of:
setting i controlled points at an end effector and a middle joint of a rope-driven mechanical arm, and correspondingly setting i control points in space;
setting the controlled points as nodes between every N degrees of freedom on the rope-driven mechanical arm according to the characteristics of the rope-driven mechanical arm, wherein the 'proximity' between the controlled points and the corresponding control points is the target to be optimized;
step three, judging whether any Euclidean distance is larger than a corresponding safety threshold value, if so, enabling the Euclidean distance to approach zero through global optimization based on a gradient projection method, and accordingly obtaining an expected configuration of the rope-driven mechanical arm in space;
and step four, judging whether any Euclidean distance is zero, and if so, indicating that the configuration of the rope-driven mechanical arm is completely consistent with the expected configuration.
And step five, judging whether any Euclidean distance is zero, if not, optimizing the whole arm freedom degree of the rope-driven mechanical arm to enable the weighted sum of the proximity degrees to be minimum, namely enabling the consistency of the whole arm configuration of the rope-driven mechanical arm and the configuration set by the control point to reach an optimized state, and achieving a 'whole arm configuration-tail end pose' collaborative planning target.
The beneficial effects of the invention are:
the method comprises the steps that aiming at the requirement that a rope-driven mechanical arm executes a narrow space detection task on a rail, intersection points of a tail end point and part of middle joints of the rope-driven mechanical arm are respectively selected as controlled points; the corresponding control point can be freely set near the controlled point; the cartesian distance between a control point and a controlled point is defined as proximity. The approach is taken as an optimized objective function by combining a gradient projection method, and the configuration of the whole arm and the adjustment of the pose of the tail end can be completed in the self-movement space of the rope-driven mechanical arm by adjusting parameters such as control points, scale factors and the like. On the premise of meeting the requirement of the terminal pose, the controlled point of the rope-driven mechanical arm approaches or is far away from the position required by the control point in the self-motion space by optimizing all degrees of freedom of the rope-driven mechanical arm. Therefore, the method meets the requirement of 'arm configuration-terminal pose' collaborative planning, and overcomes the defect that the arm configuration cannot be adjusted only by considering the terminal position in the traditional method.
Drawings
Fig. 1 is a schematic diagram of a D-H coordinate system establishment method according to an embodiment of the collaborative planning method for solving inverse kinematics of a rope-driven robot arm in the present invention.
Fig. 2 is a schematic diagram of a collaborative planning method for solving the inverse kinematics of the rope-driven mechanical arm according to an embodiment of the present invention.
Fig. 3 is a schematic diagram of a control point and a coordinate system where the control point is located in a collaborative planning method for solving inverse kinematics of a rope-driven mechanical arm according to the present invention.
Fig. 4 is a flowchart of an inverse kinematics solving method for the whole arm configuration optimization of the collaborative planning method for solving the inverse kinematics of the rope-driven mechanical arm according to the present invention.
Fig. 5 is a schematic diagram of an arm angle formed by damping coefficients c =0 and c = -0.8 according to an embodiment of the collaborative planning method for solving inverse kinematics of a rope-driven mechanical arm in the present invention.
Fig. 6 is a schematic diagram of the distribution of control points for adjusting the "configuration of the whole arm" according to an embodiment of the collaborative planning method for solving the inverse kinematics of the rope-driven robot arm in the present invention.
Detailed Description
In order to make those skilled in the art better understand the technical solution of the present invention, the following will clearly and completely describe the specific technical solution of the present invention with reference to the embodiments to help those skilled in the art to further understand the present invention. It should be apparent that the embodiments described herein are only a few embodiments of the present invention, and not all embodiments. It should be noted that the embodiments and features of the embodiments in the present application can be combined with each other without departing from the inventive concept and without conflicting therewith by those skilled in the art. All other embodiments based on the embodiments of the present invention, which can be obtained by a person of ordinary skill in the art without any creative effort, shall fall within the disclosure and the protection scope of the present invention.
Furthermore, the terms "first," "second," "step 1," "step 2," "further," and the like in the description and in the claims and the drawings of the present application are used for distinguishing between similar elements and not necessarily for describing a particular sequential or chronological order. It should be understood that the data so used may be interchanged under appropriate circumstances such that embodiments of the invention described herein may be practiced in sequences other than those described. Also, the terms "including" and "having," as well as any variations thereof, in the present invention are intended to cover non-exclusive inclusions. In addition, for those skilled in the art, the specific meanings of the above terms in the present case can be understood by combining the prior art according to specific situations.
The invention provides a collaborative planning method for solving inverse kinematics of a rope-driven mechanical arm, aiming at the defect that the traditional track planning method for solving inverse kinematics of the rope-driven mechanical arm only considers the tail end position and cannot adjust the arm configuration, and the specific technical scheme is as follows:
referring to fig. 1, fig. 1 is a schematic diagram of a D-H coordinate system establishment method according to an embodiment of the collaborative planning method for solving the inverse kinematics of the rope-driven robot arm in the present invention. The method is characterized in that a rope-driven mechanical arm is set up based on a classic D-H method, an internal relation between a working space and a joint space of the rope-driven mechanical arm is solved, and a foundation is laid for achieving 'whole arm configuration-terminal pose' collaborative planning of the rope-driven mechanical arm.
After the coordinate systems are established according to the D-H method, homogeneous transformation matrixes between the coordinate systems of the adjacent connecting rods i-1 T i Can be calculated according to the following formula:
sθ i ＝sin(θ i ),sα i ＝sin(α i )；
cθ i ＝cos(θ i ),cα i ＝cos(α i )。
according to the relation of the homogeneous transformation matrix of the rope-driven mechanical arm, the relation between the terminal coordinate system pose and each joint coordinate system pose can be obtained as shown in the formula (2):
0 T n ＝ 0 T 1 … n-1 T n ＝f(Θ) (2)
referring to fig. 2, fig. 2 is a schematic diagram of a collaborative planning method for solving inverse kinematics of a rope-driven robot arm according to an embodiment of the present invention, where the principle shown in fig. 2 is as follows: firstly, ensuring that the tail end track is consistent with the expected track, then setting i controlled points on the rope-driven mechanical arm, and correspondingly setting i control points in the space. Setting the controlled point as a node between every 4 degrees of freedom on the rope-driven mechanical arm according to the characteristics of the rope-driven mechanical arm, and setting the controlled point as a corresponding control point C i The "proximity" between (i =1,2,3,4) is then the objective to be optimized. When judging that any Euclidean distance is greater than the corresponding safety threshold(s) ti (i =1,2,3,4)), then the euclidean distance d is made by global optimization based on gradient projection i Approaching zero, thereby obtaining the expected configuration of the rope-driven mechanical arm in space. When any Euclidean distance d i And when the configuration is zero, the configuration of the rope-driven mechanical arm is completely consistent with the expected configuration. When any Euclidean distance d i When the degree of freedom of the whole arm of the rope-driven mechanical arm cannot be zero, the degree of freedom of the whole arm is optimized, so that the proximity d is close to i Weighted sum of
Minimum (lambda) i Weight coefficient), namely the consistency of the configuration of the whole arm of the rope-driven mechanical arm and the configuration set by the control point reaches an optimal state, and the 'whole arm' is realizedConfiguration-end pose "co-planning objectives.
In order to make the technical solutions of the present invention better understood by those skilled in the art, some specific technical solutions of the present invention will be clearly and completely explained below with reference to the embodiments:
(1) And selecting a control point and a controlled point of the rope-driven mechanical arm.
Referring to fig. 2, the redundancy of the configuration of the rope-driven mechanical arm is fully utilized, and the joint node N between every four degrees of freedom is divided into four i And (i =1,2,3,4) is set as the controlled point of the rope-driven robot arm. According to the positive kinematics of the robot, the controlled point N is known i The homogeneous matrix in the {0} coordinate system can be represented as:
in formula (4)
Represents a controlled point N i A position in the {0} coordinate system;
represents a controlled point N i Attitude in the {0} coordinate system.
Referring to fig. 3, fig. 3 is a schematic diagram of a control point and a coordinate system where the control point is located in a collaborative planning method for solving inverse kinematics of a rope-driven mechanical arm according to the present invention; in order to achieve the aim of flexibly adjusting the middle configuration of the rope-driven mechanical arm according to an operator, the invention provides a track planning idea of the rope-driven mechanical arm based on variable control points, the control of the whole arm configuration of the rope-driven mechanical arm at the space selection control point through VR (Virtual Reality) equipment has the characteristics of intuition and flexibility for the VR operator, but inevitable large sudden change exists among coordinates of the control points, and the control is difficult to ensureContinuity and uniformity of dotting in spatial position values. Therefore, in order to keep the characteristics of intuition and flexibility in operation, the position coordinates of the control point in the space are still obtained in an on-demand point grabbing way of the VR equipment. In the algorithm, the capture point is projected to a YZ plane of a coordinate system (i), a vertical leg Pt and a circle center O in a coordinate conversion mode i The intersection point of the connecting line and the control circle is the control point (radius R of the control circle) c And controlling the degree of rounding α).
Referring to fig. 3, the spatial coordinates of the grasping point G are set to 0 P G ＝[ 0 P x G , 0 P y G , 0 P z G ]Its coordinates converted into a coordinate system { i } are represented as i P G ＝[ i P x G , i P y G , i P z G ]Thus the foot Pt coordinate is expressed as i P Pt ＝[0, i P y G , i P z G ]. Thus, the radius R of the circle is controlled c And controlling the fillet degree alpha to satisfy the formula (5):
wherein the radius of the circle R is controlled c Can be set manually, and the following formula (6) can be satisfied without loss of generality:
(2) Establishment of configuration optimization criterion function of rope-driven mechanical arm
Referring to fig. 2 and 3, based on the above-set control point and controlled point information, the proximity of the corresponding points may be expressed as:
weighting proximityIs λ i Which satisfies the following conditions:
the weighted optimization objective can be expressed as:
in the formula of i -a weight coefficient;
s ti -a safety threshold for proximity;
d i (θ) -proximity, i.e., the distance between the corresponding control point and the controlled point.
As can be seen from the above optimization objectives, when H (θ) =0, the controlled point and the control point completely coincide, and the rope-driven mechanical arm is in the desired configuration. This translates the problem into: under the condition that the pose of the tail end is not changed, the problem of optimizing the arm shape based on variable control points is realized by self-movement of the adjusting rope-driven mechanical arm in a zero space.
(3) Track optimization method introduction of rope-driven mechanical arm based on gradient projection method
There are an infinite number of solutions to the inverse kinematics problem of a rope driven robotic arm, and it is therefore necessary to introduce some optimization criteria to obtain the most efficient set of solutions. The multi-objective comprehensive optimization of the rope-driven mechanical arm also overcomes the defect of single performance index, and the excellent performance of the rope-driven mechanical arm is more comprehensively exerted. The gradient projection method is one of the commonly used methods for the inversion solution of the rope-driven mechanical arm. The application of the gradient projection method of the rope-driven mechanical arm is to answer a question: the spatial configuration of the rope-driven mechanical arm has the largest self-motion change rate along which direction under a certain combination of the current joint space. The joint configuration of the rope-driven mechanical arm can be optimized by using a gradient projection method, the angular velocity of the joint angle of the rope-driven mechanical arm can be optimized, and the performance index H (theta) of the formula (9) is a function of each joint angle:
in the formula
-rope drive robot arm joint angular velocity;
J-Jacobian matrix of the rope-driven mechanical arm;
J + -pseudo-inverse to J;
k is a scale factor;
i is a unit matrix;
h (θ) -Performance indicator function;
wherein the content of the first and second substances,
is the gradient vector of the performance indicator function H (θ) at θ, which can be expressed as:
J + the generalized inverse of J (or pseudo-inverse) is calculated as follows:
J + ＝J T (JJ T ) -1 (12)
the first term on the right of formula (10), i.e.
Is the minimum norm solution of the equation; the second term is a homogeneous solution of the equation, orthogonal to the minimum norm solution, belonging to the null space N (J) of the matrix J, i.e. the
The homogeneous solution corresponds to the self-movement of the rope-driven mechanical arm, namely the joint movement which does not influence the terminal pose of the rope-driven mechanical arm. The self-movement can be used to change the motionAnd optimizing the performance index function H (theta) under the condition of expecting the end pose.
In equation (10), the value of the scaling factor k may be positive or negative, 1) when k is negative, which is consistent with the desire to have the controlled point as close as possible to the corresponding control point. When k takes a negative value, the planning configuration tends to the configuration constrained by the control point, namely, the proximity approaches to a zero value more and more, so that the performance index function is also reduced, and the configuration of the rope-driven mechanical arm approaches to the expected configuration. 2) When k is a positive value, the rope-driven mechanical arm is far away from the control point, but the spatial configuration of the rope-driven mechanical arm can also be adjusted to achieve the expected purposes of obstacle avoidance and the like. The value of the scaling factor k is therefore chosen according to the actual needs in the application.
The choice of the scaling factor k is very critical, since it can be seen as a ratio of the main motion to the self motion of the rope-driven robot arm. When the ratio is too large, the dominant main motion of the rope-driven mechanical arm is too obvious, and the self-motion hardly works, otherwise, when the ratio is too small, the dominant self-motion of the rope-driven mechanical arm is realized, the joint angle of the rope-driven mechanical arm jumps in a larger angle in a self-motion space, and sometimes even the rationality of the main motion cannot be ensured. The invention uses the measurement index G to measure whether the proportion of the main motion and the self motion of the rope-driven mechanical arm is reasonable, namely the G has the function of judging whether the coefficient k is in a reasonable interval, so that the value of G is about 0.5, namely the k value is an ideal value, the homogeneous solution and the special solution of the rope-driven mechanical arm can be ensured to be in the same order of magnitude at the moment, and the measurement index G is expressed as follows:
where | · | |, represents the two-norm of the matrix. The too large or too small k value can cause huge difference between a special solution and a homogeneous solution, so that the algorithm fails, a way for solving the problem is to correct the k value in real time, and the k value can be defined as follows:
where c is an arbitrarily chosen damping coefficient used to proportionally vary the magnitude of k, and thus the effect of k on the program. The value of k is therefore a ratio of the norm of the main motion of the end of the rope-driven robot to the norm of the self motion of the rope-driven robot in null space. When the c value is added, the faults such as joint overrun caused by the fact that the two norms are usually small can be adjusted.
(4) Whole arm Jacobian matrix solution
Referring to fig. 4, fig. 4 is a flowchart of an inverse kinematics solving method for the whole arm configuration optimization of the collaborative planning method for solving the inverse kinematics of the rope-driven mechanical arm according to the present invention; the method uses a Jacobian matrix solving method, and the velocity-level kinematics establishes a mapping relation between the velocity of the joint of the rope-driven mechanical arm and the velocity of the tail end. If the joint i is a rotary joint, its angular velocity
The resulting terminal linear and angular velocities were:
wherein ξ i Unit vector of the rotation axis of the joint i, ρ i→n The position vector of the joint i pointing to the tail end point of the rope-driven mechanical arm is shown.
Thus, corresponding to the revolute joint i, the Jacobian matrix i is listed as:
the resulting end motions from the motion of all joints are:
J i actually, the movement speed of the joint i is used for transmitting the movement speed of the tail end of the rope-driven mechanical armAnd (4) the ratio.
According to the definition of D-H coordinate system, z of the coordinate system { i-1} i-1 The axis points in the axial direction of the joint i (i =1, \8230;, n), so the notation below {0} is:
0 ξ i ＝ 0 z i-1 ＝ 0 T i-1 (1:3,3)(i＝1,…,n) (18)
wherein: 0 z 0 ＝[0 0 1] T ； 0 T i-1 (1:3,3)、 0 T i-1 (1 0 T i-1 The 1 st to 3 rd elements in the 3 rd and 4 th columns represent vectors 0 z i-1 And 0 p i-1 ； 0 T n (1 0 T n 1 st to 3 rd elements of the 4 th column are end position vectors 0 p n 。
By substituting the formula (18) and the formula (19) into the formula (16), a Jacobian matrix using the base coordinate system {0} as a reference system can be obtained 0 J(q)。
Referring to FIG. 4, according to the task requirements, the input known conditions are:
1) The expected pose of the end effector of the rope-driven mechanical arm in the task space is x e (or
)；
2) Current joint angle theta c ；
3) D-H coordinate system parameters { a, alpha, D, theta } (, connecting rod torsion angle, connecting rod distance and connecting rod included angle);
4) Desired positions of 4 control points
Wherein: a-connecting rod length;
alpha-connecting rod torsion angle;
d is the link distance;
theta is the link angle;
and further calculating the current pose difference dx, then obtaining the relation between the dx and a joint variable d theta by a gradient projection method by using a mapping Jacobian matrix of a joint space and a task space, and performing motion control on the rope-driven mechanical arm by taking (theta + d theta) as new input of the joint of the rope-driven mechanical arm to realize the motion of the rope-driven mechanical arm.
(5) Introduction of 'whole arm configuration-end pose' collaborative planning parameter function
Through the analysis, the 'whole arm configuration' and the 'tail end pose' can be respectively defined as parameters capable of being actively controlled in the track planning process of the rope-driven mechanical arm, and the effective control on the configuration of the rope-driven mechanical arm is realized.
(1) When the scale factor k =0, the whole-arm configuration is obtained based on the minimum norm, and is only related to the end pose parameter;
(2) when the scale factor k is not equal to 0, the configuration of the whole arm is simultaneously influenced by the control point and the terminal pose parameter due to the existence of self-movement;
(3) when the control point C i And node N i When the coordinate values are equal, the control points have no influence on the whole arm configuration;
(4) when the control point C i And node N i When the coordinate values are unequal, the whole arm is adjusted to enable the node N i Toward control point C i Moving;
(5) when the parameters of the tail end position are all restricted, the joint angle of the rope-driven mechanical arm depends on the parameters of the configuration of the whole arm;
(6) when the tip position parameter is partially constrained, the tip position parameter adjustment arm configuration without constraint may be changed.
For ease of understanding, the present invention will be described separately for scale factors, control points, and end pose parameters.
Referring to FIG. 5, to discuss the effect of the "arm configuration" and "end pose" parameters on the rope-driven robotic arm, the embodiments are connected in series in the example 0 p 0 、 0 p 9 And 0 p 18 the three points are used as the reference arm profiles of the arm types, and the state of the whole arm configuration can be macroscopically reflected through the change of the arm profiles.
When the scale factor k =0 (the damping coefficient c = 0), solving the inverse kinematics solution of the rope-driven mechanical arm by adopting the generalized inverse of the jacobian ratio, wherein the inverse kinematics solution of the rope-driven mechanical arm is the minimum norm solution for the expected end pose.
Referring to FIG. 5, before parameter changes are implemented
After the parameters are changed
The included angle therebetween is used as an arm angle between the characterizing arm profiles, and fig. 5 is a schematic diagram of the arm angle formed by a specific embodiment of the collaborative planning method for solving the inverse kinematics of the rope-driven mechanical arm under the damping coefficients c =0 and c = -0.8.
Therefore, when the scale factor k ≠ 0 (damping coefficient c ≠ 0), according to equation (14), the damping coefficient c affecting the scale factor k is used as an independent variable to calculate the mapping between the angle cosine value cos (ψ) of the dependent variable arm profile and the damping coefficient c, that is, the spatial configuration of the rope-driven robot arm can be adjusted accordingly by adjusting the damping coefficient c, wherein when the damping coefficient c =0, the rope-driven robot arm configuration corresponding to the reference arm profile.
Referring to fig. 6, fig. 6 is a schematic diagram of a control point distribution for adjusting a "whole arm configuration" according to an embodiment of the collaborative planning method for solving the inverse kinematics of the rope-driven robot arm in the present invention. When the tail end pose of the rope-driven mechanical arm is fixed, the middle configuration of the whole arm is influenced by a self-motion scale factor k (damping coefficient c), and the configuration of the rope-driven mechanical arm can be adjusted by adjusting a performance index H (theta) in an equation (9). To the characteristics of the rope-driven robot arm, 4 control points C are set in this example i And (i =1,2,3, 4), and changing the performance index H (theta) by changing the spatial position of the control point, thereby realizing the control of the whole arm configuration of the rope-driven mechanical arm.
For the rope-driven mechanical arm in the embodiment, the control points and the coordinate system are respectively arrangedThe relationship is set as: control point C 1 Located in coordinate system 4, control point C 2 Located in the coordinate system {8}, control point C 3 Located in coordinate system {12}, control point C 4 Located in the coordinate system 16.
To control point C 3 The analysis was carried out for the sake of example. When the control point is selected as C 3 The corresponding coordinate system is {12}, desirably predominantly through the joint angle θ 1 ,θ 2 …θ 12 The configuration of the arm needs to be adjusted by changing the self-movement of (2), and therefore the performance index function of the equation (10) needs to be modified to be θ 1 ,θ 2 …θ 12 The joint angle is a function of equation (20):
the configuration of the rope-driven mechanical arm can be adjusted by changing the angle alpha of the control circle. And under the condition of the same control angle, the configuration of the rope-driven mechanical arm is changed due to different control circle radiuses.
Meanwhile, the state of the end pose is determined by the working target, so the end pose change planned based on the working target also causes the corresponding change of the configuration of the rope-driven mechanical arm.
Based on the method, the invention also provides a collaborative planning system for solving the inverse kinematics of the rope-driven mechanical arm, which comprises
A position selection unit: the system comprises a control point, a position sensor and a display, wherein the control point is used for acquiring position coordinates of the control point in a space according to the position of the controlled point and operation requirements;
a position detection unit: the device is used for detecting the current position of the rope-driven mechanical arm so as to calculate the Euclidean distance between the control point and the controlled point;
a position determination unit: the device is used for judging whether the Euclidean distance is greater than a corresponding safety threshold value or not and judging whether the Euclidean distance between the control point and the controlled point is zero or not;
a planning unit: the system is used for calculating the current pose difference dx, obtaining the relation between the dx and a joint variable d theta by using a Jacobian matrix and a gradient projection method, and performing motion control on the rope-driven mechanical arm by taking (theta + d theta) as new input of a rope-driven mechanical arm joint;
an input unit: the rope driving mechanical arm is used for inputting the planned joint configuration and the angular speed of the joint angle of the rope driving mechanical arm to realize the movement of the rope driving mechanical arm;
and adjusting and improving corresponding parameters to realize collaborative planning processing of the rope-driven mechanical arm. The collaborative planning parameters comprise configuration parameters of the whole arm and pose parameters of the tail end.
The description of the specific working process of the collaborative planning system for solving the inverse kinematics of the rope-driven mechanical arm can refer to the description of the collaborative planning method for solving the inverse kinematics of the rope-driven mechanical arm, and is not repeated herein.
In addition, the present invention also provides a computer storage medium having a computer program stored thereon, which when executed by a processor, performs the steps of:
setting i controlled points at an end effector and a middle joint of a rope-driven mechanical arm, and correspondingly setting i control points in space;
setting the controlled points as nodes between every N degrees of freedom on the rope-driven mechanical arm according to the characteristics of the rope-driven mechanical arm, wherein the 'proximity' between the controlled points and the corresponding control points is a target to be optimized;
step three, judging whether any Euclidean distance is larger than a corresponding safety threshold value, if so, enabling the Euclidean distance to approach zero through global optimization based on a gradient projection method, and accordingly obtaining an expected configuration of the rope-driven mechanical arm in space;
step four, judging whether any Euclidean distance is zero, and if so, indicating that the configuration of the rope-driven mechanical arm is completely consistent with the expected configuration;
and fifthly, judging whether any Euclidean distance is zero, if not, optimizing the whole arm freedom degree of the rope-driven mechanical arm to enable the weighted sum of the proximity degrees to be minimum, namely enabling the consistency of the whole arm configuration of the rope-driven mechanical arm and the configuration set by the control point to reach an optimized state, and achieving a 'whole arm configuration-terminal pose' collaborative planning target.
The working process of the computer program stored in the computer storage medium may refer to the description of the collaborative planning method for solving the inverse kinematics of the rope-driven mechanical arm, and is not described herein again.
Meanwhile, the method is suitable for various super-redundant rope-driven mechanical arms besides the rope-driven mechanical arm.
While the preferred embodiments of the present invention have been illustrated and described, it will be understood by those skilled in the art that various changes in form and details may be made therein without departing from the spirit and scope of the invention as defined by the appended claims.
Claims (7)
1. A collaborative planning method for solving inverse kinematics of a rope-driven mechanical arm comprises the following steps:
setting i controlled points at an end effector and a middle joint of a rope-driven mechanical arm, and correspondingly setting i control points in space;
setting the controlled points as nodes between every N degrees of freedom on the rope-driven mechanical arm according to the characteristics of the rope-driven mechanical arm, wherein the 'proximity' between the controlled points and the corresponding control points is a target to be optimized;
step three, judging whether any Euclidean distance is larger than a corresponding safety threshold value, if so, enabling the Euclidean distance to approach zero through global optimization based on a gradient projection method, and accordingly obtaining an expected configuration of the rope-driven mechanical arm in space;
step four, judging whether any Euclidean distance is zero, and if so, indicating that the configuration of the rope-driven mechanical arm is completely consistent with the expected configuration;
and step five, judging whether any Euclidean distance is zero, if not, optimizing the whole arm freedom degree of the rope-driven mechanical arm to enable the weighted sum of the proximity degrees to be minimum, namely enabling the consistency of the whole arm configuration of the rope-driven mechanical arm and the configuration set by the control point to reach an optimized state, and achieving a 'whole arm configuration-tail end pose' collaborative planning target.
2. The collaborative planning method for solving inverse kinematics of a rope-driven mechanical arm according to claim 1, wherein:
for the selection of the controlled points, the redundancy of the degrees of freedom of the rope-driven mechanical arm is fully utilized, and the joint nodes between every N degrees of freedom are set as the controlled points of the rope-driven mechanical arm;
for selection of control points, in order to achieve the aim of flexibly adjusting the intermediate configuration of the rope-driven mechanical arm according to an operator, a variable control point-based track planning method for the rope-driven mechanical arm is provided, position coordinates of the control points in space are obtained in an 'on-demand' grabbing point mode, the grabbing points are projected to a YZ plane of a classic D-H (Denavit-Hartenberg) coordinate system in an algorithm through a coordinate conversion mode, and the intersection points of connecting lines of a foot and a circle center and a control circle are the control points.
3. The collaborative planning method for solving inverse kinematics of a rope-driven robotic arm according to any one of claims 1-2, wherein:
the gradient projection method is applied to solve the problem that the spatial configuration of the rope-driven mechanical arm has the maximum self-motion change rate along which direction under a certain combination of the current joint space, the joint configuration of the rope-driven mechanical arm is optimized, and meanwhile the angular speed of the joint angle of the rope-driven mechanical arm is optimized.
Establishing a mapping Jacobian (Jacobian) matrix of a joint space and a task space, and inputting known conditions:
1) The expected pose of the end effector of the rope-driven mechanical arm in the task space;
2) A current joint angle;
3) D-H coordinate system parameters;
4) A desired position of the control point;
and further calculating the current pose difference, obtaining the relation between the pose difference and the joint variable by using a Jacobian matrix and a gradient projection method, calculating new input of the joint of the rope-driven mechanical arm, and performing motion control on the rope-driven mechanical arm to realize the motion of the rope-driven mechanical arm.
4. The collaborative planning method for solving inverse kinematics of a rope-driven mechanical arm according to claims 1 to 3, characterized in that:
realizing the 'whole arm configuration-tail end pose' collaborative planning processing of the rope-driven mechanical arm by adjusting and improving corresponding parameters;
the collaborative planning parameters comprise whole arm configuration parameters and end pose parameters.
5. A collaborative planning system for solving inverse kinematics of a rope-driven mechanical arm is characterized by comprising:
a position selection unit: the system is used for acquiring the position coordinates of the control point in the space according to the operation requirement;
a position detection unit: the device is used for detecting the current configuration of the rope-driven mechanical arm so as to calculate the Euclidean distance between the control point and the controlled point;
a position determination unit: the device is used for judging whether the Euclidean distance is greater than a corresponding safety threshold value or not and judging whether the Euclidean distance between the control point and the controlled point is zero or not;
a planning unit: the system is used for calculating the current pose difference, obtaining the relation between the pose difference and the joint variable through a gradient projection method by utilizing a Jacobian matrix, calculating new input of the rope-driven mechanical arm joint, and performing motion control on the rope-driven mechanical arm;
an input unit: the rope driving mechanical arm is used for inputting the planned joint configuration and the angular speed of the joint angle of the rope driving mechanical arm to the rope driving mechanical arm, and the movement of the rope driving mechanical arm is realized.
6. The collaborative planning system for solving inverse kinematics of a rope-driven robotic arm according to claim 5, wherein:
realizing collaborative planning processing of the rope-driven mechanical arm by adjusting and improving corresponding parameters;
the collaborative planning parameters comprise whole arm configuration parameters and end pose parameters.
7. A computer storage medium, having stored thereon a computer program which, when executed by a processor, performs the steps of:
setting i controlled points at an end effector and a middle joint of a rope-driven mechanical arm, and correspondingly setting i control points in space;
setting the controlled points as nodes between every N degrees of freedom on the rope-driven mechanical arm according to the characteristics of the rope-driven mechanical arm, wherein the 'proximity' between the controlled points and the corresponding control points is the target to be optimized;
step three, judging whether any Euclidean distance is larger than a corresponding safety threshold value, if so, enabling the Euclidean distance to approach zero through global optimization based on a gradient projection method, and accordingly obtaining an expected configuration of the rope-driven mechanical arm in space;
step four, judging whether any Euclidean distance is zero, and if so, indicating that the configuration of the rope-driven mechanical arm is completely consistent with the expected configuration;
and step five, judging whether any Euclidean distance is zero, if not, optimizing the whole arm freedom degree of the rope-driven mechanical arm to enable the weighted sum of the proximity degrees to be minimum, namely enabling the consistency of the whole arm configuration of the rope-driven mechanical arm and the configuration set by the control point to reach an optimized state, and achieving a 'whole arm configuration-tail end pose' collaborative planning target.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211420266.2A CN115533920A (en) | 2022-11-15 | 2022-11-15 | Collaborative planning method and system for solving inverse kinematics of rope-driven mechanical arm and computer storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211420266.2A CN115533920A (en) | 2022-11-15 | 2022-11-15 | Collaborative planning method and system for solving inverse kinematics of rope-driven mechanical arm and computer storage medium |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115533920A true CN115533920A (en) | 2022-12-30 |
Family
ID=84720656
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211420266.2A Pending CN115533920A (en) | 2022-11-15 | 2022-11-15 | Collaborative planning method and system for solving inverse kinematics of rope-driven mechanical arm and computer storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115533920A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116252303A (en) * | 2023-04-13 | 2023-06-13 | 山西大学 | Rope drives continuous type robot rapid motion planning system |
-
2022
- 2022-11-15 CN CN202211420266.2A patent/CN115533920A/en active Pending
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116252303A (en) * | 2023-04-13 | 2023-06-13 | 山西大学 | Rope drives continuous type robot rapid motion planning system |
| CN116252303B (en) * | 2023-04-13 | 2023-07-18 | 山西大学 | Rope drives continuous type robot rapid motion planning system |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US11845186B2 (en) | Inverse kinematics solving method for redundant robot and redundant robot and computer readable storage medium using the same | |
| CN109822554B (en) | Underwater dual-arm cooperative grabbing, holding and collision avoidance integrated method and system | |
| Chen et al. | A deep reinforcement learning based method for real-time path planning and dynamic obstacle avoidance | |
| CN108908331B (en) | Obstacle avoidance method and system for super-redundant flexible robot and computer storage medium | |
| Gong et al. | Analytical inverse kinematics and self-motion application for 7-DOF redundant manipulator | |
| WO2018107851A1 (en) | Method and device for controlling redundant robot arm | |
| CN109901397B (en) | Mechanical arm inverse kinematics method using particle swarm optimization algorithm | |
| CN110682286B (en) | Real-time obstacle avoidance method for cooperative robot | |
| CN103984230B (en) | A kind of space manipulator pedestal zero disturbs optimal control method | |
| CN110561419B (en) | Arm-shaped line constraint flexible robot track planning method and device | |
| CN107685343A (en) | A kind of Mechanical transmission test parameter calibration configuration optimization method | |
| CN113146600A (en) | Flexible robot trajectory planning method and device based on kinematics iterative learning control | |
| CN111645079A (en) | Device and method for planning and controlling mechanical arm path of live working robot | |
| CN108356820B (en) | Inverse kinematics solving method for manual control of multi-joint mechanical arm | |
| CN115533920A (en) | Collaborative planning method and system for solving inverse kinematics of rope-driven mechanical arm and computer storage medium | |
| Abbasnejad et al. | Architecture optimization of 4PUS+ 1PS parallel manipulator | |
| Banga et al. | Modeling and simulation of robotic arm movement using soft computing | |
| CN111531532A (en) | Robot climbing movement speed modeling method based on momentum theory | |
| An et al. | Application of an improved particle swarm optimization algorithm in inverse kinematics solutions of manipulators | |
| Sotiropoulos et al. | Optimum docking of an unmanned underwater vehicle for high dexterity manipulation | |
| CN114274145A (en) | Real-time obstacle avoidance method for multiple mechanical arms in laparoscopic surgery | |
| Hao et al. | Virtual-leader based formation control with constant bearing guidance for underactuated AUVs | |
| Liu et al. | Task Planning of Manipulator Based on Dynamic Space Constraint and Torque Sensor | |
| CN113547519B (en) | Variable-dimension scaling-based mechanical arm inverse kinematics solving method | |
| TWI808852B (en) | Method for stable control of six-axis robotic arm by deep learning |
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 |