CN112549037B - Non-collision motion control method of double-arm robot and computer storage medium - Google Patents

Non-collision motion control method of double-arm robot and computer storage medium Download PDF

Info

Publication number
CN112549037B
CN112549037B CN202110204960.XA CN202110204960A CN112549037B CN 112549037 B CN112549037 B CN 112549037B CN 202110204960 A CN202110204960 A CN 202110204960A CN 112549037 B CN112549037 B CN 112549037B
Authority
CN
China
Prior art keywords
arm
joint
collision
robot
left arm
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202110204960.XA
Other languages
Chinese (zh)
Other versions
CN112549037A (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.)
Foshan Longshen Robot Co Ltd
Original Assignee
Foshan Longshen Robot Co Ltd
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 Foshan Longshen Robot Co Ltd filed Critical Foshan Longshen Robot Co Ltd
Priority to CN202110204960.XA priority Critical patent/CN112549037B/en
Publication of CN112549037A publication Critical patent/CN112549037A/en
Application granted granted Critical
Publication of CN112549037B publication Critical patent/CN112549037B/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/1615—Programme controls characterised by special kind of manipulator, e.g. planar, scara, gantry, cantilever, space, closed chain, passive/active joints and tendon driven manipulators
    • 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/1602—Programme controls characterised by the control system, structure, architecture
    • B25J9/161—Hardware, e.g. neural networks, fuzzy logic, interfaces, processor
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • Orthopedic Medicine & Surgery (AREA)
  • Health & Medical Sciences (AREA)
  • Artificial Intelligence (AREA)
  • General Health & Medical Sciences (AREA)
  • Evolutionary Computation (AREA)
  • Fuzzy Systems (AREA)
  • Mathematical Physics (AREA)
  • Software Systems (AREA)
  • Numerical Control (AREA)
  • Manipulator (AREA)

Abstract

The invention provides a collision-free motion control method of a double-arm robot and a computer storage medium, wherein the method comprises the following steps: constructing an inequality according to the key points of the left arm and the right arm:
Figure 523327DEST_PATH_IMAGE001
according to the expected track of the left arm end effector
Figure 808815DEST_PATH_IMAGE002
And desired trajectory of right arm end effector
Figure 811406DEST_PATH_IMAGE003
Construction of equations
Figure 600370DEST_PATH_IMAGE004
(ii) a Calculating the angular acceleration of the left arm joint and the angular acceleration of the right arm joint in real time according to the inequality and the equality; updating the angular velocity of the left arm joint and the angular velocity of the right arm joint in real time according to the angular acceleration of the left arm joint and the angular acceleration of the right arm joint respectively; the invention fully considers the joint physical limitation of the double-arm robot while carrying out collision-free motion planning, and can ensure the accessibility of the result of the double-arm robot in the whole collision-free motion planning.

Description

Non-collision motion control method of double-arm robot and computer storage medium
Technical Field
The invention relates to the technical field of robots, in particular to a collision-free motion control method of a double-arm robot and a computer storage medium.
Background
Two-arm robots are typical robotic systems designed to mimic the coordinated operation of human two arms, showing great potential in performing complex tasks. The collision-free motion planning is an important premise for the control of the double-arm robot, but due to the mutual coupling of the working spaces of the left mechanical arm and the right mechanical arm and the highly nonlinear characteristic between the spatial position type and the joint angle of the mechanical arms, the collision-free motion planning of the double-arm robot is very complicated. The current collision-free motion planning method based on the potential field method does not consider the actual physical constraint of the robot, so that the problem that part of motion tracks obtained by planning cannot be reached exists.
Disclosure of Invention
Based on the above, in order to solve the problem that the part of the motion trail obtained by planning is unreachable due to the fact that the actual physical constraint of the robot is not considered in the current potential field method-based collision-free motion planning method, the invention provides a collision-free motion control method of a double-arm robot, which has the following specific technical scheme:
a collision-free motion control method of a double-arm robot comprises the following steps:
constructing an inequality according to the key points of the left arm and the right arm:
Figure DEST_PATH_IMAGE001
；
according to the expected track of the left arm end effector
Figure 773136DEST_PATH_IMAGE002
And a desired trajectory of the right arm end effector
Figure DEST_PATH_IMAGE003
Construction of equations
Figure 126757DEST_PATH_IMAGE004
；
According to the inequality
Figure 659369DEST_PATH_IMAGE005
And equation
Figure 123848DEST_PATH_IMAGE004
Calculating the angular acceleration of the left arm joint and the angular acceleration of the right arm joint in real time;
updating the left arm joint angular velocity and the right arm joint angular velocity in real time according to the left arm joint angular acceleration and the right arm joint angular acceleration respectively, so as to realize collision-free motion control of the double-arm robot;
wherein the content of the first and second substances,
Figure 809039DEST_PATH_IMAGE006
,
Figure DEST_PATH_IMAGE007
,
Figure 333561DEST_PATH_IMAGE008
，
Figure DEST_PATH_IMAGE009
，
Figure 619049DEST_PATH_IMAGE010
，
Figure DEST_PATH_IMAGE011
，
Figure 152798DEST_PATH_IMAGE012
represents the second arm of the left arm
Figure DEST_PATH_IMAGE013
A key point pointing to the second arm
Figure 226583DEST_PATH_IMAGE014
The vector of the individual key points is,
Figure DEST_PATH_IMAGE015
represents the second arm of the left arm
Figure 656428DEST_PATH_IMAGE013
The Jacobian matrix corresponding to each key point,
Figure 163632DEST_PATH_IMAGE016
the second arm represents
Figure 969914DEST_PATH_IMAGE014
The Jacobian matrix corresponding to each key point,
Figure DEST_PATH_IMAGE017
represents the second arm of the left arm
Figure 878965DEST_PATH_IMAGE013
Key point and the second of the right arm
Figure 745289DEST_PATH_IMAGE014
The distance between the individual key points is,
Figure 224943DEST_PATH_IMAGE018
and
Figure DEST_PATH_IMAGE019
are all normal numbers, and are all positive numbers,
Figure 100496DEST_PATH_IMAGE020
a cascade form representing the joint angular velocity of the left arm and the joint angular velocity of the right arm,
Figure DEST_PATH_IMAGE021
represents the joint angular velocity of the left arm,
Figure 864052DEST_PATH_IMAGE022
represents the joint angular velocity of the right arm,
Figure DEST_PATH_IMAGE023
representing the corresponding Jacobian matrix of the left arm end effector,
Figure 166858DEST_PATH_IMAGE024
representing the corresponding Jacobian matrix of the right arm end effector,
Figure DEST_PATH_IMAGE025
and
Figure 399387DEST_PATH_IMAGE026
are all positive control constants, and are,
Figure DEST_PATH_IMAGE027
in order to be the error vector,
Figure DEST_PATH_IMAGE029
representing the velocity of the left arm end effector,
Figure DEST_PATH_IMAGE031
representing the velocity of the right arm end effector,
Figure 344209DEST_PATH_IMAGE032
indicating the position of the left arm end effector,
Figure DEST_PATH_IMAGE033
indicating the position of the right arm end effector,
Figure 493431DEST_PATH_IMAGE034
respectively are the corresponding radiuses of the key points of the left arm,
Figure DEST_PATH_IMAGE035
respectively are the corresponding radiuses of the key points of the right arm,
Figure 717870DEST_PATH_IMAGE036
is the integration time.
The non-collision motion method of the double-arm robot is realized by constructing inequality
Figure DEST_PATH_IMAGE037
And equation
Figure 686963DEST_PATH_IMAGE004
Then according to the inequality
Figure 638738DEST_PATH_IMAGE038
And equation
Figure 111308DEST_PATH_IMAGE004
The method comprises the steps of calculating the angular acceleration of a left arm joint and the angular acceleration of a right arm joint in real time, finally updating the angular velocity of the left arm joint and the angular velocity of the right arm joint in real time according to the angular acceleration of the left arm joint and the angular acceleration of the right arm joint respectively to realize collision-free motion control of the double-arm robot, fully considering joint physical limitations of the double-arm robot while performing collision-free motion planning, ensuring the accessibility of results of the double-arm robot in the whole collision-free motion planning, and solving the problem that part of motion tracks obtained by planning due to the fact that the actual physical constraints of the robot are not considered in the current collision-free motion planning method based on a potential field method is inaccessible.
Further, the angular acceleration of the left arm joint and the angular acceleration of the right arm joint both satisfy the formula
Figure DEST_PATH_IMAGE039
、
Figure 755916DEST_PATH_IMAGE040
And
Figure DEST_PATH_IMAGE041
wherein, the water-soluble polymer is a polymer,
Figure 960108DEST_PATH_IMAGE042
，
Figure DEST_PATH_IMAGE043
，
Figure 715574DEST_PATH_IMAGE044
is a normal number, and is,
Figure DEST_PATH_IMAGE045
and
Figure 573809DEST_PATH_IMAGE046
as an auxiliary variable, the number of variables,
Figure DEST_PATH_IMAGE047
and
Figure 123739DEST_PATH_IMAGE048
are respectively
Figure DEST_PATH_IMAGE049
And
Figure 818157DEST_PATH_IMAGE046
the derivative of (a) of (b),
Figure 111735DEST_PATH_IMAGE050
in the case of an angular acceleration,
Figure DEST_PATH_IMAGE051
and
Figure 824476DEST_PATH_IMAGE052
respectively representing the minimum value and the maximum value of the joint angle of the two-arm robot.
Further, the joint angle
Figure DEST_PATH_IMAGE053
Satisfy inequality
Figure 810886DEST_PATH_IMAGE054
。
Further, the real-time updating formula of the angular velocity of the left arm joint and the angular velocity of the right arm joint is
Figure DEST_PATH_IMAGE055
，
Figure 727021DEST_PATH_IMAGE056
Which is indicative of a control time interval,
Figure DEST_PATH_IMAGE057
indicating the current time of day.
Further, the collision-free motion control method of the double-arm robot further comprises the following steps: through a maleFormula (II)
Figure 824290DEST_PATH_IMAGE058
For auxiliary variable
Figure 391537DEST_PATH_IMAGE049
And
Figure 283270DEST_PATH_IMAGE046
and performing real-time updating.
Further, an inequality is constructed according to the key points of the left arm and the key points of the right arm
Figure DEST_PATH_IMAGE059
And acquiring the key point of the left arm and the key point of the right arm.
Further, the desired trajectory of the end effector is determined according to the left arm
Figure 935968DEST_PATH_IMAGE002
And a desired trajectory of the right arm end effector
Figure 571349DEST_PATH_IMAGE060
Construction of equations
Figure DEST_PATH_IMAGE061
Previously, the desired trajectory of the left arm end effector was obtained
Figure 9415DEST_PATH_IMAGE062
And a desired trajectory of the right arm end effector
Figure DEST_PATH_IMAGE063
。
Further, an inequality is constructed according to the key points of the left arm and the key points of the right arm
Figure 337628DEST_PATH_IMAGE037
Firstly, describing the left arm and the right arm of the double-arm robot respectively in a mode of combining key points and radiusesCurrent space occupation of the arm.
Further, the collision-free motion control method of the two-arm robot further comprises the step of calculating the current spatial position and the attitude of the two-arm robot according to the key points of the left arm and the key points of the right arm.
Accordingly, the present invention also provides a computer storage medium storing a computer program which, when executed by a processor, implements the collision-free motion control method of the two-arm robot as described above.
Drawings
The invention will be further understood from the following description in conjunction with the accompanying drawings. The components in the figures are not necessarily to scale, emphasis instead being placed upon illustrating the principles of the embodiments. Like reference numerals designate corresponding parts throughout the different views.
Fig. 1 is a schematic overall flow chart of a collision-free motion control method for a two-arm robot according to an embodiment of the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention more apparent, the present invention is further described in detail below with reference to embodiments thereof. It should be understood that the detailed description and specific examples, while indicating the scope of the invention, are intended for purposes of illustration only and are not intended to limit the scope of the invention.
It will be understood that when an element is referred to as being "secured to" another element, it can be directly on the other element or intervening elements may also be present. When an element is referred to as being "connected" to another element, it can be directly connected to the other element or intervening elements may also be present. The terms "vertical," "horizontal," "left," "right," and the like as used herein are for illustrative purposes only and do not represent the only embodiments.
Unless defined otherwise, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention belongs. The terminology used herein in the description of the invention is for the purpose of describing particular embodiments only and is not intended to be limiting of the invention. As used herein, the term "and/or" includes any and all combinations of one or more of the associated listed items.
The terms "first" and "second" used herein do not denote any particular order or quantity, but rather are used to distinguish one element from another.
As shown in fig. 1, a collision-free motion control method for a two-arm robot in an embodiment of the present invention includes the following steps:
constructing an inequality according to the key points of the left arm and the right arm:
Figure 212043DEST_PATH_IMAGE064
；
according to the expected track of the left arm end effector
Figure 385535DEST_PATH_IMAGE002
And a desired trajectory of the right arm end effector
Figure DEST_PATH_IMAGE065
Construction of equations
Figure 958335DEST_PATH_IMAGE066
；
According to the inequality
Figure DEST_PATH_IMAGE067
And equation
Figure 457450DEST_PATH_IMAGE004
Calculating the angular acceleration of the left arm joint and the angular acceleration of the right arm joint in real time;
updating the left arm joint angular velocity and the right arm joint angular velocity in real time according to the left arm joint angular acceleration and the right arm joint angular acceleration respectively, so as to realize collision-free motion control of the double-arm robot;
wherein the content of the first and second substances,
Figure 819161DEST_PATH_IMAGE068
,
Figure DEST_PATH_IMAGE069
,
Figure 61923DEST_PATH_IMAGE008
，
Figure 192690DEST_PATH_IMAGE009
，
Figure 597127DEST_PATH_IMAGE010
，
Figure 462446DEST_PATH_IMAGE011
，
Figure 243320DEST_PATH_IMAGE012
represents the second arm of the left arm
Figure 228594DEST_PATH_IMAGE013
A key point pointing to the second arm
Figure 803931DEST_PATH_IMAGE014
The vector of the individual key points is,
Figure 140235DEST_PATH_IMAGE015
represents the second arm of the left arm
Figure 459221DEST_PATH_IMAGE013
The Jacobian matrix corresponding to each key point,
Figure 564580DEST_PATH_IMAGE016
the second arm represents
Figure 61551DEST_PATH_IMAGE014
The Jacobian matrix corresponding to each key point,
Figure 885151DEST_PATH_IMAGE017
represents the second arm of the left arm
Figure 742249DEST_PATH_IMAGE013
Key point and the second of the right arm
Figure 702114DEST_PATH_IMAGE014
The distance between the individual key points is,
Figure 884834DEST_PATH_IMAGE018
and
Figure 195730DEST_PATH_IMAGE019
are all normal numbers, and are all positive numbers,
Figure 856518DEST_PATH_IMAGE020
a cascade form representing the joint angular velocity of the left arm and the joint angular velocity of the right arm,
Figure 670890DEST_PATH_IMAGE021
represents the joint angular velocity of the left arm,
Figure 509664DEST_PATH_IMAGE022
represents the joint angular velocity of the right arm,
Figure 42277DEST_PATH_IMAGE023
representing the corresponding Jacobian matrix of the left arm end effector,
Figure 506756DEST_PATH_IMAGE024
representing the corresponding Jacobian matrix of the right arm end effector,
Figure 441214DEST_PATH_IMAGE025
and
Figure 700157DEST_PATH_IMAGE026
are all positive control constants, and are,
Figure 720066DEST_PATH_IMAGE027
in order to be the error vector,
Figure 722657DEST_PATH_IMAGE029
representing the velocity of the left arm end effector,
Figure 511621DEST_PATH_IMAGE031
representing the velocity of the right arm end effector,
Figure 220427DEST_PATH_IMAGE032
indicating the position of the left arm end effector,
Figure 727632DEST_PATH_IMAGE033
indicating the position of the right arm end effector,
Figure 2755DEST_PATH_IMAGE034
respectively are the corresponding radiuses of the key points of the left arm,
Figure 911805DEST_PATH_IMAGE035
respectively are the corresponding radiuses of the key points of the right arm,
Figure 512551DEST_PATH_IMAGE036
is the integration time.
The non-collision motion method of the double-arm robot is realized by constructing inequality
Figure 507052DEST_PATH_IMAGE001
And equation
Figure 851445DEST_PATH_IMAGE004
Then according to the inequality
Figure 615002DEST_PATH_IMAGE070
And equation
Figure 668540DEST_PATH_IMAGE004
Calculating the angular acceleration of the left arm joint and the angular acceleration of the right arm joint in real time, and finally carrying out real implementation on the angular velocity of the left arm joint and the angular velocity of the right arm joint according to the angular acceleration of the left arm joint and the angular acceleration of the right arm joint respectivelyThe method is updated in time to realize collision-free motion control of the double-arm robot, joint physical limitation of the double-arm robot is fully considered while collision-free motion planning is carried out, accessibility of results of the double-arm robot in the whole collision-free motion planning can be guaranteed, and the problem that part of motion tracks obtained by planning due to the fact that actual physical constraint of the robot is not considered in the existing collision-free motion planning method based on a potential field method is inaccessible is solved.
In addition, by the inequality
Figure 884758DEST_PATH_IMAGE071
The collision of the left arm and the right arm of the two-arm robot can be avoided, and the equation is passed
Figure 32842DEST_PATH_IMAGE004
The dual-arm robot can complete a given task according to a desired track. That is to say, the collision-free motion control method of the double-arm robot can complete a given task according to a desired track while avoiding obstacles.
In one embodiment, an inequality is constructed from the left arm keypoints and the right arm keypoints
Figure 650905DEST_PATH_IMAGE037
In the prior art, the current space occupation conditions of the left arm and the right arm of the double-arm robot are described in a mode of combining key points and radii.
The set of the left arm key points and the radii is defined as:
Figure DEST_PATH_IMAGE072
. Wherein
Figure 859033DEST_PATH_IMAGE073
…
Figure DEST_PATH_IMAGE074
Is a key point of the left arm, and the left arm,
Figure 828126DEST_PATH_IMAGE013
the number of key points of the left arm.
The set of the right arm key points and the radii is defined as:
Figure 265054DEST_PATH_IMAGE075
. Wherein
Figure 3203DEST_PATH_IMAGE076
……
Figure 382232DEST_PATH_IMAGE077
Is a key point of the right arm,
Figure 573042DEST_PATH_IMAGE014
the number of key points of the right arm.
The key points refer to points fixed on the left and right mechanical arm bodies, such as the elbow joint center of the mechanical arm, the mass center of the small arm rod piece, the wrist joint center and the like. For any given keypoint, the position of that point in the working space can be calculated from a kinematic model of the mechanical arm, such as the ith keypoint of the left arm
Figure 62929DEST_PATH_IMAGE078
The positions of (a) can be described as:
Figure 655585DEST_PATH_IMAGE079
wherein
Figure 205515DEST_PATH_IMAGE080
Comprises a left arm key point
Figure 634353DEST_PATH_IMAGE081
With respect to the positional information of the robot arm,
Figure 927931DEST_PATH_IMAGE082
is the current joint angle of the left arm. Similarly, the j-th key point of the right arm
Figure 375093DEST_PATH_IMAGE083
Can be described as
Figure 830345DEST_PATH_IMAGE084
，
Figure 995747DEST_PATH_IMAGE085
Is the current joint angle of the right arm,
Figure 827437DEST_PATH_IMAGE086
and
Figure 394684DEST_PATH_IMAGE087
is similar to the definition of the key point, is a function describing the corresponding position information of the jth key point of the right arm, and comprises the key point of the right arm
Figure 20838DEST_PATH_IMAGE088
Position information relative to the robotic arm.
In one embodiment, the left arm joint angular acceleration and the right arm joint angular acceleration both satisfy the formula
Figure 427198DEST_PATH_IMAGE039
、
Figure 62579DEST_PATH_IMAGE040
And
Figure 218754DEST_PATH_IMAGE041
wherein, the water-soluble polymer is a polymer,
Figure 281388DEST_PATH_IMAGE089
，
Figure 421382DEST_PATH_IMAGE090
，
Figure 860454DEST_PATH_IMAGE044
is a normal number, and is,
Figure 136714DEST_PATH_IMAGE045
and
Figure 120982DEST_PATH_IMAGE046
as an auxiliary variable, the number of variables,
Figure 482693DEST_PATH_IMAGE047
and
Figure 459876DEST_PATH_IMAGE091
are respectively
Figure 856223DEST_PATH_IMAGE049
And
Figure 260659DEST_PATH_IMAGE046
the derivative of (a) of (b),
Figure 109666DEST_PATH_IMAGE050
in the case of an angular acceleration,
Figure 624961DEST_PATH_IMAGE092
and
Figure 626547DEST_PATH_IMAGE052
respectively representing the minimum value and the maximum value of the joint angle of the two-arm robot.
In one embodiment, the joint angle
Figure 201884DEST_PATH_IMAGE053
Satisfy inequality
Figure 538188DEST_PATH_IMAGE093
. Therefore, when the double-arm robot is subjected to collision-free motion planning, the situation that the left arm joint angle and/or the right arm joint angle exceed the limit can be avoided.
In one embodiment, the collision-free motion control method of the two-arm robot further comprises calculating the current spatial position and attitude of the two-arm robot according to the left arm key point and the right arm key point.
In one embodiment, the real-time update formula of the left arm joint angular velocity and the right arm joint angular velocity is
Figure 857174DEST_PATH_IMAGE094
，
Figure 962533DEST_PATH_IMAGE095
Which is indicative of a control time interval,
Figure 708772DEST_PATH_IMAGE096
indicating the current time of day.
In one embodiment, the collision-free motion control method of the two-arm robot further comprises the following steps: by the formula
Figure 266792DEST_PATH_IMAGE058
For auxiliary variable
Figure 389469DEST_PATH_IMAGE049
And
Figure 100067DEST_PATH_IMAGE046
and performing real-time updating.
By aiming at the angular velocity of the left arm joint, the angular velocity of the right arm joint and the auxiliary variable
Figure 17208DEST_PATH_IMAGE097
And
Figure 328103DEST_PATH_IMAGE046
and the real-time updating is carried out, so that the motion track of the double-arm robot in each collision-free motion planning period can be calculated in real time, and the offline planning or the human body adjustment of the collision-free motion track is not needed.
In one embodiment, an inequality is constructed from the left arm keypoints and the right arm keypoints
Figure 988892DEST_PATH_IMAGE098
And acquiring the key point of the left arm and the key point of the right arm.
In one embodiment, the desired trajectory is determined based on the left arm end effector
Figure 803264DEST_PATH_IMAGE002
And a desired trajectory of the right arm end effector
Figure 156885DEST_PATH_IMAGE060
Construction of equations
Figure 689498DEST_PATH_IMAGE066
Previously, the desired trajectory of the left arm end effector was obtained
Figure 153977DEST_PATH_IMAGE062
And a desired trajectory of the right arm end effector
Figure 836238DEST_PATH_IMAGE063
。
In one embodiment, the present invention also provides a computer storage medium storing a computer program which, when executed by a processor, implements the collision-free motion control method of the two-arm robot as described above.
The technical features of the embodiments described above may be arbitrarily combined, and for the sake of brevity, all possible combinations of the technical features in the embodiments described above are not described, but should be considered as being within the scope of the present specification as long as there is no contradiction between the combinations of the technical features.
The above-mentioned embodiments only express several embodiments of the present invention, and the description thereof is more specific and detailed, but not construed as limiting the scope of the invention. It should be noted that, for a person skilled in the art, several variations and modifications can be made without departing from the inventive concept, which falls within the scope of the present invention. Therefore, the protection scope of the present patent shall be subject to the appended claims.

Claims (10)

1. A collision-free motion control method of a double-arm robot is characterized by comprising the following steps:
constructing an inequality according to the key points of the left arm and the right arm:
Figure 172449DEST_PATH_IMAGE001
；
according to the expected track of the left arm end effector
Figure 767378DEST_PATH_IMAGE002
And a desired trajectory of the right arm end effector
Figure 966279DEST_PATH_IMAGE003
Construction of equations
Figure 369578DEST_PATH_IMAGE004
；
According to the inequality
Figure 628521DEST_PATH_IMAGE005
And equation
Figure 382850DEST_PATH_IMAGE004
Calculating the angular acceleration of the left arm joint and the angular acceleration of the right arm joint in real time;
updating the left arm joint angular velocity and the right arm joint angular velocity in real time according to the left arm joint angular acceleration and the right arm joint angular acceleration respectively, so as to realize collision-free motion control of the double-arm robot;
wherein the content of the first and second substances,
Figure 119862DEST_PATH_IMAGE006
,
Figure 706831DEST_PATH_IMAGE007
,
Figure 136675DEST_PATH_IMAGE008
，
Figure 112722DEST_PATH_IMAGE009
，
Figure 653424DEST_PATH_IMAGE010
，
Figure 296895DEST_PATH_IMAGE011
，
Figure 694378DEST_PATH_IMAGE012
represents the second arm of the left arm
Figure 423300DEST_PATH_IMAGE013
A key point pointing to the second arm
Figure 502115DEST_PATH_IMAGE014
The vector of the individual key points is,
Figure 92DEST_PATH_IMAGE015
represents the second arm of the left arm
Figure 506160DEST_PATH_IMAGE016
The Jacobian matrix corresponding to each key point,
Figure 722377DEST_PATH_IMAGE017
the second arm represents
Figure 683511DEST_PATH_IMAGE018
The Jacobian matrix corresponding to each key point,
Figure 35995DEST_PATH_IMAGE019
represents the second arm of the left arm
Figure 978543DEST_PATH_IMAGE016
Key point and the second of the right arm
Figure 416478DEST_PATH_IMAGE018
The distance between the individual key points is,
Figure 633833DEST_PATH_IMAGE020
and
Figure 106402DEST_PATH_IMAGE021
are all normal numbers, and are all positive numbers,
Figure 954273DEST_PATH_IMAGE022
a cascade form representing the joint angular velocity of the left arm and the joint angular velocity of the right arm,
Figure 879503DEST_PATH_IMAGE023
represents the joint angular velocity of the left arm,
Figure 103811DEST_PATH_IMAGE024
represents the joint angular velocity of the right arm,
Figure 430887DEST_PATH_IMAGE025
representing a Jacobian matrix corresponding to the left arm end effector,
Figure 292402DEST_PATH_IMAGE026
representing a Jacobian matrix corresponding to the right arm end effector,
Figure 704929DEST_PATH_IMAGE027
and
Figure 467348DEST_PATH_IMAGE028
are all positively controlledThe constant number is a constant number,
Figure 648931DEST_PATH_IMAGE029
in order to be the error vector,
Figure 104183DEST_PATH_IMAGE030
representing the velocity of the left arm end effector,
Figure 800744DEST_PATH_IMAGE031
representing the velocity of the right arm end effector,
Figure 366854DEST_PATH_IMAGE032
indicating the position of the left arm end effector,
Figure 402943DEST_PATH_IMAGE033
indicating the position of the right arm end effector,
Figure 29097DEST_PATH_IMAGE034
is the radius corresponding to the key point of the left arm,
Figure 416216DEST_PATH_IMAGE035
is the radius corresponding to the key point of the right arm,
Figure 333487DEST_PATH_IMAGE036
is the integration time.
2. The collision-free motion control method of a dual-arm robot as claimed in claim 1, wherein the angular acceleration of the left arm joint and the angular acceleration of the right arm joint both satisfy the formula
Figure 489662DEST_PATH_IMAGE037
、
Figure 286717DEST_PATH_IMAGE038
And
Figure 895553DEST_PATH_IMAGE039
wherein, the water-soluble polymer is a polymer,
Figure 69045DEST_PATH_IMAGE040
，
Figure 79726DEST_PATH_IMAGE041
，
Figure 375579DEST_PATH_IMAGE042
is a normal number, and is,
Figure 471711DEST_PATH_IMAGE043
and
Figure 917735DEST_PATH_IMAGE044
as an auxiliary variable, the number of variables,
Figure 48502DEST_PATH_IMAGE045
and
Figure 187360DEST_PATH_IMAGE046
are respectively
Figure 816793DEST_PATH_IMAGE047
And
Figure 66509DEST_PATH_IMAGE048
the derivative of (a) of (b),
Figure 51782DEST_PATH_IMAGE049
in the case of an angular acceleration,
Figure 361541DEST_PATH_IMAGE050
and
Figure 432265DEST_PATH_IMAGE051
respectively representing the minimum value and the maximum value of the joint angle of the two-arm robot.
3. The collision-free motion control method of a two-arm robot as claimed in claim 2, wherein the joint angle is set to be equal to or greater than a predetermined value
Figure 485672DEST_PATH_IMAGE052
Satisfy inequality
Figure 653348DEST_PATH_IMAGE053
。
4. The collision-free motion control method of the dual-arm robot as claimed in claim 3, wherein the real-time update formula of the angular velocity of the left arm joint and the angular velocity of the right arm joint is
Figure 134008DEST_PATH_IMAGE054
，
Figure 426449DEST_PATH_IMAGE055
Which is indicative of a control time interval,
Figure 283546DEST_PATH_IMAGE056
indicating the current time of day.
5. The collision-free motion control method of the two-arm robot as claimed in claim 4, wherein the collision-free motion control method of the two-arm robot further comprises the steps of: by the formula
Figure 977833DEST_PATH_IMAGE057
For auxiliary variable
Figure 442443DEST_PATH_IMAGE058
And
Figure 487760DEST_PATH_IMAGE059
and performing real-time updating.
6. The collision-free motion control method of the dual-arm robot as claimed in claim 1, wherein an inequality is constructed based on the key points of the left arm and the key points of the right arm
Figure 882969DEST_PATH_IMAGE060
And acquiring the key point of the left arm and the key point of the right arm.
7. The collision-free motion control method of a dual-arm robot as claimed in claim 1, wherein the trajectory is determined according to the desired trajectory of the left-arm end effector
Figure 431762DEST_PATH_IMAGE002
And a desired trajectory of the right arm end effector
Figure 519804DEST_PATH_IMAGE061
Construction of equations
Figure 583575DEST_PATH_IMAGE004
Previously, the desired trajectory of the left arm end effector was obtained
Figure 516896DEST_PATH_IMAGE002
And a desired trajectory of the right arm end effector
Figure 185775DEST_PATH_IMAGE003
。
8. The collision-free motion control method of a dual-arm robot as claimed in claim 7, wherein an inequality is constructed based on the key points of the left arm and the key points of the right arm
Figure 444718DEST_PATH_IMAGE062
In the prior art, the current space occupation conditions of the left arm and the right arm of the double-arm robot are described in a mode of combining key points and radii.
9. The collision-free motion control method of the two-arm robot as claimed in claim 8, further comprising calculating the current spatial position and attitude of the two-arm robot based on the left arm key point and the right arm key point.
10. A computer storage medium characterized in that it stores a computer program which, when executed by a processor, implements the collision-free motion control method of a dual-arm robot according to any one of claims 1 to 9.
CN202110204960.XA 2021-02-24 2021-02-24 Non-collision motion control method of double-arm robot and computer storage medium Active CN112549037B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110204960.XA CN112549037B (en) 2021-02-24 2021-02-24 Non-collision motion control method of double-arm robot and computer storage medium

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110204960.XA CN112549037B (en) 2021-02-24 2021-02-24 Non-collision motion control method of double-arm robot and computer storage medium

Publications (2)

Publication Number Publication Date
CN112549037A CN112549037A (en) 2021-03-26
CN112549037B true CN112549037B (en) 2021-06-01

Family

ID=75034652

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110204960.XA Active CN112549037B (en) 2021-02-24 2021-02-24 Non-collision motion control method of double-arm robot and computer storage medium

Country Status (1)

Country Link
CN (1) CN112549037B (en)

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106695797A (en) * 2017-02-22 2017-05-24 哈尔滨工业大学深圳研究生院 Compliance control method and system based on collaborative operation of double-arm robot
CN106945020A (en) * 2017-05-18 2017-07-14 哈尔滨工业大学 A kind of space double mechanical arms system motion control method for coordinating
WO2018091103A1 (en) * 2016-11-18 2018-05-24 Abb Schweiz Ag A robot arm system and a method for handling an object by a robot arm system during lead through programming
CN108638058A (en) * 2018-04-23 2018-10-12 华南理工大学 A kind of posture decision dynamic programming method
CN108638057A (en) * 2018-04-23 2018-10-12 华南理工大学 A kind of humanoid robot both arms motion planning method
CN109333537A (en) * 2018-10-29 2019-02-15 华中科技大学 A kind of the basis coordinates system scaling method and system of both arms cooperation robot system
CN111360824A (en) * 2020-02-27 2020-07-03 中科新松有限公司 Double-arm self-collision detection method and computer-readable storage medium
DE102019102798A1 (en) * 2019-02-05 2020-08-06 Franka Emika Gmbh Combine two individual robot manipulators into one robot system by calibration
CN111629867A (en) * 2017-12-12 2020-09-04 皮尔茨公司 Collision-free motion planning in closed motion systems

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2018091103A1 (en) * 2016-11-18 2018-05-24 Abb Schweiz Ag A robot arm system and a method for handling an object by a robot arm system during lead through programming
CN106695797A (en) * 2017-02-22 2017-05-24 哈尔滨工业大学深圳研究生院 Compliance control method and system based on collaborative operation of double-arm robot
CN106945020A (en) * 2017-05-18 2017-07-14 哈尔滨工业大学 A kind of space double mechanical arms system motion control method for coordinating
CN111629867A (en) * 2017-12-12 2020-09-04 皮尔茨公司 Collision-free motion planning in closed motion systems
CN108638058A (en) * 2018-04-23 2018-10-12 华南理工大学 A kind of posture decision dynamic programming method
CN108638057A (en) * 2018-04-23 2018-10-12 华南理工大学 A kind of humanoid robot both arms motion planning method
CN109333537A (en) * 2018-10-29 2019-02-15 华中科技大学 A kind of the basis coordinates system scaling method and system of both arms cooperation robot system
DE102019102798A1 (en) * 2019-02-05 2020-08-06 Franka Emika Gmbh Combine two individual robot manipulators into one robot system by calibration
CN111360824A (en) * 2020-02-27 2020-07-03 中科新松有限公司 Double-arm self-collision detection method and computer-readable storage medium

Also Published As

Publication number Publication date
CN112549037A (en) 2021-03-26

Similar Documents

Publication Publication Date Title
CN111538949A (en) Redundant robot inverse kinematics solving method and device and redundant robot
CN107490965B (en) Multi-constraint trajectory planning method for space free floating mechanical arm
WO2018107851A1 (en) Method and device for controlling redundant robot arm
CN110275436B (en) RBF neural network self-adaptive control method of multi-single-arm manipulator
AU2021103765A4 (en) Robotic arm motion programming method based on fixed-parameter neural network
Miller et al. Motion capture from inertial sensing for untethered humanoid teleoperation
Xu et al. Motion planning of manipulators for simultaneous obstacle avoidance and target tracking: An RNN approach with guaranteed performance
CN110682286B (en) Real-time obstacle avoidance method for cooperative robot
Tang et al. Swarm robots search for multiple targets based on an improved grouping strategy
Cao et al. ESO-based robust and high-precision tracking control for aerial manipulation
Mbakop et al. Path planning and control of mobile soft manipulators with obstacle avoidance
Bryson et al. Co-operative localisation and mapping for multiple UAVs in unknown environments
Dexu et al. Adaptive robust control based on RBF neural networks for duct cleaning robot
CN114932549A (en) Motion planning method and device of spatial redundant mechanical arm
Zhang et al. Adaptive learning-based distributed control of cooperative robot arm manipulation for unknown objects
Jing et al. A recursive dynamic modeling and control for dual-arm manipulator with elastic joints
CN112549037B (en) Non-collision motion control method of double-arm robot and computer storage medium
Chen et al. Optimizing the obstacle avoidance trajectory and positioning error of robotic manipulators using multigroup ant colony and quantum behaved particle swarm optimization algorithms
Huang An adaptive impedance control scheme for constrained robots
Casalino et al. Human pose estimation in presence of occlusion using depth camera sensors, in human-robot coexistence scenarios
Yamaguchi A cooperative hunting behavior by multiple nonholonomic mobile robots
Cheng et al. Robust tracking control of a unicycle-type wheeled mobile manipulator using a hybrid sliding mode fuzzy neural network
Kim et al. Three-link planar arm control using reinforcement learning
Chaparro-Altamirano et al. Kinematic and workspace analysis of a parallel robot used in security applications
Teka et al. Modeling and simulation of cooperative transport of an object by two mobile manipulators on an uneven terrain using KSOM network

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