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
dual
control
task
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

本发明公开了一种基于任务分解的双臂冗余机械臂动态协同控制方法，包括以下步骤：给定双臂的目标任务；为双臂选定多个特征点；建立双臂系统的运动学方程，求解双臂的控制点位置与速度；通过控制点建立双臂的自避障任务，建立冗余机械臂的关节速度通解方程，并加入动态决策因子和双臂的自避障任务；直至双臂完成目标任务。实现了冗余双臂运动学任务分解的动态决策控制方法，可以根据双臂实时的运动状态采取不同的控制策略，使双臂在保证安全的基础上完成给定的目标任务。

Figure 202110269727

The invention discloses a dual-arm redundant manipulator dynamic cooperative control method based on task decomposition, comprising the following steps: given a target task of the dual-arm; selecting a plurality of feature points for the dual-arm; establishing the kinematics of the dual-arm system equations to solve the position and speed of the control points of the arms; establish the self-obstacle avoidance task of the arms through the control points, establish the joint speed general solution equation of the redundant manipulator, and add the dynamic decision factor and the self-obstacle avoidance task of the arms; until Complete the target task with both arms. The dynamic decision control method of redundant dual-arm kinematics task decomposition is realized, and different control strategies can be adopted according to the real-time motion state of the dual-arm, so that the dual-arm can complete the given target task on the basis of ensuring safety.

Figure 202110269727

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.一种基于任务分解的双臂冗余机械臂动态协同控制方法，其特征在于，包括以下步骤：1. a dual-arm redundant manipulator dynamic collaborative control method based on task decomposition, is characterized in that, comprises the following steps: 步骤1、给定双臂各自的目标任务；Step 1. Given the respective target tasks of the arms; 步骤2、在双臂设置多个特征点，表示双臂所在的空间位置；Step 2. Set multiple feature points on the arms to indicate the spatial position of the arms; 步骤3、建立双臂系统的运动学方程，求解特征点的空间位置并相应确定控制点的空间位置，获取双臂控制点空间速度；Step 3, establishing the kinematic equation of the dual-arm system, solving the spatial position of the feature point and correspondingly determining the spatial position of the control point, and obtaining the spatial velocity of the dual-arm control point; 步骤4、确定双臂的自避障任务，建立冗余机械臂的关节速度通解方程，并加入动态决策因子和双臂的自避障任务；Step 4. Determine the self-obstacle avoidance task of the two arms, establish the joint velocity general solution equation of the redundant manipulator, and add the dynamic decision factor and the self-obstacle avoidance task of the two arms; 步骤5、判断双臂是否完成目标任务，如未完成，至步骤3。Step 5. Determine whether the arms have completed the target task, if not, go to Step 3. 2.根据权利要求1所述的基于任务分解的双臂冗余机械臂动态协同控制方法，其特征在于，步骤1具体为：给定机械臂末端在笛卡尔空间的目标位置：2. The dual-arm redundant manipulator dynamic cooperative control method based on task decomposition according to claim 1, wherein step 1 is specifically: the target position of the given manipulator end in Cartesian space: [xd，yd，zd][x d , y d , z d ] 其中，xd，yd，zd分别为机械臂基座坐标系下的三维坐标值。Among them, x d , y d , and z d are the three-dimensional coordinate values in the coordinate system of the robot arm base, respectively. 3.根据权利要求2所述的基于任务分解的双臂冗余机械臂动态协同控制方法，其特征在于，步骤2具体为：在双臂的关节与连杆处设定多个特征点，在机械臂的各个关节中心处设置特征点，并在机械臂的前三个连杆的中心处设置特征点。3. The method for dynamic cooperative control of dual-arm redundant manipulators based on task decomposition according to claim 2, wherein step 2 is specifically: setting a plurality of feature points at the joints and connecting rods of the dual arms, Feature points are set at the centers of each joint of the robotic arm, and feature points are set at the centers of the first three links of the robotic arm. 4.根据权利要求3所述的基于任务分解的双臂冗余机械臂动态协同控制方法，其特征在于，确定控制点的空间位置具体如下：4. The dual-arm redundant manipulator dynamic cooperative control method based on task decomposition according to claim 3, is characterized in that, determining the spatial position of the control point is as follows: 以某一侧臂基座坐标系作为参考坐标系，通过正运动学方程获得双臂2n个特征点的位置，用Pri(i＝1，2，...，n)代表该侧臂第i个特征点的位置，n表示特征点数量，用Plj(j＝1，2，...，n)代表另一侧臂臂第j个特征点的位置：Taking a certain side arm base coordinate system as the reference coordinate system, the positions of 2n feature points of the two arms are obtained through the forward kinematics equation, and P ri (i=1, 2,...,n) is used to represent the number of the side arm. The positions of i feature points, n represents the number of feature points, and P lj (j=1, 2,...,n) represents the position of the j-th feature point of the other arm arm: Pri＝fri(qr)＝(xri，yri，zri)P ri =f ri (q r )=(x ri , y ri , z ri ) fri(·)为点Pri处的正运动学公式，qr为该侧臂的关节角度向量，(xri，yri，zri)为Pri的三维空间坐标值；f ri (·) is the positive kinematics formula at the point P ri , q r is the joint angle vector of the side arm, (x ri , y ri , z ri ) is the three-dimensional space coordinate value of P ri ; Pli＝fli(ql)+[0，L，0]＝(xli，yli，zli)P li =f li (q l )+[0, L, 0]=(x li , y li , z li ) fli(·)为点Pli处的正运动学公式，ql为另一侧臂的关节角度向量，(xli，yli，zli)为Pli的坐标值；计算双臂各个特征点之间的距离，用dij代表一侧臂第i个特征点和另一侧臂第j个特征点之间的距离：f li (·) is the positive kinematics formula at point P li , q l is the joint angle vector of the other arm, (x li , y li , z li ) is the coordinate value of P li ; calculate each feature of both arms The distance between points, use d ij to represent the distance between the i-th feature point of one arm and the j-th feature point of the other arm: dij＝||Pri-Plj||d ij =||P ri -P lj || 维护一个n×n的距离为D，并选定双臂特征点距离最短的两个特征点作为双臂的控制点，用Pr代表一侧臂的控制点位置，用Pl代表另一侧臂的控制点位置。Maintain a distance of n × n as D, and select the two feature points with the shortest distance between the feature points of the two arms as the control points of the two arms, use P r to represent the position of the control point of one arm, and use P l to represent the other side. The arm's control point location. 5.根据权利要求4所述的基于任务分解的双臂冗余机械臂动态协同控制方法，其特征在于，步骤3中，通过雅可比矩阵获取双臂控制点空间速度。5 . The method for dynamic cooperative control of dual-arm redundant manipulators based on task decomposition according to claim 4 , wherein, in step 3, the spatial velocity of the dual-arm control points is obtained through the Jacobian matrix. 6 . 6.根据权利要求5所述的基于任务分解的双臂冗余机械臂动态协同控制方法，其特征在于，获取双臂控制点空间速度具体为：6. The dual-arm redundant manipulator dynamic cooperative control method based on task decomposition according to claim 5, is characterized in that, obtaining the space velocity of the dual-arm control point is specifically: 通过双臂的关节速度向量和控制点处的雅可比矩阵，获得控制点处的笛卡尔空间速度，用vr代表一侧臂的控制点速度，用vl代表另一侧臂的控制点速度：Through the joint velocity vector of the two arms and the Jacobian matrix at the control point, the Cartesian space velocity at the control point is obtained, using v r to represent the control point velocity of one arm, and v l to represent the control point velocity of the other arm :
Figure FDA0002973759390000021
Figure FDA0002973759390000021
Figure FDA0002973759390000022
Figure FDA0002973759390000022
 其中，Jr和Jl分别为Pr与Pl的雅可比矩阵，
Figure FDA0002973759390000023
和
Figure FDA0002973759390000024
分别为双臂此时的关节速度向量。Among them, J r and J l are the Jacobian matrices of P r and P l , respectively,
Figure FDA0002973759390000023
and
Figure FDA0002973759390000024
are the joint velocity vectors of both arms at this time, respectively. 7.根据权利要求6所述的基于任务分解的双臂冗余机械臂动态协同控制方法，其特征在于，所述步骤4具体为：7. The method for dynamic cooperative control of dual-arm redundant manipulators based on task decomposition according to claim 6, wherein the step 4 is specifically: 确定两个控制点的相对位置向量Pa：Determine the relative position vector Pa of the two control points: Pa＝Pr-Pl P a =P r -P l 设定回退速度为vb，扩大双臂控制点距离的速度，速度方向与相对位置向量方向相同，速度的大小随着控制点距离变化：Set the retraction speed as v b , and expand the speed of the distance between the control points of the arms. The direction of the speed is the same as the direction of the relative position vector, and the magnitude of the speed changes with the distance of the control points:
Figure FDA0002973759390000025
Figure FDA0002973759390000025
 式中，d表示双臂控制点之间的空间距离，γb控制避障速度随d变化的快慢，σb为避障速度开始产生作用的控制阈值；In the formula, d represents the spatial distance between the control points of the two arms, γ b controls the speed of the obstacle avoidance speed changing with d, and σ b is the control threshold at which the obstacle avoidance speed begins to take effect; 确定两个控制点的相对速度向量va：Determine the relative velocity vector va of the two control points : va＝vl-vr v a =v l -v r 相对位置向量与相对速度向量之间的夹角
Figure FDA0002973759390000026
The angle between the relative position vector and the relative velocity vector
Figure FDA0002973759390000026
Figure FDA0002973759390000027
Figure FDA0002973759390000027
 设定闪避速度vd，当一侧臂持续迫近时，另一侧臂相应的控制点产生向侧面闪避的速度，其方向正交于相对位置向量和相对速度向量，其大小随着控制点距离变化：Set the dodging speed v d . When one arm continues to approach, the corresponding control point of the other arm generates a speed to dodge to the side. Variety:
Figure FDA0002973759390000031
Figure FDA0002973759390000031
 式中，γd控制动态决策因子随d变化的快慢，γd为动态决策因子开始产生作用的控制阈值参数；In the formula, γ d controls how fast the dynamic decision factor changes with d, and γ d is the control threshold parameter that the dynamic decision factor starts to take effect; 将回退速度与闪避速度相加获得躲避速度vh：Add the retreat speed and the dodge speed to get the dodge speed v h : vh＝vb+vd v h = v b +v d 通过控制点的雅可比矩阵将躲避速度映射到关节空间
Figure FDA0002973759390000032
Mapping dodging velocities to joint space via the Jacobian matrix of the control points
Figure FDA0002973759390000032
Figure FDA0002973759390000033
Figure FDA0002973759390000033
 式中
Figure FDA0002973759390000034
为控制点的雅可比矩阵的广义逆矩阵；in the formula
Figure FDA0002973759390000034
is the generalized inverse of the Jacobian matrix of the control points; 对于冗余机械臂，假设其自由度为m，其第i个关节角度为qi(i＝1，2，...，m)，对应关节角度向量为q，完成目标轨迹yd所需关节速度的通解为：For the redundant manipulator, assuming that its degree of freedom is m, its ith joint angle is q i (i=1, 2, ..., m), and the corresponding joint angle vector is q, which is required to complete the target trajectory y d . The general solution for joint velocity is:
Figure FDA0002973759390000035
Figure FDA0002973759390000035
 式中，J为目标轨迹yd对应的雅可比矩阵，J+为J的广义逆矩阵，Im为m阶单位矩阵，k为任意的m维空间矢量，
Figure FDA0002973759390000036
为目标轨迹的微分函数。In the formula, J is the Jacobian matrix corresponding to the target trajectory y d , J + is the generalized inverse matrix of J, I m is the m-order unit matrix, k is any m-dimensional space vector,
Figure FDA0002973759390000036
is the differential function of the target trajectory. 8.根据权利要求7所述的基于任务分解的双臂冗余机械臂动态协同控制方法，其特征在于，所述加入动态决策因子α为：8. The method for dynamic cooperative control of dual-arm redundant manipulators based on task decomposition according to claim 7, wherein the adding dynamic decision factor α is:
Figure FDA0002973759390000037
Figure FDA0002973759390000037
 其中动态决策因子α为：The dynamic decision factor α is:
Figure FDA0002973759390000038
Figure FDA0002973759390000038
 式中，d表示双臂控制点之间的空间距离，γ控制动态决策因子随d变化的快慢，σ为动态决策因子开始产生作用的控制阈值；In the formula, d represents the spatial distance between the control points of the two arms, γ controls the speed of the dynamic decision factor changing with d, and σ is the control threshold at which the dynamic decision factor begins to take effect; 将机械臂实现自避障动作时各轴关节的角速度作为机械臂运动的子运动：The angular velocity of each axis joint when the robot arm realizes self-obstruction avoidance action is taken as the sub-motion of the robot arm movement:
Figure FDA0002973759390000039
Figure FDA0002973759390000039
 9.根据权利要求8所述的基于任务分解的双臂冗余机械臂动态协同控制方法，其特征在于，机械臂的当前位置与目标位置的误差在阈值内，则说明双臂完成目标任务，如果机械臂的当前位置与目标位置的误差超过阈值，则返回步骤3。9. The dual-arm redundant manipulator dynamic collaborative control method based on task decomposition according to claim 8, wherein the error between the current position of the manipulator and the target position is within the threshold value, then it is explained that the two arms complete the target task, If the error between the current position of the robot arm and the target position exceeds the threshold, go back to step 3. 10.根据权利要求9所述的基于任务分解的双臂冗余机械臂动态协同控制方法，其特征在于，所述步骤5具体为：10. The method for dynamic cooperative control of dual-arm redundant manipulators based on task decomposition according to claim 9, wherein the step 5 is specifically: 通过正运动学方程获取机械臂的当前位置与姿态：Obtain the current position and attitude of the robotic arm through the forward kinematics equation: Pe＝fe(q)＝[xe，ye，ze]P e = fe (q)=[x e , y e , z e ] 其中，Pe是机械臂末端的位置，fe(·)为机械臂末端的正运动学公式，q为机械臂当前的关节角度向量，[xe，ye，ze]为Pe的空间坐标值；Among them, P e is the position of the end of the manipulator, f e ( ) is the positive kinematics formula of the end of the manipulator, q is the current joint angle vector of the manipulator, [x e , y e , z e ] is the P e space coordinate value; 给定误差阈值∈，如果机械臂的当前位置与目标位置的误差在阈值内：Given an error threshold ∈, if the error between the current position of the manipulator and the target position is within the threshold:
Figure FDA0002973759390000041
Figure FDA0002973759390000041
 控制结束，如果机械臂的当前位置与目标位置的误差超过阈值：Control ends, if the error between the current position of the robot arm and the target position exceeds the threshold:
Figure FDA0002973759390000042
Figure FDA0002973759390000042
 回至步骤3。Go back to step 3.
CN202110269727.XA 2021-03-12 2021-03-12 A Dynamic Cooperative Control Method of Dual-Arm Redundant Manipulators 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 A Dynamic Cooperative Control Method of Dual-Arm Redundant Manipulators 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 A Dynamic Cooperative Control Method of Dual-Arm Redundant Manipulators 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 A Dynamic Cooperative Control Method of Dual-Arm Redundant Manipulators 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
CN109397271B (en) A 7-DOF anthropomorphic robotic arm and its control method and system
CN111538949A (en) Redundant robot inverse kinematics solution method, device and redundant robot
CN111515949B (en) Double-arm transmission and reception position selection method for double-arm cooperative robot
CN110421547A (en) A kind of tow-armed robot collaboration impedance adjustment based on estimated driving force model
CN106503373A (en) The method for planning track that a kind of Dual-robot coordination based on B-spline curves is assembled
CN105183009B (en) A kind of redundant mechanical arm method for controlling trajectory
Hu et al. Adaptive variable impedance control of dual-arm robots for slabstone installation
CN115625711A (en) A collaborative control method for dual-arm robots considering terminal forces
CN114571469A (en) Zero-space real-time obstacle avoidance control method and system for mechanical arm
CN113334379A (en) Master-slave following and collision avoidance method based on virtual force
CN114589701B (en) Damping least square-based multi-joint mechanical arm obstacle avoidance inverse kinematics method
CN106844951B (en) A method and system for solving the inverse kinematics of a hyper-redundant robot based on piecewise geometry
CN108908347A (en) One kind is towards redundancy mobile mechanical arm error-tolerance type repetitive motion planning method
CN105234930B (en) Control method of motion of redundant mechanical arm based on configuration plane
CN109940615A (en) A final-state network optimization method for synchronous repetitive motion planning of a dual-arm manipulator
CN111791234A (en) Anti-collision control algorithm for working positions of multiple robots in narrow space
Yang et al. Collision avoidance trajectory planning for a dual-robot system: Using a modified APF method
CN113084797A (en) Dynamic cooperative control method for double-arm redundant mechanical arm based on task decomposition
Tang et al. Coordinated motion planning of dual-arm space robot with deep reinforcement learning
CN108638057B (en) Double-arm motion planning method for humanoid robot
CN117415806A (en) Obstacle avoidance track planning method for mechanical arm for loading and unloading drill rod under variable target condition
CN116540721A (en) Optimal trajectory planning method for space robot based on improved genetic particle swarm optimization algorithm
CN114800527A (en) Force application control method and system for tail end of mobile operation mechanical arm
Qian et al. Path planning approach for redundant manipulator based on Jacobian pseudoinverse-RRT algorithm

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