CN113843794B - Method and device for planning coordinated movement of two mechanical arms, electronic equipment and storage medium - Google Patents
Method and device for planning coordinated movement of two mechanical arms, electronic equipment and storage medium Download PDFInfo
- Publication number
- CN113843794B CN113843794B CN202111122347.XA CN202111122347A CN113843794B CN 113843794 B CN113843794 B CN 113843794B CN 202111122347 A CN202111122347 A CN 202111122347A CN 113843794 B CN113843794 B CN 113843794B
- Authority
- CN
- China
- Prior art keywords
- arm
- point
- planning
- task
- expected
- 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
Links
- 230000033001 locomotion Effects 0.000 title claims abstract description 87
- 238000000034 method Methods 0.000 title claims abstract description 47
- 230000007704 transition Effects 0.000 claims description 18
- 238000004590 computer program Methods 0.000 description 8
- 230000006870 function Effects 0.000 description 6
- 230000003287 optical effect Effects 0.000 description 6
- 238000010586 diagram Methods 0.000 description 5
- 230000008569 process Effects 0.000 description 5
- 238000012545 processing Methods 0.000 description 5
- 238000004891 communication Methods 0.000 description 3
- 230000004888 barrier function Effects 0.000 description 2
- 239000000463 material Substances 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 239000013307 optical fiber Substances 0.000 description 2
- 230000000644 propagated effect Effects 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- 230000004075 alteration Effects 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000005474 detonation Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 239000004973 liquid crystal related substance Substances 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 230000006641 stabilisation Effects 0.000 description 1
- 238000011105 stabilization Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000013022 venting Methods 0.000 description 1
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
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F30/00—Computer-aided design [CAD]
- G06F30/20—Design optimisation, verification or simulation
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F2111/00—Details relating to CAD techniques
- G06F2111/04—Constraint-based CAD
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Computer Hardware Design (AREA)
- Evolutionary Computation (AREA)
- Geometry (AREA)
- General Engineering & Computer Science (AREA)
- General Physics & Mathematics (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
- Numerical Control (AREA)
Abstract
The invention provides a planning method and a planning device for coordinated movement of two mechanical arms, electronic equipment and a storage medium, wherein the planning method comprises the following steps: setting working spaces of the two mechanical arms, the main arm and the auxiliary arm, a cooperation space of the two mechanical arms, a loose and tight task constraint space of the main arm and a loose and tight task constraint space of the auxiliary arm, and randomly selecting a current expected cooperation task point of the two mechanical arms in the cooperation space; when the planning of the current wheel starts, if the main arm and the auxiliary arm both move to the current expected cooperative task point of the two mechanical arms, the planning is successful; and if at least one point which does not move to the current two-mechanical-arm expected cooperative task point exists, the planning of the current round is considered to be failed, the auxiliary arm and the main arm are sequentially returned to respective initial positions, one point is randomly selected in the neighborhood space of the current two-mechanical-arm expected cooperative task point to serve as the current two-mechanical-arm expected cooperative task point, and the next round of planning is carried out. The coordinated motion planning method in the master-slave mode is adopted, and the planning efficiency of the operation tasks of the two mechanical arms is improved.
Description
Technical Field
The disclosure belongs to the field of double-mechanical-arm operation tasks, and particularly relates to a double-mechanical-arm coordinated motion planning method and device, electronic equipment and a storage medium.
Background
The double mechanical arms have larger working space and more flexible operation capability than the single mechanical arm, so that the double mechanical arms are widely concerned and researched. However, during the operation task of the two mechanical arms, the conditions of joint motion limitation, singular position, collision avoidance, task constraint and the like need to be met. However, under the above condition, how to coordinate the motions of the two robots to a reasonable cooperative task point and how to plan the motions according to the expected trajectory as much as possible still has great difficulty. Therefore, it is necessary to develop an effective method for planning the coordinated movement of the two mechanical arms, so as to achieve smart and precise operation of complex operation tasks.
Disclosure of Invention
The present disclosure is directed to solving one of the problems set forth above.
Therefore, the method for planning coordinated movement of two robots, which is provided by the embodiment of the first aspect of the present disclosure and is capable of improving planning efficiency of two robot operation tasks based on a master-slave mode, includes:
setting working spaces of the two mechanical arms, a main arm, an auxiliary arm and a cooperation space of the two mechanical arms, wherein the working space when the mechanical arm performs motion planning along an expected task track is defined as a tight task constraint space, so that the tight task constraint space of the main arm and the tight task constraint space of the auxiliary arm are set; the method comprises the following steps of setting a loose task constraint space of a main arm and a loose task constraint space of an auxiliary arm by using a working space of the mechanical arm when the mechanical arm performs motion planning in an allowable error range around a neighborhood space of an expected task track, an actual position and an expected position of the mechanical arm; setting the initial position of the main arm and the initial position of the auxiliary arm, and randomly selecting a task point in the double-mechanical-arm cooperative space as a current expected cooperative task point of the double mechanical arms;
at the starting moment of planning of the current wheel, the main arm and the auxiliary arm move according to a main arm movement planning strategy and an auxiliary arm movement planning strategy based on a tightness task constraint space respectively; if the main arm and the auxiliary arm move to the current expected cooperative task point of the two mechanical arms, planning is successful; and if at least one of the main arm and the auxiliary arm does not move to the current two-mechanical-arm expected cooperative task point, considering that the current round of planning fails, sequentially returning the auxiliary arm and the main arm to respective initial positions, randomly selecting a point in a neighborhood space of the current two-mechanical-arm expected cooperative task point as the current two-mechanical-arm expected cooperative task point, and performing next round of planning until the failure frequency reaches a set maximum frequency, and considering that the planning fails if at least one of the main arm and the auxiliary arm does not move to the current two-mechanical-arm expected cooperative task point.
The method for planning the coordinated movement of the two mechanical arms provided by the embodiment of the first aspect of the disclosure has the following characteristics and beneficial effects:
the method adopts a coordinated motion planning method in a master-slave mode, so that the planning efficiency of the operation tasks of the two mechanical arms is improved; based on a movement planning mode of a tight task constraint space, the mechanical arm can carry out movement planning along an expected track to the maximum extent in the tight task constraint space, and when the conditions of joint movement limitation, singular position, collision avoidance, task constraint and the like occur, the movement planning can be carried out in the tight task constraint space and the movement planning can be carried out in the tight task constraint space as soon as possible.
In some embodiments, the working space of the two mechanical arms is set to be W, and the working space of the main arm is set to be W 1 The working space of the auxiliary arm is W 2 The cooperation space of the two mechanical arms is W co Satisfies the following conditions: w 1 ∈W,W 2 ∈W,W co =W 1 ∩W 2 (ii) a Setting the expected task trajectories of the main arm and the auxiliary arm to be P respectively 1 And P 2 Setting tight task constraint spaces of the main arm and the auxiliary arm to be respectivelyAndsatisfies the following conditions:andthe loose task constraint spaces of the main arm and the auxiliary arm are respectively set asAndsatisfies the following conditions:andsetting the initial positions of the main arm and the auxiliary arm to be respectivelyAndsatisfies the following conditions:and
in some embodiments, the expected task tracks of the main arm and the auxiliary arm are respectively divided into a plurality of segments, the end points of each segment form corresponding expected track points, and the last expected track point of the main arm and the auxiliary arm is the expected cooperative task point of the current two mechanical arms; the main arm movement planning strategy and the auxiliary arm movement planning strategy are respectively used for sequentially planning each expected track point on each expected task track as follows:
solving the inverse kinematics of the mechanical arm from the current expected track point to the next expected track point, if the inverse kinematics has the solution, enabling the mechanical arm to move from the current expected track point to the next expected track point on a tight task constraint space of the mechanical arm, and then planning the next expected track point; if no solution exists, a plurality of adjacent points are randomly selected in a neighborhood space of the current expected track point, the adjacent point with the minimum cost from the current expected track point to each adjacent point is obtained as an optimal adjacent point, the mechanical arm moves to the optimal adjacent point from the current expected track point on a loose task constraint space of the mechanical arm, the current expected track point is updated to be the optimal adjacent point, if the distance from the current expected track point to the next expected track point is smaller than a set distance threshold value, next expected track point planning is carried out, if the distance from the current expected track point to the next expected track point is larger than or equal to the set distance threshold value, a point is selected from the path from the current expected track point to the next expected track point to be a transition point, the mechanical arm moves to the transition point from the current expected track point on the loose task constraint space of the mechanical arm, the current expected track point is updated to be the transition point, and then next expected track point planning is carried out.
In some embodiments, the current expected trace points are found to be in accordance withMinimum cost J of the neighboring points * :
Wherein,the mechanical arm joint space is at the s-th adjacent point P at the T moment n_s And M is the total number of neighboring points randomly selected in the neighborhood space of the current expected locus point.
In some embodiments, the transition point is calculated as:
P mid =P k +a*(P k+1 -P k )/||P k+1 -P k ||
wherein, P mid Is the transition point; a is a constant, a is an element (0, 1); p k And P k+1 Respectively the current expected track point and the next expected track point.
The two arms coordinated movement planning apparatus provided in the embodiment of the second aspect of the present disclosure includes:
the system comprises a parameter setting module, a task planning module and a task planning module, wherein the parameter setting module is used for setting working spaces of two mechanical arms, a working space of a main arm, a working space of an auxiliary arm and a cooperation space of the two mechanical arms, wherein the working space when the mechanical arms perform motion planning along expected task tracks of the mechanical arms is defined as a tight task constraint space, and the tight task constraint space of the main arm and the tight task constraint space of the auxiliary arm are set according to the tight task constraint space; the method comprises the steps that a neighborhood space of a mechanical arm around an expected task track of the mechanical arm, and an operation space when an actual position and an expected position are subjected to motion planning within an allowable error range are called as a loose task constraint space, so that a loose task constraint space of a main arm and a loose task constraint space of an auxiliary arm are set; setting an initial position of a main arm and an initial position of an auxiliary arm, and randomly selecting a task point in the two mechanical arm cooperation space as a current expected cooperation task point of the two mechanical arms; and
the planning module is used for planning the coordinated motion of the two mechanical arms; for each round of planning, enabling the main arm and the auxiliary arm to respectively move according to a main arm movement planning strategy and an auxiliary arm movement planning strategy based on a tightness task constraint space; if the main arm and the auxiliary arm both move to the current expected cooperative task point of the two mechanical arms, planning is successful; and if at least one of the main arm and the auxiliary arm does not move to the current two-mechanical-arm expected cooperative task point, considering that the current round of planning fails, sequentially returning the auxiliary arm and the main arm to respective initial positions, randomly selecting a point in a neighborhood space of the current two-mechanical-arm expected cooperative task point as the current two-mechanical-arm expected cooperative task point, and performing next round of planning until the failure frequency reaches a set maximum frequency, and considering that the planning fails if at least one of the main arm and the auxiliary arm does not move to the current two-mechanical-arm expected cooperative task point.
An embodiment of a third aspect of the present disclosure provides an electronic device, including:
at least one processor, and a memory communicatively coupled to the at least one processor;
wherein the memory stores instructions executable by the at least one processor, the instructions configured to perform the above-described two-robot coordinated motion planning method.
A fourth aspect of the present disclosure provides a computer-readable storage medium, which stores computer instructions, where the computer instructions are used to cause the computer to execute the above two-robot coordinated motion planning method.
Drawings
Fig. 1 is a schematic diagram of an operation task of a planning method for coordinated movement of two robots according to an embodiment of the first aspect of the present disclosure.
Fig. 2 is a flowchart of a two-robot coordinated movement planning method according to an embodiment of the disclosure.
Fig. 3 is a flowchart of a mechanical arm motion planning strategy based on a tight task constraint space according to a first embodiment of the disclosure.
Fig. 4 is a schematic structural diagram of a two-robot coordinated movement planning apparatus according to an embodiment of the second aspect of the present disclosure.
Fig. 5 is a schematic structural diagram of an electronic device according to an embodiment of the third aspect of the present disclosure.
Detailed Description
In order to make the objects, technical solutions and advantages of the present application more apparent, the present application is described in further detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the present application and are not intended to limit the present application.
On the contrary, this application is intended to cover any alternatives, modifications, equivalents, and alternatives that may be included within the spirit and scope of the application as defined by the appended claims. Furthermore, in the following detailed description of the present application, certain specific details are set forth in order to provide a better understanding of the present application. It will be apparent to one skilled in the art that the present application may be practiced without these specific details.
Referring to fig. 1 schematically, an operation task scene of the planning method for coordinated motion of two mechanical arms provided in an embodiment of the first aspect of the present disclosure is schematically illustrated, where one mechanical arm of the two mechanical arms is used as a main arm, and the other mechanical arm is used as an auxiliary arm, and the planning method for coordinated motion of two mechanical arms provided in an embodiment of the first aspect of the present disclosure includes:
setting working spaces of the two mechanical arms, a main arm, an auxiliary arm and a cooperation space of the two mechanical arms, wherein the working space when the mechanical arm performs motion planning along an expected task track is defined as a tight task constraint space, so that the tight task constraint space of the main arm and the tight task constraint space of the auxiliary arm are set; the method comprises the steps that a neighborhood space of a mechanical arm around an expected task track of the mechanical arm, and an operation space when an actual position and an expected position are subjected to motion planning within an allowable error range are called as a loose task constraint space, so that a loose task constraint space of a main arm and a loose task constraint space of an auxiliary arm are set; setting the initial position of a main arm and the initial position of an auxiliary arm, and randomly selecting a task point in a double-mechanical-arm cooperation space as a current expected cooperation task point of the double mechanical arms;
at the starting moment of planning of the current wheel, the main arm and the auxiliary arm move according to a main arm movement planning strategy and an auxiliary arm movement planning strategy based on a tightness task constraint space respectively; if the main arm and the auxiliary arm both move to the current expected cooperative task point of the two mechanical arms, the planning is successful; and if at least one of the main arm and the auxiliary arm does not move to the current expected cooperative task point of the two mechanical arms, considering that the planning of the current round is failed, sequentially returning the auxiliary arm and the main arm to respective initial positions, randomly selecting a point in the neighborhood space of the current expected cooperative task point of the two mechanical arms as the expected cooperative task point of the current mechanical arms, and planning the next round until the failure times reach the set maximum times, and considering that the planning is failed if at least one of the main arm and the auxiliary arm still does not move to the current expected cooperative task point of the two mechanical arms.
According to the method provided by the embodiment of the first aspect of the disclosure, the cooperative task point is planned based on the coordinated motion of the two mechanical arms, wherein the main arm moves and is planned to the cooperative task point at first, and after the target is reached, the auxiliary arm moves and is planned to the cooperative task point again. The master-slave mode coordinated motion planning method can effectively avoid repeated calculation amount in the process of the two mechanical arms coordinated motion planning. When the main arm moves to plan, the auxiliary arm can be used as a static barrier, and similarly, when the auxiliary arm moves to plan, the main arm can be used as a static barrier, so that the movement planning efficiency is improved; the mechanical arm is preferentially subjected to motion planning in a tight task constraint space according to an expected track, the optimal path is ensured, when the mechanical arm cannot be subjected to motion planning in the tight task constraint space due to obstacles, singular positions and the like, the mechanical arm is subjected to motion planning in a loose task constraint space, the two mechanical arm operation tasks are ensured to continue to work, and at the moment, although the mechanical arm does not move in the expected track, the mechanical arm can be returned to the expected track again to perform motion planning when the tight task constraint space condition is met.
In some embodiments, referring to fig. 2, a method for planning coordinated movement of two robots according to an embodiment of the first aspect of the present disclosure specifically includes the following steps:
1) One of the two mechanical arms is used as the mechanical armMain arm M 1 The other mechanical arm is used as an auxiliary arm M 2 (ii) a Setting the working space of the two mechanical arms as W and the main arm M 1 Has a working space of W 1 Auxiliary arm M 2 Has a working space of W 2 The cooperation space of the two mechanical arms is W co And satisfies the following conditions: w 1 ∈W,W 2 ∈W,W co =W 1 ∩W 2 (ii) a Provided with a main arm M 1 Is P 1 Auxiliary arm M 2 Is P 2 The expected cooperative task point of the two mechanical arms is P co (ii) a Defining the operation space when the mechanical arm performs motion planning along the expected task track as a tight task constraint space W h Setting a main arm M 1 A tight task constraint space ofSatisfies the following conditions:auxiliary arm M 2 A tight task constraint space ofSatisfies the following conditions:defining a working space when the mechanical arm moves and plans around a neighborhood space of an expected task track, an actual position and an expected position within an allowable error range as a loose task constraint space W s Setting a main arm M 1 A loose task constraint space ofSatisfies the following conditions:auxiliary arm M 2 The loose task constraint space ofSatisfies the following conditions:provided with a main arm M 1 Is in an initial position ofSatisfies the following conditions: namely thatIs positioned to remove the two mechanical arm cooperation space W co Main arm workspace W of 1 In the space of (2), a sub-arm M is arranged 2 Is in an initial position ofSatisfies the following conditions:namely thatIs positioned to remove the two mechanical arms cooperation space W co In the auxiliary arm work space W 2 In the space of (a); setting the planning failure times of the expected cooperative task points as fail _ count, and initializing the fail _ count =0; setting the maximum number of times of the planning failure of the expected cooperative task point as N _ fail; setting the radius of the neighborhood space as epsilon; in the double mechanical arm cooperation space W co Randomly selecting one task point as an expected cooperative task point P of the current two mechanical arms co 。
2) Judging whether the fail _ count is smaller than the N _ fail, if the fail _ count is larger than or equal to the N _ fail, judging that the coordinated movement planning of the two mechanical arms fails and the operation task cannot be completed; if fail _ count is less than N _ fail, then step 3) is performed.
3) Main arm M 1 And the auxiliary arm M 2 Respectively according to a main arm motion planning strategy and an auxiliary arm motion planning strategy based on a tightness task constraint spaceMotion if main arm M 1 And the auxiliary arm M 2 In which there is at least one desired cooperative task point P that does not move to the current two robots co If yes, executing step 4); if the main arm M 1 And the auxiliary arm M 2 All move to the expected cooperative task point P of the current two mechanical arms co If so, the planning is successful, and the method is ended.
4) The auxiliary arm M2 is returned to the initial state of the auxiliary armThen make the main arm M 1 Returning to the initial state of the main armAt the current expected cooperative task point P of the two mechanical arms co Randomly selecting a point in the neighborhood space as a near-neighbor point P new And order P co =P new I.e. the neighboring point P new As a new two-robot expected cooperative task point P co 。
5) Let fail _ count = fail _ count +1, return to step 2).
In some embodiments, the expected task tracks of the main arm and the auxiliary arm are respectively divided into a plurality of segments, the end points of each segment form corresponding expected track points, and the last expected track point of the main arm and the auxiliary arm is the current expected cooperative task point of the two mechanical arms; the main arm motion planning strategy and the auxiliary arm motion planning strategy based on the elastic task constraint space are respectively used for sequentially planning the expected track points on the expected task tracks as follows:
solving the inverse kinematics of the mechanical arm from the current expected track point to the next expected track point, if the solution exists, enabling the mechanical arm to move from the current expected track point to the next expected track point on a tight task constraint space of the mechanical arm, and then planning the next expected track point; if no solution exists, a plurality of adjacent points are randomly selected in a neighborhood space of the current expected track point, the adjacent point with the minimum cost from the current expected track point to each adjacent point is obtained as the optimal adjacent point, the mechanical arm moves from the current expected track point to the optimal adjacent point on a loose task constraint space of the mechanical arm, the current expected track point is updated to be the optimal adjacent point, if the distance from the current expected track point to the next expected track point is smaller than a set distance threshold value, the next expected track point is planned, if the distance from the current expected track point to the next expected track point is larger than or equal to the set distance threshold value, a point is selected from the path from the current expected track point to the next expected track point to be a transition point, the mechanical arm moves from the current expected track point to the transition point on the loose task constraint space of the mechanical arm, the current expected track point is updated to be the transition point, and then the next expected track point is planned.
In some embodiments, referring to fig. 3, the mechanical arm motion planning strategy based on the tight task constraint space respectively includes:
31 Equally dividing the expected task track of the mechanical arm into N +1 segments, and forming a corresponding expected track point P by the end point of each segment 0 ,P 1 ,…,P k ,…,P N Thereby forming a set of desired trajectories { P } 0 ,P 1 ,…,P k ,…,P N },P N =P co K =0,1,2, \8230, N; the selection number of the adjacent points is M =3, the radius of the adjacent space of the current expected locus point is eta =0.02, the lower limit of the distance between the two positions is gamma =0.01, and the proportionality coefficient a =0.5. Initializing the current mechanical arm to be located at the expected track point P 0 Where k =0,n =10.
32 Solving from the expected trace point P) k To P k+1 If the solution exists, the mechanical arm is in a tight task constraint space W h Up from the current expected track point P k Move to the next desired track point P k+1 And let k = k +1, further judge if k = N, the output plan is successful, otherwise, go to step 32); if not, go to step 33).
33 If k = N-1, the output planning fails, and the mechanical arm motion planning ends; otherwise, go to step 34).
34 At the current desired track point P) k Randomly selecting M neighbor points P in the neighborhood space n_1 ~P n_M Calculating the current expected track point P k To a neighboring point P n_1 ~P n_M The near-neighbor point with the minimum intermediate cost is taken as the optimal near-neighbor point P *nearnest Mechanical arm in loose task constraint space W s Up from the current expected track point P k Move to the optimal neighbor point P *nearnest And let k = k +1, update the current position P k Is p *nearnest (ii) a Judgment of P k To P k+1 If it is less than γ, then go back to step 32); otherwise, step 35) is executed;
35 From the current expected track point P) k To the next desired track point P k+1 Selecting a middle point as a transition point on the path, and arranging the mechanical arm in a loose task constraint space W s Up from the current expected track point P k Move to the transition point P mid Update the current position P k Is P mid Returning to step 32).
In some embodiments, the current desired track point P is calculated as k To a neighboring point P n_1 ~P n_M Middle minimum cost J * :
Wherein,for the mechanical arm joint space at the time of T at the adjacent point P n_s The angular position vector is, similarly, T-1 for the previous time and T +1 for the next time.
In some embodiments, in order to make the planned trajectory of the mechanical arm satisfy the smooth stabilization, the transition point P is calculated according to the following formula mid :
P mid =P k +a*(P k+1 -P k )/||P k+1 -P k ||
Wherein a is a set constant, a is belonged to (0, 1), and when a approaches to 0, a transition point P is selected mid Closer to the current desired track point P k When a gets overWhen approaching 1, the selected transition point P mid Closer to the next desired track point P k+1 。
Referring to fig. 4, a two-robot coordinated motion planning apparatus provided in an embodiment of the second aspect of the present disclosure includes:
the system comprises a parameter setting module, a task planning module and a task planning module, wherein the parameter setting module is used for setting working spaces of two mechanical arms, a working space of a main arm, a working space of an auxiliary arm and a cooperation space of the two mechanical arms, wherein the working space when the mechanical arms perform motion planning along expected task tracks of the mechanical arms is defined as a tight task constraint space, and the tight task constraint space of the main arm and the tight task constraint space of the auxiliary arm are set according to the tight task constraint space; the method comprises the following steps of setting a loose task constraint space of a main arm and a loose task constraint space of an auxiliary arm by using a working space of the mechanical arm when the mechanical arm performs motion planning in an allowable error range around a neighborhood space of an expected task track, an actual position and an expected position of the mechanical arm; setting the initial position of a main arm and the initial position of an auxiliary arm, and randomly selecting a task point in a double-mechanical-arm cooperation space as a current expected cooperation task point of the double mechanical arms; and
the planning module is used for planning the coordinated motion of the two mechanical arms; for each round of planning, the main arm and the auxiliary arm move according to a main arm movement planning strategy and an auxiliary arm movement planning strategy based on a tightness task constraint space respectively; if the main arm and the auxiliary arm both move to the current expected cooperative task point of the two mechanical arms, the planning is successful; and if at least one of the main arm and the auxiliary arm does not move to the current expected cooperative task point of the double mechanical arms, considering that the planning of the current round is failed, sequentially returning the auxiliary arm and the main arm to the respective initial positions, randomly selecting a point in the neighborhood space of the current expected cooperative task point of the double mechanical arms as the expected cooperative task point of the current double mechanical arms, and performing the next round of planning until the failure times reach the set maximum times, and considering that the planning is failed if at least one of the main arm and the auxiliary arm still does not move to the current expected cooperative task point of the double mechanical arms.
In order to implement the foregoing embodiment, the present disclosure further provides a computer-readable storage medium, on which a computer program is stored, where the computer program is executed by a processor, and is used to execute the dual-robot arm detonation eliminating operation control method of the foregoing embodiment.
Referring now to FIG. 5, a block diagram of an electronic device 100 suitable for use in implementing embodiments of the present disclosure is presented. It should be noted that the electronic device 100 includes a dual-robot explosion-venting operation control system, wherein the electronic device in the embodiment of the present disclosure may include, but is not limited to, a mobile terminal such as a mobile phone, a notebook computer, a digital broadcast receiver, a PDA (personal digital assistant), a PAD (tablet computer), a PMP (portable multimedia player), a vehicle terminal (e.g., a vehicle navigation terminal), and the like, and a fixed terminal such as a digital TV, a desktop computer, a server, and the like. The electronic device shown in fig. 5 is only an example, and should not bring any limitation to the functions and the scope of use of the embodiments of the present disclosure.
As shown in fig. 5, the electronic device 100 may include a processing means (e.g., a central processing unit, a graphic processor, etc.) 101 that may perform various appropriate actions and processes according to a program stored in a Read Only Memory (ROM) 102 or a program loaded from a storage means 108 into a Random Access Memory (RAM) 103. In the RAM 103, various programs and data necessary for the operation of the electronic apparatus 100 are also stored. The processing device 101, the ROM102, and the RAM 103 are connected to each other through a bus 104. An input/output (I/O) interface 105 is also connected to bus 104.
Generally, the following devices may be connected to the I/O interface 105: input devices 106 including, for example, a touch screen, touch pad, keyboard, mouse, camera, microphone, etc.; an output device 107 including, for example, a Liquid Crystal Display (LCD), a speaker, a vibrator, and the like; storage devices 108 including, for example, magnetic tape, hard disk, etc.; and a communication device 109. The communication means 109 may allow the electronic device 100 to communicate wirelessly or by wire with other devices to exchange data. While fig. 5 illustrates an electronic device 100 having various means, it is to be understood that not all illustrated means are required to be implemented or provided. More or fewer devices may alternatively be implemented or provided.
In particular, the processes described above with reference to the flow diagrams may be implemented as computer software programs, according to embodiments of the present disclosure. For example, the present embodiments include a computer program product comprising a computer program embodied on a computer readable medium, the computer program comprising program code for performing the method illustrated in the flow chart. In such an embodiment, the computer program may be downloaded and installed from a network through the communication means 109, or installed from the storage means 108, or installed from the ROM 102. The computer program, when executed by the processing device 101, performs the above-described functions defined in the methods of the embodiments of the present disclosure.
It should be noted that the computer readable medium in the present disclosure can be a computer readable signal medium or a computer readable storage medium or any combination of the two. A computer readable storage medium may be, for example, but not limited to, an electronic, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any combination of the foregoing. More specific examples of the computer readable storage medium may include, but are not limited to: an electrical connection having one or more wires, a portable computer diskette, a hard disk, a Random Access Memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disc read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the foregoing. In the present disclosure, a computer readable storage medium may be any tangible medium that can contain, or store a program for use by or in connection with an instruction execution system, apparatus, or device. In contrast, in the present disclosure, a computer readable signal medium may include a propagated data signal with computer readable program code embodied therein, for example, in baseband or as part of a carrier wave. Such a propagated data signal may take many forms, including, but not limited to, electro-magnetic, optical, or any suitable combination thereof. A computer readable signal medium may also be any computer readable medium that is not a computer readable storage medium and that can communicate, propagate, or transport a program for use by or in connection with an instruction execution system, apparatus, or device. Program code embodied on a computer readable medium may be transmitted using any appropriate medium, including but not limited to: electrical wires, optical cables, RF (radio frequency), etc., or any suitable combination of the foregoing.
The computer readable medium may be embodied in the electronic device; or may exist separately without being assembled into the electronic device.
The computer readable medium carries one or more programs which, when executed by the electronic device, cause the electronic device to:
setting working spaces of the two mechanical arms, a main arm, an auxiliary arm and a cooperation space of the two mechanical arms, wherein the working space when the mechanical arm performs motion planning along an expected task track is defined as a tight task constraint space, so that the tight task constraint space of the main arm and the tight task constraint space of the auxiliary arm are set; the method comprises the steps that a neighborhood space of a mechanical arm around an expected task track of the mechanical arm, and an operation space when an actual position and an expected position are subjected to motion planning within an allowable error range are called as a loose task constraint space, so that a loose task constraint space of a main arm and a loose task constraint space of an auxiliary arm are set; setting the initial position of the main arm and the initial position of the auxiliary arm, and randomly selecting a task point in the double-mechanical-arm cooperative space as a current expected cooperative task point of the double mechanical arms;
at the starting moment of planning of the current wheel, the main arm and the auxiliary arm move according to a main arm movement planning strategy and an auxiliary arm movement planning strategy respectively; if the main arm and the auxiliary arm move to the current expected cooperative task point of the two mechanical arms, planning is successful; and if at least one of the main arm and the auxiliary arm does not move to the current two-mechanical-arm expected cooperative task point, considering that the planning of the current round is failed, sequentially returning the auxiliary arm and the main arm to respective initial positions, randomly selecting a point in a neighborhood space of the current two-mechanical-arm expected cooperative task point as the current two-mechanical-arm expected cooperative task point, and performing next round planning until the failure times reach a set maximum number, and considering that the planning is failed if at least one of the main arm and the auxiliary arm does not move to the current two-mechanical-arm expected cooperative task point.
Computer program code for carrying out operations for aspects of the present disclosure may be written in any combination of one or more programming languages, including an object oriented programming language such as Java, smalltalk, C + +, python, and conventional procedural programming languages, such as the "C" programming language or similar programming languages. The program code may execute entirely on the user's computer, partly on the user's computer, as a stand-alone software package, partly on the user's computer and partly on a remote computer or entirely on the remote computer or server. In the case of a remote computer, the remote computer may be connected to the user's computer through any type of network, including a Local Area Network (LAN) or a Wide Area Network (WAN), or the connection may be made to an external computer (for example, through the Internet using an Internet service provider).
In the description herein, reference to the description of the term "one embodiment," "some embodiments," "an example," "a specific example," or "some examples," etc., means that a particular feature, structure, material, or characteristic described in connection with the embodiment or example is included in at least one embodiment or example of the application. In this specification, the schematic representations of the terms used above are not necessarily intended to refer to the same embodiment or example. Furthermore, the particular features, structures, materials, or characteristics described may be combined in any suitable manner in any one or more embodiments or examples. Furthermore, various embodiments or examples and features of different embodiments or examples described in this specification can be combined and combined by one skilled in the art without contradiction.
Furthermore, the terms "first", "second" and "first" are used for descriptive purposes only and are not to be construed as indicating or implying relative importance or to implicitly indicate the number of technical features indicated. Thus, a feature defined as "first" or "second" may explicitly or implicitly include at least one such feature. In the description of the present application, "plurality" means at least two, e.g., two, three, etc., unless explicitly specified otherwise.
Any process or method descriptions in flow charts or otherwise described herein may be understood as representing modules, segments, or portions of code which include one or more executable instructions for implementing specific logical functions or steps in the process, and alternate implementations are included within the scope of the preferred embodiment of the present application in which functions may be executed out of order from that shown or discussed, including substantially concurrently or in reverse order, depending on the functionality involved, as would be understood by those reasonably skilled in the art of the present application.
The logic and/or steps represented in the flowcharts or otherwise described herein, such as an ordered listing of executable instructions that can be considered to implement logical functions, can be embodied in any computer-readable medium for use by or in connection with an instruction execution system, apparatus, or device, such as a computer-based system, processor-containing system, or other system that can fetch the instructions from the instruction execution system, apparatus, or device and execute the instructions. For the purposes of this description, a "computer-readable medium" can be any means that can contain, store, communicate, propagate, or transport the program for use by or in connection with the instruction execution system, apparatus, or device. More specific examples (a non-exhaustive list) of the computer-readable medium would include the following: an electrical connection (electronic device) having one or more wires, a portable computer diskette (magnetic device), a Random Access Memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber device, and a portable compact disc read-only memory (CDROM). Additionally, the computer-readable medium could even be paper or another suitable medium upon which the program is printed, as the program can be electronically captured, via for instance optical scanning of the paper or other medium, then compiled, interpreted or otherwise processed in a suitable manner if necessary, and then stored in a computer memory.
It should be understood that portions of the present application may be implemented in hardware, software, firmware, or a combination thereof. In the above embodiments, the various steps or methods may be implemented in software or firmware stored in memory and executed by a suitable instruction execution system. For example, if implemented in hardware, as in another embodiment, any one or combination of the following technologies, which are well known in the art, may be used: a discrete logic circuit having a logic gate circuit for implementing a logic function on a data signal, an application specific integrated circuit having an appropriate combinational logic gate circuit, a Programmable Gate Array (PGA), a Field Programmable Gate Array (FPGA), or the like.
It will be understood by those skilled in the art that all or part of the steps carried by the method for implementing the above embodiments may be implemented by a program instructing associated hardware to complete, and the developed program may be stored in a computer-readable storage medium, and when executed, the program includes one or a combination of the steps of the method embodiments.
In addition, functional units in the embodiments of the present application may be integrated into one processing module, or each unit may exist alone physically, or two or more units are integrated into one module. The integrated module can be realized in a hardware mode, and can also be realized in a software functional module mode. The integrated module, if implemented in the form of a software functional module and sold or used as a separate product, may also be stored in a computer readable storage medium.
The storage medium mentioned above may be a read-only memory, a magnetic or optical disk, etc. Although embodiments of the present application have been shown and described above, it is understood that the above embodiments are exemplary and should not be construed as limiting the present application, and that variations, modifications, substitutions and alterations may be made to the above embodiments by those of ordinary skill in the art within the scope of the present application.
Claims (8)
1. A planning method for coordinated movement of two mechanical arms is characterized by comprising the following steps:
setting working spaces of the two mechanical arms, a main arm, an auxiliary arm and a cooperation space of the two mechanical arms, wherein the working space when the mechanical arm performs motion planning along an expected task track is defined as a tight task constraint space, so that the tight task constraint space of the main arm and the tight task constraint space of the auxiliary arm are set; the method comprises the following steps of setting a loose task constraint space of a main arm and a loose task constraint space of an auxiliary arm by using a working space of the mechanical arm when the mechanical arm performs motion planning in an allowable error range around a neighborhood space of an expected task track, an actual position and an expected position of the mechanical arm; setting an initial position of the main arm and an initial position of the auxiliary arm, and randomly selecting a task point in the two mechanical arm cooperation space as a current expected cooperation task point of the two mechanical arms;
at the starting moment of planning of the current wheel, the main arm and the auxiliary arm move according to a main arm movement planning strategy and an auxiliary arm movement planning strategy based on a tightness task constraint space respectively; if the main arm and the auxiliary arm both move to the current expected cooperative task point of the two mechanical arms, planning is successful; and if at least one of the main arm and the auxiliary arm does not move to the current two-mechanical-arm expected cooperative task point, considering that the current round of planning fails, sequentially returning the auxiliary arm and the main arm to respective initial positions, randomly selecting a point in a neighborhood space of the current two-mechanical-arm expected cooperative task point as the current two-mechanical-arm expected cooperative task point, and performing next round of planning until the failure frequency reaches a set maximum frequency, and considering that the planning fails if at least one of the main arm and the auxiliary arm does not move to the current two-mechanical-arm expected cooperative task point.
2. The coordinated movement planning method for two robot arms according to claim 1, wherein the working space of the two robot arms is W, and the working space of the main arm is W 1 The working space of the auxiliary arm is W 2 The cooperation space of the two mechanical arms is W co And satisfies the following conditions: w 1 ∈W,W 2 ∈W,W co =W 1 ∩W 2 (ii) a Setting the expected task trajectories of the main arm and the auxiliary arm to be P respectively 1 And P 2 Setting tight task constraint spaces of the main arm and the auxiliary arm to be respectivelyAndsatisfies the following conditions:andthe loose task constraint spaces of the main arm and the auxiliary arm are respectively set asAndsatisfies the following conditions:andsetting the initial positions of the main arm and the auxiliary arm to be respectivelyAndsatisfies the following conditions:and
3. the coordinated movement planning method for two mechanical arms according to claim 2, wherein the expected task tracks of the main arm and the auxiliary arm are respectively divided into a plurality of segments, the end points of each segment form corresponding expected track points, and the last expected track point of the main arm and the auxiliary arm is the current expected cooperative task point of the two mechanical arms; the main arm movement planning strategy and the auxiliary arm movement planning strategy are respectively used for sequentially planning each expected track point on each expected task track as follows:
solving the inverse kinematics of the mechanical arm from the current expected track point to the next expected track point, if the inverse kinematics has the solution, enabling the mechanical arm to move from the current expected track point to the next expected track point on a tight task constraint space of the mechanical arm, and then planning the next expected track point; if no solution exists, a plurality of adjacent points are randomly selected in a neighborhood space of the current expected track point, the adjacent point with the minimum cost from the current expected track point to each adjacent point is obtained as an optimal adjacent point, the mechanical arm moves to the optimal adjacent point from the current expected track point on a loose task constraint space of the mechanical arm, the current expected track point is updated to be the optimal adjacent point, if the distance from the current expected track point to the next expected track point is smaller than a set distance threshold value, next expected track point planning is carried out, if the distance from the current expected track point to the next expected track point is larger than or equal to the set distance threshold value, a point is selected from the path from the current expected track point to the next expected track point to be a transition point, the mechanical arm moves to the transition point from the current expected track point on the loose task constraint space of the mechanical arm, the current expected track point is updated to be the transition point, and then next expected track point planning is carried out.
4. The dual-robot arm of claim 3The coordinated motion planning method is characterized in that the minimum cost J from the current expected track point to each adjacent point is obtained according to the following formula * :
5. The method for planning the coordinated movement of two robots according to claim 3, wherein the transition point is calculated according to the following formula:
P mid =P k +a*(P k+1 -P k )/‖P k+1 -P k ‖
wherein, P mid Is the transition point; a is a constant, a belongs to (0, 1); p k And P k+1 Respectively the current expected track point and the next expected track point.
6. A two arms coordinate motion planning device which characterized in that includes:
the system comprises a parameter setting module, a task planning module and a task planning module, wherein the parameter setting module is used for setting working spaces of two mechanical arms, a working space of a main arm, a working space of an auxiliary arm and a cooperation space of the two mechanical arms, wherein the working space when the mechanical arms perform motion planning along expected task tracks of the mechanical arms is defined as a tight task constraint space, and the tight task constraint space of the main arm and the tight task constraint space of the auxiliary arm are set according to the tight task constraint space; the method comprises the steps that a neighborhood space of a mechanical arm around an expected task track of the mechanical arm, and an operation space when an actual position and an expected position are subjected to motion planning within an allowable error range are called as a loose task constraint space, so that a loose task constraint space of a main arm and a loose task constraint space of an auxiliary arm are set; setting an initial position of a main arm and an initial position of an auxiliary arm, and randomly selecting a task point in the two mechanical arm cooperation space as a current expected cooperation task point of the two mechanical arms; and
the planning module is used for planning the coordinated motion of the two mechanical arms; for each round of planning, enabling the main arm and the auxiliary arm to respectively move according to a main arm movement planning strategy and an auxiliary arm movement planning strategy based on a tightness task constraint space; if the main arm and the auxiliary arm both move to the current expected cooperative task point of the two mechanical arms, planning is successful; and if at least one of the main arm and the auxiliary arm does not move to the current two-mechanical-arm expected cooperative task point, considering that the current round of planning fails, sequentially returning the auxiliary arm and the main arm to respective initial positions, randomly selecting a point in a neighborhood space of the current two-mechanical-arm expected cooperative task point as the current two-mechanical-arm expected cooperative task point, and performing next round of planning until the failure frequency reaches a set maximum frequency, and considering that the planning fails if at least one of the main arm and the auxiliary arm does not move to the current two-mechanical-arm expected cooperative task point.
7. An electronic device, comprising:
at least one processor, and a memory communicatively coupled to the at least one processor;
wherein the memory stores instructions executable by the at least one processor, the instructions being configured to perform the dual-robot coordinated movement planning method of any of the preceding claims 1-5.
8. A computer-readable storage medium storing computer instructions for causing a computer to perform the method for planning the coordinated movement of two robots according to any one of claims 1 to 5.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111122347.XA CN113843794B (en) | 2021-09-24 | 2021-09-24 | Method and device for planning coordinated movement of two mechanical arms, electronic equipment and storage medium |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111122347.XA CN113843794B (en) | 2021-09-24 | 2021-09-24 | Method and device for planning coordinated movement of two mechanical arms, electronic equipment and storage medium |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113843794A CN113843794A (en) | 2021-12-28 |
CN113843794B true CN113843794B (en) | 2022-12-09 |
Family
ID=78979251
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111122347.XA Active CN113843794B (en) | 2021-09-24 | 2021-09-24 | Method and device for planning coordinated movement of two mechanical arms, electronic equipment and storage medium |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113843794B (en) |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106426164A (en) * | 2016-09-27 | 2017-02-22 | 华南理工大学 | Redundancy dual-mechanical-arm multi-index coordinate exercise planning method |
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 |
CN109822554A (en) * | 2019-03-20 | 2019-05-31 | 华中科技大学 | Towards underwater both arms collaboration crawl, embraces and take and collision prevention integral method and system |
CN109940615A (en) * | 2019-03-13 | 2019-06-28 | 浙江科技学院 | A kind of final state network optimized approach towards the synchronous repeating motion planning of dual-arm robot |
CN110900605A (en) * | 2019-12-02 | 2020-03-24 | 浙江大学 | Multi-constraint machining optimization method for coordinated mechanical arm based on speed reconfiguration |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
DE102018202321A1 (en) * | 2018-02-15 | 2019-08-22 | Robert Bosch Gmbh | Coordination system, handling equipment and procedures |
-
2021
- 2021-09-24 CN CN202111122347.XA patent/CN113843794B/en active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106426164A (en) * | 2016-09-27 | 2017-02-22 | 华南理工大学 | Redundancy dual-mechanical-arm multi-index coordinate exercise planning method |
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 |
CN109940615A (en) * | 2019-03-13 | 2019-06-28 | 浙江科技学院 | A kind of final state network optimized approach towards the synchronous repeating motion planning of dual-arm robot |
CN109822554A (en) * | 2019-03-20 | 2019-05-31 | 华中科技大学 | Towards underwater both arms collaboration crawl, embraces and take and collision prevention integral method and system |
CN110900605A (en) * | 2019-12-02 | 2020-03-24 | 浙江大学 | Multi-constraint machining optimization method for coordinated mechanical arm based on speed reconfiguration |
Also Published As
Publication number | Publication date |
---|---|
CN113843794A (en) | 2021-12-28 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN107980109B (en) | Robot motion trajectory planning method and related device | |
CN106964156B (en) | Path finding method and device | |
JP2019119041A (en) | Robot motion path planning method, apparatus, storage medium, and terminal device | |
WO2020135608A1 (en) | Industrial robot demonstration track recurrence method and system and robot | |
US9639813B2 (en) | Method and device for three-weight message-passing optimization scheme | |
CN109648560A (en) | Space tracking transition method, system and the robot of industrial robot | |
CN113050643B (en) | Unmanned vehicle path planning method, unmanned vehicle path planning device, electronic equipment and computer readable medium | |
CN103220339B (en) | Cursor display packing, device and system in remote operation | |
CN116690588A (en) | Multi-mechanical arm multi-task target acquisition method, device, equipment and storage medium | |
JP2020125669A (en) | Method and device for outputting information | |
CN111123951A (en) | Biped robot and track following method and device thereof | |
CN113843794B (en) | Method and device for planning coordinated movement of two mechanical arms, electronic equipment and storage medium | |
CN115533905A (en) | Virtual and real transfer learning method and device of robot operation technology and storage medium | |
CN111158368A (en) | Biped robot and track following method and device thereof | |
CN111595340B (en) | Path determining method and device and electronic equipment | |
CN113650011B (en) | Method and device for planning splicing path of mechanical arm | |
CN106826814B (en) | Motion control method and motion control system of robot | |
CN113805578B (en) | Unmanned vehicle path optimization method and related equipment | |
CN114851190B (en) | Low-frequency drive and control integrated-oriented mechanical arm track planning method and system | |
CN113129366A (en) | Monocular SLAM (simultaneous localization and mapping) initialization method and device and electronic equipment | |
WO2022110452A1 (en) | Robot motion planning method and apparatus, mobile robot and storage medium | |
CN113804208B (en) | Unmanned vehicle path optimization method and related equipment | |
CN113043277B (en) | Multi-joint mechanism trajectory planning method and device, electronic equipment and storage medium | |
CN109246606B (en) | Expansion method and device of robot positioning network, terminal equipment and storage medium | |
CN113050641B (en) | Path planning method and related equipment |
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 |