CN113084797A - Dynamic cooperative control method for double-arm redundant mechanical arm based on task decomposition - Google Patents

Dynamic cooperative control method for double-arm redundant mechanical arm based on task decomposition Download PDF

Info

Publication number
CN113084797A
CN113084797A CN202110269727.XA CN202110269727A CN113084797A CN 113084797 A CN113084797 A CN 113084797A CN 202110269727 A CN202110269727 A CN 202110269727A CN 113084797 A CN113084797 A CN 113084797A
Authority
CN
China
Prior art keywords
arm
arms
speed
double
control
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
CN202110269727.XA
Other languages
Chinese (zh)
Other versions
CN113084797B (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.)
South China University of Technology SCUT
Original Assignee
South China University of Technology SCUT
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 South China University of Technology SCUT filed Critical South China University of Technology SCUT
Priority to CN202110269727.XA priority Critical patent/CN113084797B/en
Publication of CN113084797A publication Critical patent/CN113084797A/en
Application granted granted Critical
Publication of CN113084797B publication Critical patent/CN113084797B/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/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
    • 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
    • B25J9/1666—Avoiding collision or forbidden zones
    • 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/1679—Programme controls characterised by the tasks executed
    • B25J9/1682—Dual arm manipulator; Coordination of several manipulators

Landscapes

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

Abstract

The invention discloses a dynamic cooperative control method for double-arm redundant mechanical arms based on task decomposition, which comprises the following steps of: target tasks of the given double arms; selecting a plurality of characteristic points for the two arms; establishing a kinematic equation of a double-arm system, and solving the position and the speed of a control point of a double arm; establishing a self-obstacle avoidance task of two arms through a control point, establishing a joint speed general solution equation of the redundant mechanical arm, and adding a dynamic decision factor and the self-obstacle avoidance task of the two arms; until both arms complete the target task. The dynamic decision control method for redundant double-arm kinematics task decomposition is realized, and different control strategies can be adopted according to the real-time motion state of double arms, so that the double arms complete given target tasks on the basis of ensuring safety.

Description

Dynamic cooperative control method for double-arm redundant mechanical arm based on task decomposition
Technical Field
The invention belongs to the technical field of decision control of a double-arm robot, and relates to a dynamic cooperative control method of double-arm redundant mechanical arms based on task decomposition.
Background
With the development of science and technology and the progress of industrial production, a single mechanical arm system is often difficult to meet the requirements of automation and intellectualization of a robot, and a dual-redundancy mechanical arm system is taken as one of the key points of robot research by virtue of high adaptability, strong collaboration and high flexibility. Because of the problem of overlapping operation spaces of the two-arm system, the research of the two-arm coordination and collision avoidance method is the key point of the two-arm system.
At present, various methods and strategies exist for the problem of the coordination control of the two arms. Many studies focus on keeping the arms at a reasonable safe distance during motion control; or find a collision-free path in the feasible space. For solving the problem of double-arm self-obstacle avoidance, the following solutions are available at present:
(1) planning a collision-free path from a starting point to a terminal point in the configuration space;
(2) normalizing the initial planned path, establishing a coordination space, and changing the speed on a path point to enable two arms to avoid each other before collision;
(3) the distance between the two arms is used as a limiting condition for motion control, and the safe distance of the two arms is maintained;
(4) a potential field is established for the two arms, and the two arms have mutual repulsion, so that the two arms are guided to keep a relatively safe distance.
However, in the above method, the multi-degree-of-freedom two-arm system has a problem of high-dimensional calculation in the configuration space and the coordination space. The multi-degree-of-freedom mechanical arm system has higher calculation complexity and is difficult to meet the timeliness requirement of real-time control; the limiting double-arm distance function only considers the relative distance between double arms, does not take the relative speed as the content of consideration, and is difficult to avoid the special condition that collision happens when the mechanical arm runs at high speed. Wu Changcheng et al put forward a method for detecting mechanical arm self-collision based on space vector geometric distance in the double-arm robot self-collision detection and motion planning, and plan self-obstacle avoidance by using linear repulsion as an operator through an improved artificial potential field method. And constructing a potential field and the like by constructing a corresponding field function through integration of the mechanical arm connecting rod, wherein the field function is used as a basis for mechanical arm control. On one hand, the calculation complexity is improved by constructing a field in a three-dimensional space, and on the other hand, the potential field method has the problem that the mechanical arm falls into a local minimum value, and finally, the task is difficult to complete. Aiming at the problems, on one hand, the method takes the relative speed between the two arms as one of the control bases, and can deal with the dangerous situation of high-speed approach between the two arms; on the other hand, an obstacle avoidance task is directly constructed in a working space, the conversion of a field function or other spaces is avoided, the planning efficiency is improved, and the real-time performance of mechanical arm control is enhanced.
Disclosure of Invention
The invention aims to overcome the defects of the prior art and provides a dynamic cooperative control method of double-arm redundant mechanical arms based on task decomposition.
The invention is realized by at least one of the following technical schemes.
A dynamic cooperative control method for double-arm redundant mechanical arms based on task decomposition comprises the following steps:
step 1, respective target tasks of two arms are given;
step 2, setting a plurality of characteristic points on the double arms to show the spatial positions of the double arms;
step 3, establishing a kinematic equation of the double-arm system, solving the spatial position of the characteristic point, correspondingly determining the spatial position of the control point, and acquiring the spatial speed of the double-arm control point;
step 4, determining a self-obstacle avoidance task of the two arms, establishing a joint speed general solution equation of the redundant mechanical arm, and adding a dynamic decision factor and the self-obstacle avoidance task of the two arms;
and 5, judging whether the double arms finish the target task or not, if not, going to the step 3.
Preferably, step 1 specifically comprises: given the target position of the end of the robot arm in cartesian space:
[xd,yd,zd]
wherein x isd,yd,zdRespectively are three-dimensional coordinate values under a mechanical arm base coordinate system.
Preferably, step 2 specifically comprises: a plurality of characteristic points are set at joints and connecting rods of the two arms, the characteristic points are set at the centers of all joints of the mechanical arm, and the characteristic points are set at the centers of the first three connecting rods of the mechanical arm.
Preferably, the spatial positions of the control points are determined as follows:
a certain side arm base coordinate system is used as a reference coordinate system, and the reference coordinate system is obtained through a positive kinematic equationPosition of 2n characteristic points of the arms, using Pri(i is 1,2, …, n) represents the position of the ith characteristic point of the side arm, n represents the number of characteristic points, and P is used for representing the number of the characteristic pointslj(j ═ 1,2, …, n) represents the position of the jth characteristic point of the other side arm:
Pri＝fri(qr)＝(xri,yri,zri)
fri(. is a point P)riPositive kinematic formula of (a), qrIs the joint angle vector of the side arm, (x)ri,yri,zri) Is PriThree-dimensional space coordinate values of (a);
Pli＝fli(ql)+[0,L,0]＝(xli,yli,zli)
fli(. is a point P)liPositive kinematic formula of (a), qlIs the joint angle vector of the other side arm, (x)li,yli,zli) Is PliThe coordinate values of (a); calculating the distance between each characteristic point of the two arms by dijRepresents the distance between the ith characteristic point of one side arm and the jth characteristic point of the other side arm:
dij＝‖Pri-Plj‖
maintaining an n x n distance as D, selecting two characteristic points with shortest distance between the characteristic points of the two arms as control points of the two arms, and using PrRepresenting the position of the control point of a side arm, by PlRepresenting the control point position of the other side arm.
Preferably, in step 3, the space velocity of the two-arm control point is obtained through a jacobian matrix.
Preferably, the obtaining of the spatial velocity of the dual-arm control point specifically includes:
obtaining the Cartesian space velocity at the control point through the joint velocity vector of the two arms and the Jacobian matrix at the control point, and using vrRepresenting the control point velocity of a side arm, by vlControl point speed representing the other side arm:
Figure BDA0002973759400000031
Figure BDA0002973759400000032
wherein, JrAnd JlAre respectively PrAnd PlThe jacobian matrix of (a) is,
Figure BDA0002973759400000033
and
Figure BDA0002973759400000034
the joint velocity vectors at this time of both arms are respectively.
Preferably, the step 4 specifically comprises:
determining a relative position vector P of two control pointsa：
Pa＝Pr-Pl
Setting the retraction speed to vbAnd the speed of the distance between the two arm control points is enlarged, the speed direction is the same as the direction of the relative position vector, and the speed changes along with the distance between the control points:
Figure BDA0002973759400000035
where d represents the spatial distance between the two-arm control points, γbControlling the speed of obstacle avoidance varying with d, sigmabA control threshold value for starting to act on the obstacle avoidance speed;
determining relative velocity vectors v for two control pointsa：
va＝vl-vr
Angle between relative position vector and relative velocity vector
Figure BDA0002973759400000036
Figure BDA0002973759400000037
Setting of evasion speed vdWhen one side arm is continuously approaching, the corresponding control point of the other side arm generates a speed dodging to the side, the direction of the speed is orthogonal to the relative position vector and the relative speed vector, and the magnitude of the speed is changed along with the distance of the control points:
Figure BDA0002973759400000038
in the formula, gammadControlling the speed of the dynamic decision factor changing with d, gammadA control threshold parameter that acts for the dynamic decision factor to begin;
adding the back-off speed and the dodging speed to obtain the dodging speed vh：
vh＝vb+vd
Mapping avoidance speed to joint space through jacobian matrix of control points
Figure BDA0002973759400000041
Figure BDA0002973759400000042
In the formula
Figure BDA0002973759400000043
A generalized inverse of a jacobian matrix that is a control point;
for the redundant mechanical arm, the degree of freedom is assumed to be m, and the ith joint angle is assumed to be qi(i is 1,2, …, m), and the corresponding joint angle vector is q, completing the target trajectory ydThe general solution for the required joint velocity is:
Figure BDA0002973759400000044
wherein J is the target trajectory ydCorresponding toJacobian matrix, J+Is a generalized inverse matrix of J, ImIs an m-order identity matrix, k is an arbitrary m-dimensional space vector,
Figure BDA0002973759400000045
is a differential function of the target trajectory.
Preferably, the dynamic decision factor α is:
Figure BDA0002973759400000046
wherein the dynamic decision factor α is:
Figure BDA0002973759400000047
in the formula, d represents the space distance between two arms of control points, gamma controls the speed of the dynamic decision factor changing along with d, and sigma is a control threshold value for the dynamic decision factor to start to act;
the angular velocity of each shaft joint when the mechanical arm realizes the self-obstacle avoidance action is used as the sub-motion of the mechanical arm motion:
Figure BDA0002973759400000048
preferably, if the error between the current position of the mechanical arm and the target position is within the threshold, it indicates that the two arms complete the target task, and if the error between the current position of the mechanical arm and the target position exceeds the threshold, the process returns to step 3.
Preferably, the step 5 specifically comprises:
acquiring the current position and posture of the mechanical arm through a positive kinematic equation:
Pe＝fe(q)＝[xe,ye,ze]
wherein, PeIs the position of the end of the arm, fe(. to) is a positive kinematic formula of the tail end of the mechanical arm, and q is a current joint angle vector of the mechanical arm，[xe,ye,ze]Is PeA spatial coordinate value of (a);
given an error threshold e, if the error of the current position of the robotic arm from the target position is within the threshold:
Figure BDA0002973759400000051
and finishing control, and if the error between the current position of the mechanical arm and the target position exceeds a threshold value:
Figure BDA0002973759400000052
and returning to the step 3.
Compared with the prior art, the invention has the following beneficial effects:
the spatial position of the mechanical arm is represented by a plurality of characteristic points, so that the spatial expression of the multi-link structure is simplified. And (4) comprehensively considering the relative distance and the relative speed of the control points of the double arms, and establishing the self-obstacle avoidance task of the double arms. And the double arms can realize different control strategies under different conditions by utilizing the dynamic decision factors.
Drawings
FIG. 1 is a view showing a robot arm configuration of the present example;
FIG. 2 is a schematic diagram of the dual-arm feature point selection of the present example;
FIG. 3 is a diagram illustrating simulation results of angular positions of joints of a right arm when a dynamic decision control method of task decomposition is used in example 1;
FIG. 4 is a diagram illustrating simulation results of the angle positions of joints of the left arm when a dynamic decision control method of task decomposition is used in example 1;
FIG. 5 is a diagram illustrating simulation results of angular positions of a right arm joint in example 1 without using a dynamic decision control method for task decomposition;
FIG. 6 is a diagram illustrating simulation results of the positions of the left arm joint angles in example 1 without using a dynamic decision control method for task decomposition;
FIG. 7 is a diagram of the shortest distance between two arms in case of using the dynamic decision control method of task decomposition in example 1;
FIG. 8 is a schematic diagram of the shortest distance between two arms of example 1 without using the dynamic decision control method of task decomposition;
FIG. 9 is a schematic diagram of the end position of the right arm in case of using the dynamic decision control method of task decomposition in example 1;
FIG. 10 is a schematic diagram of example 2 dual arm position;
FIG. 11 is a schematic diagram of the two-arm position in solid lines for example 2 using the dynamic decision control method of task decomposition;
FIG. 12 is a schematic diagram of a two-arm position dashed line when the dynamic decision control method of task decomposition is not used in example 2;
FIG. 13 is a schematic diagram of the dual arm position of example 2 without the use of a dynamic decision control method of task decomposition;
FIG. 14 is a schematic diagram of the dual arm end positions of example 2 without the use of a dynamic decision control method of task decomposition;
FIG. 15 is a diagram of dual arm end positions when using the dynamic decision control method of task decomposition for example 2;
FIG. 16 is a schematic diagram of the dual arm end positions when using the dynamic decision control method of task decomposition of example 2;
FIG. 17 is a schematic diagram of the dual arm end positions when using the dynamic decision control method of task decomposition of example 3;
FIG. 18 is a schematic diagram of the dual arm end positions when using the dynamic decision control method of task decomposition of example 3;
FIG. 19 is a schematic diagram of the dual arm end positions of example 3 without the use of the dynamic decision control method of task decomposition;
FIG. 20 is a schematic diagram of the dual arm end positions of example 3 without the use of the dynamic decision control method of task decomposition;
FIG. 21 is a schematic diagram of the dual arm end positions of example 3 without the use of the dynamic decision control method of task decomposition;
FIG. 22 is a schematic diagram of the dual arm end positions when using the dynamic decision control method of task decomposition of example 3;
FIG. 23 is a schematic diagram of the dual arm end positions when using the dynamic decision control method of task decomposition of example 3;
FIG. 24 is a schematic diagram of the dual arm end positions when using the dynamic decision control method of task decomposition of example 4;
FIG. 25 is a schematic diagram of the dual arm end positions when using the dynamic decision control method of task decomposition of example 4;
FIG. 26 is a schematic diagram of the dual arm end positions of example 4 without the use of the dynamic decision control method of task decomposition;
FIG. 27 is a schematic diagram of the dual arm end positions of example 4 without the use of the dynamic decision control method of task decomposition;
FIG. 28 is a schematic diagram of the dual arm end positions of example 4 without the use of the dynamic decision control method of task decomposition;
FIG. 29 is a schematic diagram of the dual arm end positions when using the dynamic decision control method of task decomposition of example 4;
FIG. 30 is a schematic diagram of the two-arm end positions when using the dynamic decision control method of task decomposition in example 4.
Detailed Description
The embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings.
As shown in fig. 1 and fig. 2, the method for dynamic cooperative control of dual-arm redundant manipulator based on task decomposition in this embodiment includes the following steps:
step 1, respective target tasks of the two arms are given, namely the target position of the tail end of the mechanical arm in a Cartesian space is given:
[xd,yd,zd]
wherein x isd，yd，zdRespectively are three-dimensional coordinate values under a mechanical arm base coordinate system.
And 2, setting a plurality of characteristic points at the joints and the connecting rods of the two arms, setting the characteristic points at the centers of the joints of the mechanical arm, and setting the characteristic points at the center of the longer connecting rod. The characteristic points approximately represent the spatial positions of the two arms, and a plurality of characteristic points are used as spatial expressions of the two-arm system.
Step 3, establishing a kinematic equation of the double-arm system, solving the spatial position of the characteristic points, determining the spatial position of the control points, and acquiring the spatial speed of the double-arm control points through a Jacobian matrix, wherein the kinematic equation specifically comprises the following steps:
and (3) taking the right arm base coordinate system as a reference coordinate system, and obtaining the positions of all characteristic points of the two arms through a positive kinematic equation:
Pri＝fri(qr)＝(xri，yri，zri)
wherein (x)ri，yri，zri) Is a point PriThree-dimensional space coordinate value of fri(. is a point P)riPositive kinematic formula of (a), qrIs the joint vector of the right arm, Pri(i is 1,2, …, n) represents the position of the ith feature point of the right arm, n represents the number of feature points, and P is usedlj(j ═ 1,2, …, n) represents the position of the jth feature point of the left arm:
Pli＝fli(ql)+[0，L，0]＝(xli，yli，zli)
wherein f isli(. is a point P)liPositive kinematic formula of (a), qlIs the joint vector of the left arm, (x)li，yli，zli) Is PliThe three-dimensional space coordinate value of (2).
Calculating the distance between each characteristic point of the two arms by dijRepresents the distance between the ith characteristic point of the right arm and the jth characteristic point of the left arm:
dij＝‖Pri-Plj‖
maintaining an n x n distance as D, selecting two characteristic points with shortest distance between the characteristic points of the two arms as control points of the two arms, and using PrRepresenting the position of the control point of the right arm, by PlRepresenting the control point position of the left arm.
By the joint velocity vector of the two arms and the jacobian matrix at the control point at this time, the cartesian space velocity at the control point can be obtained:
Figure BDA0002973759400000071
Figure BDA0002973759400000072
wherein, JrAnd JlAre respectively PrAnd PlThe jacobian matrix of (a) is,
Figure BDA0002973759400000073
and
Figure BDA0002973759400000074
the joint velocity vectors v of the right arm and the left arm at the momentrRepresenting the control point velocity, v, of the right armlRepresenting the control point velocity of the left arm.
Step 4, determining a self-obstacle avoidance task of two arms, establishing a joint speed general solution equation of the redundant mechanical arm, and adding a dynamic decision factor and the self-obstacle avoidance task of the two arms, wherein the method specifically comprises the following steps:
determining a relative position vector P of two control pointsa：
Pa＝Pr-Pl
Setting a rollback speed vb: speed of extending the two-arm control point distance. Its direction is the same as the relative position vector, its magnitude varies with control point distance:
Figure BDA0002973759400000075
where d represents the spatial distance between the two-arm control points, γbControlling the speed of obstacle avoidance varying with d, sigmabA control threshold value for starting to act on the obstacle avoidance speed;
determining relative velocity vectors v for two control pointsa：
va＝vl-vr
Relative position vector andangle between relative velocity vectors
Figure BDA0002973759400000081
Figure BDA0002973759400000082
Setting of evasion speed vd: dodging the continuously approaching left arm control point sideways. Its direction is orthogonal to the relative position vector and the relative velocity vector, its magnitude varies with the control point distance:
Figure BDA0002973759400000083
where d represents the spatial distance between the two-arm control points, γbControlling the speed of obstacle avoidance varying with d, sigmabA control threshold that acts to initiate obstacle avoidance speed.
Adding the back-off speed and the dodging speed to obtain the dodging speed vh：
vh＝vb+vd
Mapping avoidance speed to joint space through jacobian matrix of control points
Figure BDA0002973759400000084
Figure BDA0002973759400000085
In the formula
Figure BDA0002973759400000086
A generalized inverse of the jacobian matrix of the control points.
For the redundant mechanical arm, the degree of freedom is assumed to be m, and the ith joint angle is assumed to be qi(i is 1,2, …, m), and the corresponding joint vector is q, completing the target trajectory ydThe general solution to the required joint velocity is
Figure BDA0002973759400000087
Wherein J is ydJacobian matrix corresponding to the target trajectory, J+Is a generalized inverse matrix of J, ImIs an m-order identity matrix, k is an arbitrary m-dimensional space vector,
Figure BDA0002973759400000088
is a differential function of the target trajectory.
Adding a dynamic decision factor alpha:
Figure BDA0002973759400000089
wherein the dynamic decision factor α is:
Figure BDA00029737594000000810
and when the mechanical arm realizes self-obstacle avoidance action, the angular speed of each shaft joint is used as the sub-motion of the mechanical arm motion.
Figure BDA00029737594000000811
And 5, judging whether the double arms finish the target task or not, if not, going to the step 3.
Acquiring the current position and posture of the mechanical arm through a positive kinematic equation:
Pe＝fe(q)＝[xe，ye，ze]
wherein, PeIs the position of the end of the arm, fe(. h) is a positive kinematic formula of the end of the mechanical arm, q is the current joint vector of the mechanical arm, [ x ]e,ye,ze]Is PeThe three-dimensional space coordinate value of (2).
Given an error threshold e, if the error between the current position of the mechanical arm and the target position is within a certain threshold:
Figure BDA0002973759400000091
the control method ends. If the error between the current position of the mechanical arm and the target position exceeds a threshold value:
Figure BDA0002973759400000092
and returning to the step 3.
The process of the invention is applied to the specific example 1 below:
1) given the target location:
2) left arm target position: [ 0.300; -0.300; 0.000],
3) left arm current position:
4)ql[0.1；-1.27；-1.574；-0.3045；2.854；0.0]→[-0.4930；-0.8244；-1.1924；-0.3680；3.1993；0.0]
5) right arm target position: [ 0.500; 0.200; -0.250],
6) current position of right arm:
7)qr[0.538；-0.883；-0.8633；-0.13；2.643；0]→[-0.1959；-0.4492；-1.8690；-1.3579；0.8448；-0.0258]
the simulation results are shown as the solid lines in fig. 3, fig. 4, and fig. 7, fig. 8, and fig. 9, and it can be seen from the simulation results that when the dynamic decision control method of redundant dual-arm kinematics task decomposition is used, the dual arms have reached the target position at the end while ensuring the safety distance, and the target task is completed on the basis of ensuring the safety. As shown by the dotted lines in fig. 5, 6, and 7, when the dynamic decision control method of redundant dual-arm kinematics task decomposition is not used, the dual arms collide already at the initial stage of the movement, and the movement fails because the task cannot be continued.
The process of the invention is applied to the specific example 2 below:
1) given the target location:
2) left arm target position: [ 0.350; -0.260; 0.050],
3) left arm current position:
4)ql[0.1；-1.27；-1.574；-0.3045；2.854；0.0]→[-0.3791；-1.1397；-1.4941；-0.3544；2.8478；0.0]
5) right arm target position: [ 0.500; 0.250 of the total weight of the mixture; -0.200],
6) current position of right arm:
7)qr[0.538；-0.883；-0.8633；-0.13；2.643；0]→[-0.0275；-0.6141；-1.8763；-1.2149；0.9729；-0.0115]
the simulation results are shown in fig. 10, fig. 11, and fig. 14, which are solid lines, fig. 15, and fig. 16, and it can be seen from the simulation results that when the dynamic decision control method of redundant dual-arm kinematics task decomposition is used, the dual arms have reached the target position at the end while ensuring the safety distance, and the target task is completed on the basis of ensuring the safety. As shown by the dotted lines in fig. 12, 13, and 14, when the dynamic decision control method of redundant dual-arm kinematics task decomposition is not used, the dual arms collide already at the initial stage of the movement, and the movement fails because the task cannot be continued.
The process of the invention is applied to the specific example 3 below:
1) given the target location:
2) left arm target position: [ 0.320; -0.280; 0.025],
3) left arm current position:
4)ql[0.1；-1.27；-1.574；-0.3045；2.854；0.0]→[-0.4866；-1.2512；-1.7589；-0.5077；2.6847；0.0]
5) right arm target position: [ 0.450; 0.230; -0.200],
6) current position of right arm:
7)qr[0.538；-0.883；-0.8633；-0.13；2.643；0]→[-0.1263；-0.6679；-2.1252；-1.4080；0.8633；-0.0179]
the simulation results are shown in fig. 17, fig. 18, and fig. 21, which are solid lines, fig. 22, and fig. 23, and it can be seen from the simulation results that when the dynamic decision control method of redundant dual-arm kinematics task decomposition is used, the dual arms reach the target position while ensuring the safety distance, and complete the target task on the basis of ensuring the safety. As shown by the dotted lines in fig. 19, 20, and 21, when the dynamic decision control method of redundant dual-arm kinematics task decomposition is not used, the dual arms collide already at the initial stage of the movement, and the movement fails because the task cannot be continued.
The process of the invention is applied to the specific example 4 below:
1) given the target location:
2) left arm target position: [ 0.320; -0.260; -0.050],
3) left arm current position:
4)ql[0.1；-1.27；-1.574；-0.3045；2.854；0.0]→[-0.3855；-0.9143；-1.5747；-0.6605；3.0188；0.0]
5) right arm target position: [ 0.450; 0.220; -0.250],
6) current position of right arm:
7)qr[0.538；-0.883；-0.8633；-0.13；2.643；0]→[-0.0956；-0.4515；-2.0497；-1.5670；0.9600；-0.0159]
the simulation results are shown in fig. 24, fig. 25, and fig. 28, and fig. 29 and fig. 30, and it can be seen from the simulation results that when the dynamic decision control method of redundant two-arm kinematics task decomposition is used, the two arms reach the target position at the end while the safety distance is ensured, and the target task is completed on the basis of ensuring safety. As shown by the dotted lines in fig. 26, 27, and 28, when the dynamic decision control method of redundant dual-arm kinematics task decomposition is not used, the dual arms collide at the initial stage of the movement, and the movement fails because the task cannot be continued.
The above-mentioned contents are only for illustrating the technical idea of the present invention, and the protection scope of the present invention is not limited thereby, and any modification made on the basis of the technical idea of the present invention falls within the protection scope of the claims of the present invention.

Claims (10)

1. A dynamic cooperative control method for double-arm redundant mechanical arms based on task decomposition is characterized by comprising the following steps:
step 1, respective target tasks of two arms are given;
step 2, setting a plurality of characteristic points on the double arms to show the spatial positions of the double arms;
step 3, establishing a kinematic equation of the double-arm system, solving the spatial position of the characteristic point, correspondingly determining the spatial position of the control point, and acquiring the spatial speed of the double-arm control point;
step 4, determining a self-obstacle avoidance task of the two arms, establishing a joint speed general solution equation of the redundant mechanical arm, and adding a dynamic decision factor and the self-obstacle avoidance task of the two arms;
and 5, judging whether the double arms finish the target task or not, if not, going to the step 3.
2. The dynamic cooperative control method for the double-arm redundant manipulator based on task decomposition according to claim 1, wherein the step 1 is specifically as follows: given the target position of the end of the robot arm in cartesian space:
[xd，yd，zd]
wherein x isd，yd，zdRespectively are three-dimensional coordinate values under a mechanical arm base coordinate system.
3. The dynamic cooperative control method for the double-arm redundant manipulator based on task decomposition according to claim 2, wherein the step 2 is specifically: a plurality of characteristic points are set at joints and connecting rods of the two arms, the characteristic points are set at the centers of all joints of the mechanical arm, and the characteristic points are set at the centers of the first three connecting rods of the mechanical arm.
4. The dynamic cooperative control method for the double-arm redundant manipulator based on task decomposition as claimed in claim 3, wherein the spatial positions of the control points are determined as follows:
taking a certain side arm base coordinate system as a reference coordinateThe positions of 2n characteristic points of the double arms are obtained by a positive kinematic equation and are Pri(i ═ 1, 2.. times.n) represents the position of the ith characteristic point of the side arm, n represents the number of characteristic points, and P is used for representing the number of characteristic pointslj(j ═ 1, 2.. times, n) represents the position of the jth characteristic point of the other side arm:
Pri＝fri(qr)＝(xri，yri，zri)
fri(. is a point P)riPositive kinematic formula of (a), qrIs the joint angle vector of the side arm, (x)ri，yri，zri) Is PriThree-dimensional space coordinate values of (a);
Pli＝fli(ql)+[0，L，0]＝(xli，yli，zli)
fli(. is a point P)liPositive kinematic formula of (a), qlIs the joint angle vector of the other side arm, (x)li，yli，zli) Is PliThe coordinate values of (a); calculating the distance between each characteristic point of the two arms by dijRepresents the distance between the ith characteristic point of one side arm and the jth characteristic point of the other side arm:
dij＝||Pri-Plj||
maintaining an n x n distance as D, selecting two characteristic points with shortest distance between the characteristic points of the two arms as control points of the two arms, and using PrRepresenting the position of the control point of a side arm, by PlRepresenting the control point position of the other side arm.
5. The method for dynamic cooperative control of dual-arm redundant manipulator based on task decomposition as claimed in claim 4, wherein in step 3, the spatial velocity of the dual-arm control point is obtained by Jacobian matrix.
6. The dynamic cooperative control method for the dual-arm redundant manipulator based on task decomposition according to claim 5, wherein the obtaining of the spatial velocity of the dual-arm control point specifically comprises:
obtaining the Cartesian space velocity at the control point through the joint velocity vector of the two arms and the Jacobian matrix at the control point, and using vrRepresenting the control point velocity of a side arm, by vlControl point speed representing the other side arm:
Figure FDA0002973759390000021
Figure FDA0002973759390000022
wherein, JrAnd JlAre respectively PrAnd PlThe jacobian matrix of (a) is,
Figure FDA0002973759390000023
and
Figure FDA0002973759390000024
the joint velocity vectors at this time of both arms are respectively.
7. The dynamic cooperative control method for the dual-arm redundant manipulator based on task decomposition according to claim 6, wherein the step 4 is specifically:
determining a relative position vector P of two control pointsa：
Pa＝Pr-Pl
Setting the retraction speed to vbAnd the speed of the distance between the two arm control points is enlarged, the speed direction is the same as the direction of the relative position vector, and the speed changes along with the distance between the control points:
Figure FDA0002973759390000025
where d represents the spatial distance between the two-arm control points, γbControlling obstacle avoidance speed to vary with dFast and slow, σbA control threshold value for starting to act on the obstacle avoidance speed;
determining relative velocity vectors v for two control pointsa：
va＝vl-vr
Angle between relative position vector and relative velocity vector
Figure FDA0002973759390000026
Figure FDA0002973759390000027
Setting of evasion speed vdWhen one side arm is continuously approaching, the corresponding control point of the other side arm generates a speed dodging to the side, the direction of the speed is orthogonal to the relative position vector and the relative speed vector, and the magnitude of the speed is changed along with the distance of the control points:
Figure FDA0002973759390000031
in the formula, gammadControlling the speed of the dynamic decision factor changing with d, gammadA control threshold parameter that acts for the dynamic decision factor to begin;
adding the back-off speed and the dodging speed to obtain the dodging speed vh：
vh＝vb+vd
Mapping avoidance speed to joint space through jacobian matrix of control points
Figure FDA0002973759390000032
Figure FDA0002973759390000033
In the formula
Figure FDA0002973759390000034
A generalized inverse of a jacobian matrix that is a control point;
for the redundant mechanical arm, the degree of freedom is assumed to be m, and the ith joint angle is assumed to be qi(i 1, 2.. times, m), the corresponding joint angle vector is q, and the target trajectory y is completeddThe general solution for the required joint velocity is:
Figure FDA0002973759390000035
wherein J is the target trajectory ydCorresponding Jacobian matrix, J+Is a generalized inverse matrix of J, ImIs an m-order identity matrix, k is an arbitrary m-dimensional space vector,
Figure FDA0002973759390000036
is a differential function of the target trajectory.
8. The dynamic cooperative control method for the double-arm redundant manipulator based on task decomposition according to claim 7, wherein the dynamic decision factor α is:
Figure FDA0002973759390000037
wherein the dynamic decision factor α is:
Figure FDA0002973759390000038
in the formula, d represents the space distance between two arms of control points, gamma controls the speed of the dynamic decision factor changing along with d, and sigma is a control threshold value for the dynamic decision factor to start to act;
the angular velocity of each shaft joint when the mechanical arm realizes the self-obstacle avoidance action is used as the sub-motion of the mechanical arm motion:
Figure FDA0002973759390000039
9. the method for dynamic cooperative control of dual-arm redundant manipulator based on task decomposition of claim 8, wherein if the error between the current position of the manipulator and the target position is within a threshold, it indicates that the dual-arm completes the target task, and if the error between the current position of the manipulator and the target position exceeds the threshold, the method returns to step 3.
10. The dynamic cooperative control method for the dual-arm redundant manipulator based on task decomposition according to claim 9, wherein the step 5 is specifically:
acquiring the current position and posture of the mechanical arm through a positive kinematic equation:
Pe＝fe(q)＝[xe，ye，ze]
wherein, PeIs the position of the end of the arm, fe(. h) is a positive kinematic formula of the tail end of the mechanical arm, q is a current joint angle vector of the mechanical arm, [ x ]e，ye，ze]Is PeA spatial coordinate value of (a);
given an error threshold e, if the error of the current position of the robotic arm from the target position is within the threshold:
Figure FDA0002973759390000041
and finishing control, and if the error between the current position of the mechanical arm and the target position exceeds a threshold value:
Figure FDA0002973759390000042
and returning to the step 3.
CN202110269727.XA 2021-03-12 2021-03-12 Dynamic cooperative control method for double-arm redundant mechanical arm based on task decomposition Active CN113084797B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110269727.XA CN113084797B (en) 2021-03-12 2021-03-12 Dynamic cooperative control method for double-arm redundant mechanical arm based on task decomposition

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110269727.XA CN113084797B (en) 2021-03-12 2021-03-12 Dynamic cooperative control method for double-arm redundant mechanical arm based on task decomposition

Publications (2)

Publication Number Publication Date
CN113084797A true CN113084797A (en) 2021-07-09
CN113084797B CN113084797B (en) 2022-11-18

Family

ID=76667053

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110269727.XA Active CN113084797B (en) 2021-03-12 2021-03-12 Dynamic cooperative control method for double-arm redundant mechanical arm based on task decomposition

Country Status (1)

Country Link
CN (1) CN113084797B (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114131606A (en) * 2021-12-07 2022-03-04 亿嘉和科技股份有限公司 Task scheduling method for double-arm inspection robot

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH0720915A (en) * 1993-07-06 1995-01-24 Fanuc Ltd Synchronism control method for robot
JP2018015854A (en) * 2016-07-29 2018-02-01 セイコーエプソン株式会社 Robot, robot control device, robot system, and control method
CN108638058A (en) * 2018-04-23 2018-10-12 华南理工大学 A kind of posture decision dynamic programming method
CN110421556A (en) * 2019-06-14 2019-11-08 河北工业大学 A kind of method for planning track and even running method of redundancy both arms service robot Realtime collision free

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH0720915A (en) * 1993-07-06 1995-01-24 Fanuc Ltd Synchronism control method for robot
JP2018015854A (en) * 2016-07-29 2018-02-01 セイコーエプソン株式会社 Robot, robot control device, robot system, and control method
CN108638058A (en) * 2018-04-23 2018-10-12 华南理工大学 A kind of posture decision dynamic programming method
CN110421556A (en) * 2019-06-14 2019-11-08 河北工业大学 A kind of method for planning track and even running method of redundancy both arms service robot Realtime collision free

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114131606A (en) * 2021-12-07 2022-03-04 亿嘉和科技股份有限公司 Task scheduling method for double-arm inspection robot
CN114131606B (en) * 2021-12-07 2024-05-10 亿嘉和科技股份有限公司 Task scheduling method for double-arm inspection robot

Also Published As

Publication number Publication date
CN113084797B (en) 2022-11-18

Similar Documents

Publication Publication Date Title
JP5114019B2 (en) Method for controlling the trajectory of an effector
CN108621163A (en) A kind of redundancy tow-armed robot cooperation control method towards remittance tenon technique
CN111515949B (en) Double-arm transmission and reception position selection method for double-arm cooperative robot
CN111300420B (en) Method for solving minimum path of joint space corner of mechanical arm
KR20110015833A (en) Method and apparatus of path planing for a robot
CN108714894A (en) A kind of dynamic method for solving dual redundant mechanical arm and colliding with each other
CN110421556A (en) A kind of method for planning track and even running method of redundancy both arms service robot Realtime collision free
Li et al. A hybrid visual servo control method for simultaneously controlling a nonholonomic mobile and a manipulator
Vafaei et al. Integrated controller for an over-constrained cable driven parallel manipulator: KNTU CDRPM
CN111469129A (en) Double-mechanical-arm-based ship hull plate curved surface forming collision-free motion planning method
CN113043286A (en) Multi-degree-of-freedom mechanical arm real-time obstacle avoidance path planning system and method
CN113084797B (en) Dynamic cooperative control method for double-arm redundant mechanical arm based on task decomposition
CN117245663A (en) Rope-driven space flexible mechanical arm force position type fusion compliant control method and system
CN114800491A (en) Redundant mechanical arm zero-space obstacle avoidance planning method
CN110695994A (en) Finite time planning method for cooperative repetitive motion of double-arm manipulator
Yang et al. Collision avoidance trajectory planning for a dual-robot system: using a modified APF method
Tang et al. Coordinated motion planning of dual-arm space robot with deep reinforcement learning
CN114800527A (en) Force application control method and system for tail end of mobile operation mechanical arm
Mehmood et al. Analysis of end-effector position and orientation for 2P-3R planer pneumatic robotic arm
CN114274145A (en) Real-time obstacle avoidance method for multiple mechanical arms in laparoscopic surgery
Qizhi et al. On the kinematics analysis and motion planning of the manipulator of a mobile robot
Li et al. Teleoperation of upper-body humanoid robot platform with hybrid motion mapping strategy
CN115056230B (en) Three-wheeled omnidirectional mobile mechanical arm repetitive motion planning method based on pseudo-inverse
Kuppan Chetty et al. A heuristic approach towards path planning and obstacle avoidance control of planar manipulator
Chen et al. A novel autonomous obstacle avoidance path planning method for manipulator in joint space

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