CN113119116B - Mechanical arm motion planning method and device, readable storage medium and mechanical arm - Google Patents
Mechanical arm motion planning method and device, readable storage medium and mechanical arm Download PDFInfo
- Publication number
- CN113119116B CN113119116B CN202110301669.4A CN202110301669A CN113119116B CN 113119116 B CN113119116 B CN 113119116B CN 202110301669 A CN202110301669 A CN 202110301669A CN 113119116 B CN113119116 B CN 113119116B
- Authority
- CN
- China
- Prior art keywords
- pose
- random
- mechanical arm
- motion planning
- random pose
- 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
- 238000000034 method Methods 0.000 title claims abstract description 46
- 238000005070 sampling Methods 0.000 claims abstract description 37
- 238000005457 optimization Methods 0.000 claims abstract description 31
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 16
- 230000006870 function Effects 0.000 claims description 49
- 238000004590 computer program Methods 0.000 claims description 20
- 230000008569 process Effects 0.000 description 12
- 238000004364 calculation method Methods 0.000 description 9
- 238000010586 diagram Methods 0.000 description 7
- 230000004044 response Effects 0.000 description 4
- 230000008878 coupling Effects 0.000 description 3
- 238000010168 coupling process Methods 0.000 description 3
- 238000005859 coupling reaction Methods 0.000 description 3
- 238000012545 processing Methods 0.000 description 3
- 238000004891 communication Methods 0.000 description 2
- 230000005484 gravity Effects 0.000 description 2
- 238000013507 mapping Methods 0.000 description 2
- 230000000295 complement effect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000005381 potential energy Methods 0.000 description 1
- 238000006467 substitution reaction 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
- B25J9/1666—Avoiding collision or forbidden zones
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J18/00—Arms
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1679—Programme controls characterised by the tasks executed
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
Abstract
The application belongs to the technical field of robots, and particularly relates to a mechanical arm motion planning method and device, a computer readable storage medium and a mechanical arm. The method comprises the following steps: determining a virtual optimal motion planning path of the mechanical arm from a preset initial pose to a preset target pose; randomly sampling in a free space of the mechanical arm to obtain a first random pose of the mechanical arm; performing iterative optimization on the first random pose according to the virtual optimal motion planning path to obtain an optimized second random pose; and taking the second random pose as a random sampling result in a preset mechanical arm motion planning algorithm to carry out motion planning on the mechanical arm. According to the method and the device, the virtual optimal motion planning path is used as a guiding basis during random sampling, and iterative optimization is performed on the original random pose obtained by random sampling, so that the random sampling result has obvious guidance, a large number of invalid searches can be reduced, and the planning efficiency is greatly improved.
Description
Technical Field
The application belongs to the technical field of robots, and particularly relates to a mechanical arm motion planning method and device, a computer-readable storage medium and a mechanical arm.
Background
In the prior art, the mechanical arm is generally planned by using a planning algorithm such as rapid-expansion Random Trees (RRT) or Bi-directional rapid-expansion Random Trees (Bi-directional RRT). However, the algorithms are randomly and uniformly sampled in the whole free space, guidance is lacked, a large number of invalid searches exist, and the efficiency is low.
Disclosure of Invention
In view of this, embodiments of the present application provide a method and an apparatus for planning a motion of a robot arm, a computer-readable storage medium, and a robot arm, so as to solve the problems that an existing method for planning a motion of a robot arm lacks guidance and has low efficiency.
A first aspect of an embodiment of the present application provides a method for planning motion of a robot arm, which may include:
determining a virtual optimal motion planning path of the mechanical arm from a preset initial pose to a preset target pose;
randomly sampling in a free space of the mechanical arm to obtain a first random pose of the mechanical arm;
taking the projection of the first random pose on the virtual optimal motion planning path as a virtual optimal pose; performing iterative optimization on the first random pose according to the virtual optimal pose to obtain an optimized second random pose;
and taking the second random pose as a random sampling result in a preset mechanical arm motion planning algorithm to plan the motion of the mechanical arm.
Further, the performing iterative optimization on the first random pose according to the virtual optimal pose to obtain an optimized second random pose may include:
establishing a gravitational field function corresponding to the virtual optimal pose;
and performing iterative optimization on the first random pose according to the gravitational field function to obtain an optimized second random pose.
Further, the performing iterative optimization on the first random pose according to the gravitational field function to obtain an optimized second random pose may include:
taking the first random pose as a current random pose;
calculating the gradient of the current random pose according to the gravitational field function;
updating and calculating the current random pose according to the gradient of the current random pose to obtain an updated random pose;
if the gradient of the current random pose does not meet the preset iteration termination condition, taking the updated random pose as a new current random pose, and returning to execute the step of calculating the gradient of the current random pose according to the gravitational field function and the subsequent steps;
and if the gradient of the current random pose meets the iteration termination condition, determining the updated random pose as the optimized second random pose.
Further, the establishing of the gravitational field function corresponding to the virtual optimal pose may include:
establishing the gravitational field function according to:
wherein q is goal For the virtual optimal pose, q is the current random pose,
d (q, q) is a predetermined scale factor goal ) Is q and q goal The distance between the two or more of the two or more,
is a predetermined distance threshold, U att (q) is the gravitational field function.
Further, the calculating the gradient of the current random pose according to the gravitational field function may include:
calculating a gradient of the current random pose according to:
wherein,
and the gradient of the current random pose is obtained.
Further, the updating and calculating the current random pose according to the gradient of the current random pose to obtain an updated random pose may include:
updating and calculating the current random pose according to the following formula:
wherein q is the current random pose,
and (3) regarding the gradient of the current random pose, wherein eta is a preset updating step length, and q' is the updating random pose.
Further, the determining a virtual optimal motion planning path for the mechanical arm to move from a preset initial pose to a preset target pose may include:
and determining a straight-line path connecting the initial pose and the target pose as the virtual optimal motion planning path.
A second aspect of an embodiment of the present application provides a robot arm movement planning apparatus, which may include:
the virtual path determining module is used for determining a virtual optimal motion planning path for the mechanical arm to move from a preset initial pose to a preset target pose;
the random sampling module is used for carrying out random sampling in the free space of the mechanical arm to obtain a first random pose of the mechanical arm;
the iterative optimization module is used for taking the projection of the first random pose on the virtual optimal motion planning path as a virtual optimal pose; performing iterative optimization on the first random pose according to the virtual optimal pose to obtain an optimized second random pose;
and the motion planning module is used for performing motion planning on the mechanical arm by taking the second random pose as a random sampling result in a preset mechanical arm motion planning algorithm.
Further, the iterative optimization module may include:
the gravitational field function establishing unit is used for establishing a gravitational field function corresponding to the virtual optimal pose;
and the iterative optimization unit is used for performing iterative optimization on the first random pose according to the gravitational field function to obtain an optimized second random pose.
Further, the iterative optimization unit may include:
a current random pose determining subunit, configured to use the first random pose as a current random pose;
the gradiometer unit is used for calculating the gradient of the current random pose according to the gravitational field function;
the updating calculation subunit is used for performing updating calculation on the current random pose according to the gradient of the current random pose to obtain an updated random pose;
a current random pose updating subunit, configured to, if the gradient of the current random pose does not meet a preset iteration termination condition, take the updated random pose as a new current random pose;
and the second random pose determining subunit is used for determining the updated random pose as the optimized second random pose if the gradient of the current random pose meets the iteration termination condition.
Further, the gravitational field function establishing unit is specifically configured to establish the gravitational field function according to the following formula:
wherein q is goal Q is the current random pose for the virtual optimal pose,
is a predetermined scale factor, d (q, q) goal ) Is q and q goal The distance between the two or more of the two or more,
is a predetermined distance threshold, U att (q) is the gravitational field function.
Further, the gradiometer unit is specifically configured to calculate a gradient of the current random pose according to the following formula:
wherein,
and the gradient of the current random pose is obtained.
Further, the update calculation subunit is specifically configured to perform update calculation on the current random pose according to the following equation:
wherein q is the current random pose,
and the gradient of the current random pose is defined, eta is a preset updating step length, and q' is the updating random pose.
Further, the virtual path determining module is specifically configured to determine a straight-line path connecting the initial pose and the target pose as the virtual optimal motion planning path.
A third aspect of embodiments of the present application provides a computer-readable storage medium, which stores a computer program, and the computer program, when executed by a processor, implements the steps of any one of the above-mentioned mechanical arm motion planning methods.
A fourth aspect of the embodiments of the present application provides a robot arm, including a memory, a processor, and a computer program stored in the memory and executable on the processor, where the processor implements the steps of any one of the above methods for planning the motion of the robot arm when executing the computer program.
A fifth aspect of embodiments of the present application provides a computer program product, which, when run on a robot arm, causes the robot arm to perform the steps of any of the robot arm motion planning methods described above.
Compared with the prior art, the embodiment of the application has the advantages that: the method comprises the steps of determining a virtual optimal motion planning path of a mechanical arm from a preset initial pose to a preset target pose; randomly sampling in a free space of the mechanical arm to obtain a first random pose of the mechanical arm; performing iterative optimization on the first random pose according to the virtual optimal motion planning path to obtain an optimized second random pose; and taking the second random pose as a random sampling result in a preset mechanical arm motion planning algorithm to carry out motion planning on the mechanical arm. According to the method and the device, the virtual optimal motion planning path in an ideal state is used as a guiding basis during random sampling, the original random pose (namely the first random pose) obtained by random sampling is subjected to iterative optimization, the original random pose is pulled upwards to the direction of the virtual optimal motion planning path, the optimized random pose (namely the second random pose) is obtained, the random sampling result obtained in the method and the device has obvious guiding performance, the motion planning is performed on the mechanical arm based on the random sampling result, a large amount of invalid searches can be reduced, and the planning efficiency is greatly improved.
Drawings
In order to more clearly illustrate the technical solutions in the embodiments of the present application, the drawings needed for the embodiments or the prior art descriptions will be briefly described below, and it is obvious that the drawings in the following description are only some embodiments of the present application, and it is obvious for those skilled in the art to obtain other drawings without creative efforts.
Fig. 1 is a flowchart illustrating an embodiment of a method for planning a motion of a robot arm according to an embodiment of the present disclosure;
FIG. 2 is a schematic flow diagram of iterative optimization of a first random pose according to a virtual optimal motion planning path;
FIG. 3 is a schematic flow diagram of iterative optimization of a first random pose according to a gravitational field function;
fig. 4 is a structural diagram of an embodiment of a robot motion planning apparatus according to an embodiment of the present disclosure;
fig. 5 is a schematic block diagram of a robot according to an embodiment of the present disclosure.
Detailed Description
In order to make the objects, features and advantages of the present invention more apparent and understandable, the technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present application, and it is apparent that the embodiments described below are only a part of the embodiments of the present application, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present application.
It will be understood that the terms "comprises" and/or "comprising," when used in this specification and the appended claims, specify the presence of stated features, integers, steps, operations, elements, and/or components, but do not preclude the presence or addition of one or more other features, integers, steps, operations, elements, components, and/or groups thereof.
It is also to be understood that the terminology used in the description of the present application herein is for the purpose of describing particular embodiments only and is not intended to be limiting of the application. As used in the specification of the present application and the appended claims, the singular forms "a," "an," and "the" are intended to include the plural forms as well, unless the context clearly indicates otherwise.
It should be further understood that the term "and/or" as used in this specification and the appended claims refers to and includes any and all possible combinations of one or more of the associated listed items.
As used in this specification and the appended claims, the term "if" may be interpreted contextually as "when", "upon" or "in response to a determination" or "in response to a detection". Similarly, the phrase "if it is determined" or "if a [ described condition or event ] is detected" may be interpreted contextually to mean "upon determining" or "in response to determining" or "upon detecting [ described condition or event ]" or "in response to detecting [ described condition or event ]".
In addition, in the description of the present application, the terms "first," "second," "third," and the like are used solely to distinguish one from another, and are not to be construed as indicating or implying relative importance.
Referring to fig. 1, an embodiment of a method for planning motion of a robot arm in an embodiment of the present application may include:
and S101, determining a virtual optimal motion planning path of the mechanical arm from a preset initial pose to a preset target pose.
In the embodiment of the present application, the set of poses of all the joints of the robot arm, i.e., the C space, may be described by a pose space (configuration space), and when the robot arm and the obstacle are considered, the C space may be divided into two spaces, i.e., the obstacle space C Obs And free space C free 。
Wherein, C Obs This can be described by the following formula:
i.e. C Obs A set of poses satisfying the following conditions: this pose (i.e., q) belongs to C space and the mechanical arm in this pose (i.e., robot (q)) intersects the obstacle (i.e., obs) without being empty, and both collide.
Accordingly, C free This can be described by the following formula:
C free ＝C-C Obs
i.e. C free Is C Obs The complement of (c).
C free The selectable area is the area in which the mechanical arm can move and the optimal track can be found in the shortest time. The basic idea of the mechanical arm motion planning algorithm is based on C free Establishing a path diagram for solving the problem of mechanical arm motion planning, wherein the specific process can comprise the following steps:
(1) Determining a working space W of the mechanical arm;
(2) Determining an obstacle Obs and a mechanical arm Robot in a working space W;
(3) Determining the space C corresponding to the mechanical arm and the obstacle space C Obs And free space C free ；
(4) Determining the initial pose q of the mechanical arm init And object pose q end ；
(5) In free space C free The collision-free path tau of the mechanical arm is obtained by sampling according to a mechanical arm motion planning algorithm, and the path needs to satisfy tau 0,1]→C free Wherein 0 represents the starting time point of the motion planning, 1 represents the ending time point of the motion planning, and the pose of the mechanical arm of the path at any planning time point between the starting time point and the ending time point is in the free space C free And the pose of the robot arm at the starting time point of the path is the initial pose, i.e., τ [0 ]]＝q init The pose of the mechanical arm at the termination time point of the path is the target pose, namely tau 1]＝q end 。
In the embodiment of the present application, a straight-line path connecting the initial pose and the target pose may be determined as a virtual optimal motion planning path. It should be noted that this virtual optimal motion planning path is only an optimal path in an ideal state, and this path may pass through the obstacle space, and thus is not feasible in practice. But the path can be used as a guide basis of actual motion planning, and the random search without direction in the motion planning process is reduced.
And S102, randomly sampling in a free space of the mechanical arm to obtain a first random pose of the mechanical arm.
In the embodiment of the present application, random sampling may be performed in free space in any manner in the prior art. For the convenience of distinguishing, the result obtained by performing random sampling by using the prior art is referred to as a first random pose.
And S103, performing iterative optimization on the first random pose according to the virtual optimal motion planning path to obtain an optimized second random pose.
As shown in fig. 2, step S103 may specifically include the following processes:
and step S1031, mapping the first random pose to the virtual optimal motion planning path to obtain a virtual optimal pose corresponding to the first random pose.
In the embodiment of the present application, a projection of the first random pose on the virtual optimal motion planning path may be used as the virtual optimal pose.
And S1032, establishing a gravitational field function corresponding to the virtual optimal pose.
Specifically, the gravitational field function may be established according to:
wherein q is goal Is a virtual optimal pose, q is a current random pose,
the specific value of the preset scale factor can be set according to the actual condition, and d (q, q) goal ) Is q and q goal In between the distance between the first and second electrodes is less than the predetermined distance,
for a predetermined distance threshold, the specific value thereof may be set according to the actual situation, U att (q) is a gravitational field function. As can be seen from the above equation, the gravitational field function is a piecewise function when
The magnitude of the gravitational potential energy is in direct proportion to the square of the distance from the current random pose to the virtual optimal pose; when it comes to
And in time, the value of the gravity calculation function is reduced, so that the problem of overlarge gravity when the current random pose is far away from the virtual optimal pose is solved.
And step S1033, performing iterative optimization on the first random pose according to the gravitational field function to obtain an optimized second random pose.
As shown in fig. 3, step S1033 may specifically include the following processes:
and step S10331, taking the first random pose as the current random pose.
And step S10332, calculating the gradient of the current random pose according to the gravitational field function.
Specifically, the gravitational field function may be derived for the distance to obtain a gradient calculation formula as shown in the following equation:
wherein,
and the gradient of the current random pose is obtained.
And step S10333, updating and calculating the current random pose according to the gradient of the current random pose to obtain an updated random pose.
Specifically, the current random pose may be updated according to the following equation:
wherein q is the current random pose,
for the gradient of the current random pose, eta is a preset updating step length, the specific value of eta can be set according to the actual situation, and q' is the updating random pose. Since the gradient direction is the direction in which the function rises the fastest, a local optimal solution can be obtained through gradient iteration.
Step S10334, determining whether the gradient of the current random pose satisfies a preset iteration termination condition.
In the embodiment of the present application, the iteration termination condition may be: and the gradient of the current planning path is smaller than a preset gradient threshold value, or the iterative optimization time length is larger than a preset time length threshold value. The specific values of the gradient threshold and the duration threshold can be set according to actual conditions. By the method, the iterative optimization time length can be controlled, so that the influence on the overall sampling efficiency due to excessive time consumption is avoided while the extension to the gravitational field direction is ensured.
If the gradient of the current random pose does not meet the preset iteration termination condition, executing step S10335; if the gradient of the current random pose satisfies the iteration termination condition, step S10336 is executed.
And step S10335, taking the updated random pose as a new current random pose.
After step S10335 is performed, execution returns to step S10332 and subsequent steps.
And step S10336, determining the updated random pose as the optimized random pose.
For the sake of distinction, the optimized random pose is referred to herein as the second random pose.
And S104, taking the second random pose as a random sampling result in a preset mechanical arm motion planning algorithm to plan the motion of the mechanical arm.
In the embodiment of the present application, any one of the mechanical arm motion planning algorithms in the prior art may be used to plan the motion of the mechanical arm, including but not limited to RRT or BiRRT and other specific planning algorithms, and the specific planning process may refer to the content in the prior art, which is not described herein again. However, it should be noted that, in the embodiment of the present application, on the basis of the existing mechanical arm motion planning algorithm, the process of random sampling is optimized, for each random sampling, the optimized second random pose is used to replace the original first random pose as a random sampling result, and the motion planning is performed on the mechanical arm based on the random sampling result.
In summary, the embodiment of the application takes the virtual optimal motion planning path in an ideal state as a guiding basis during random sampling, and iteratively optimizes an original random pose (namely, a first random pose) obtained by random sampling, so that the original random pose is pulled in the direction of the virtual optimal motion planning path to obtain an optimized random pose (namely, a second random pose), and the obtained random sampling result has obvious guidance.
It should be understood that, the sequence numbers of the steps in the foregoing embodiments do not imply an execution sequence, and the execution sequence of each process should be determined by its function and inherent logic, and should not constitute any limitation to the implementation process of the embodiments of the present application.
Fig. 4 shows a structural diagram of an embodiment of a robot arm movement planning apparatus provided in an embodiment of the present application, which corresponds to the robot arm movement planning method in the foregoing embodiment.
In this embodiment, a robot motion planning apparatus may include:
the virtual path determining module 401 is configured to determine a virtual optimal motion planning path for the mechanical arm to move from a preset initial pose to a preset target pose;
a random sampling module 402, configured to perform random sampling in a free space of the mechanical arm to obtain a first random pose of the mechanical arm;
an iterative optimization module 403, configured to perform iterative optimization on the first random pose according to the virtual optimal motion planning path to obtain an optimized second random pose;
and a motion planning module 404, configured to perform motion planning on the mechanical arm by using the second random pose as a random sampling result in a preset mechanical arm motion planning algorithm.
Further, the iterative optimization module may include:
a pose mapping unit, configured to map the first random pose to the virtual optimal motion planning path to obtain a virtual optimal pose corresponding to the first random pose;
the gravitational field function establishing unit is used for establishing a gravitational field function corresponding to the virtual optimal pose;
and the iterative optimization unit is used for performing iterative optimization on the first random pose according to the gravitational field function to obtain an optimized second random pose.
Further, the iterative optimization unit may include:
a current random pose determining subunit, configured to use the first random pose as a current random pose;
the gradiometer unit is used for calculating the gradient of the current random pose according to the gravitational field function;
the updating calculation subunit is used for updating and calculating the current random pose according to the gradient of the current random pose to obtain an updated random pose;
a current random pose updating subunit, configured to, if the gradient of the current random pose does not meet a preset iteration termination condition, take the updated random pose as a new current random pose;
and the second random pose determining subunit is used for determining the updated random pose as the optimized second random pose if the gradient of the current random pose meets the iteration termination condition.
Further, the gravitational field function establishing unit is specifically configured to establish the gravitational field function according to the following formula:
wherein q is goal Q is the current random pose for the virtual optimal pose,
d (q, q) is a predetermined scale factor goal ) Is q and q goal The distance between the two or more of the two or more,
is a predetermined distance threshold, U att (q) is the gravitational field function.
Further, the gradiometer unit is specifically configured to calculate a gradient of the current random pose according to the following formula:
wherein,
and the gradient of the current random pose is obtained.
Further, the update calculation subunit is specifically configured to perform update calculation on the current random pose according to the following equation:
wherein q is the current random pose,
and the gradient of the current random pose is defined, eta is a preset updating step length, and q' is the updating random pose.
Further, the virtual path determining module is specifically configured to determine a straight path connecting the initial pose and the target pose as the virtual optimal motion planning path.
It can be clearly understood by those skilled in the art that, for convenience and brevity of description, the specific working processes of the above-described apparatuses, modules and units may refer to the corresponding processes in the foregoing method embodiments, and are not described herein again.
In the above embodiments, the descriptions of the respective embodiments have respective emphasis, and reference may be made to the related descriptions of other embodiments for parts that are not described or illustrated in a certain embodiment.
Fig. 5 shows a schematic block diagram of a robot arm provided in an embodiment of the present application, and only a part related to the embodiment of the present application is shown for convenience of description.
As shown in fig. 5, the robot arm 5 of this embodiment includes: a processor 50, a memory 51 and a computer program 52 stored in said memory 51 and executable on said processor 50. The processor 50, when executing the computer program 52, implements the steps in the above-described various embodiments of the robot arm motion planning method, such as steps S101 to S104 shown in fig. 1. Alternatively, the processor 50, when executing the computer program 52, implements the functions of each module/unit in each device embodiment described above, for example, the functions of the modules 401 to 404 shown in fig. 4.
Illustratively, the computer program 52 may be partitioned into one or more modules/units, which are stored in the memory 51 and executed by the processor 50 to accomplish the present application. The one or more modules/units may be a series of computer program instruction segments capable of performing specific functions, which are used to describe the execution of the computer program 52 in the robot arm 5.
Those skilled in the art will appreciate that figure 5 is merely an example of a robotic arm 5 and does not constitute a limitation of the robotic arm 5 and may include more or fewer components than shown, or some components in combination, or different components, for example the robotic arm 5 may also include input output devices, network access devices, buses, etc.
The Processor 50 may be a Central Processing Unit (CPU), other general purpose Processor, a Digital Signal Processor (DSP), an Application Specific Integrated Circuit (ASIC), a Field Programmable Gate Array (FPGA) or other Programmable logic device, discrete Gate or transistor logic device, discrete hardware component, etc. A general purpose processor may be a microprocessor or the processor may be any conventional processor or the like.
The memory 51 may be an internal storage unit of the robot 5, such as a hard disk or memory of the robot 5. The memory 51 may also be an external storage device of the robot arm 5, such as a plug-in hard disk, a Smart Media Card (SMC), a Secure Digital (SD) Card, a Flash memory Card (Flash Card), and the like, provided on the robot arm 5. Further, the memory 51 may also include both an internal storage unit and an external storage device of the robot arm 5. The memory 51 is used for storing the computer program and other programs and data required by the robot arm 5. The memory 51 may also be used to temporarily store data that has been output or is to be output.
It will be apparent to those skilled in the art that, for convenience and brevity of description, only the above-mentioned division of the functional units and modules is illustrated, and in practical applications, the above-mentioned function distribution may be performed by different functional units and modules according to needs, that is, the internal structure of the apparatus is divided into different functional units or modules to perform all or part of the above-mentioned functions. Each functional unit and module in the embodiments may be integrated in one processing unit, or each unit may exist alone physically, or two or more units are integrated in one unit, and the integrated unit may be implemented in a form of hardware, or in a form of software functional unit. In addition, specific names of the functional units and modules are only for convenience of distinguishing from each other, and are not used for limiting the protection scope of the present application. The specific working processes of the units and modules in the system may refer to the corresponding processes in the foregoing method embodiments, and are not described herein again.
In the above embodiments, the descriptions of the respective embodiments have respective emphasis, and reference may be made to the related descriptions of other embodiments for parts that are not described or illustrated in a certain embodiment.
Those of ordinary skill in the art will appreciate that the various illustrative elements and algorithm steps described in connection with the embodiments disclosed herein may be implemented as electronic hardware or combinations of computer software and electronic hardware. Whether such functionality is implemented as hardware or software depends upon the particular application and design constraints imposed on the implementation. Skilled artisans may implement the described functionality in varying ways for each particular application, but such implementation decisions should not be interpreted as causing a departure from the scope of the present application.
In the embodiments provided herein, it should be understood that the disclosed apparatus/robot and method may be implemented in other ways. For example, the above-described apparatus/robot embodiments are merely illustrative, and for example, the division of the modules or units is merely a logical division, and other divisions may be realized in practice, for example, a plurality of units or components may be combined or integrated into another system, or some features may be omitted, or not executed. In addition, the shown or discussed mutual coupling or direct coupling or communication connection may be an indirect coupling or communication connection through some interfaces, devices or units, and may be in an electrical, mechanical or other form.
The units described as separate parts may or may not be physically separate, and parts displayed as units may or may not be physical units, may be located in one place, or may be distributed on a plurality of network units. Some or all of the units can be selected according to actual needs to achieve the purpose of the solution of the embodiment.
In addition, functional units in the embodiments of the present application may be integrated into one processing unit, or each unit may exist alone physically, or two or more units are integrated into one unit. The integrated unit can be realized in a form of hardware, and can also be realized in a form of a software functional unit.
The integrated modules/units, if implemented in the form of software functional units and sold or used as separate products, may be stored in a computer readable storage medium. Based on such understanding, all or part of the flow in the method of the embodiments described above can be realized by a computer program, which can be stored in a computer-readable storage medium and can realize the steps of the embodiments of the methods described above when the computer program is executed by a processor. Wherein the computer program comprises computer program code, which may be in the form of source code, object code, an executable file or some intermediate form, etc. The computer-readable storage medium may include: any entity or device capable of carrying the computer program code, recording medium, usb disk, removable hard disk, magnetic disk, optical disk, computer Memory, read-Only Memory (ROM), random Access Memory (RAM), electrical carrier wave signals, telecommunications signals, software distribution medium, and the like. It should be noted that the computer-readable storage medium may contain content that is subject to appropriate increase or decrease as required by legislation and patent practice in jurisdictions, for example, in some jurisdictions, computer-readable storage media may not include electrical carrier signals and telecommunications signals in accordance with legislation and patent practice.
The above-mentioned embodiments are only used to illustrate the technical solutions of the present application, and not to limit the same; although the present application has been described in detail with reference to the foregoing embodiments, it should be understood by those of ordinary skill in the art that: the technical solutions described in the foregoing embodiments may still be modified, or some technical features may be equivalently replaced; such modifications and substitutions do not depart from the spirit and scope of the embodiments of the present application, and they should be construed as being included in the present application.
Claims (10)
1. A mechanical arm motion planning method is characterized by comprising the following steps:
determining a virtual optimal motion planning path of the mechanical arm from a preset initial pose to a preset target pose;
randomly sampling in a free space of the mechanical arm to obtain a first random pose of the mechanical arm;
taking the projection of the first random pose on the virtual optimal motion planning path as a virtual optimal pose; performing iterative optimization on the first random pose according to the virtual optimal pose to obtain an optimized second random pose;
and taking the second random pose as a random sampling result in a preset mechanical arm motion planning algorithm to carry out motion planning on the mechanical arm.
2. The mechanical arm motion planning method according to claim 1, wherein the performing iterative optimization on the first random pose according to the virtual optimal pose to obtain an optimized second random pose comprises:
establishing a gravitational field function corresponding to the virtual optimal pose;
and performing iterative optimization on the first random pose according to the gravitational field function to obtain an optimized second random pose.
3. The method for planning the motion of the mechanical arm according to claim 2, wherein the iteratively optimizing the first random pose according to the gravitational field function to obtain an optimized second random pose comprises:
taking the first random pose as a current random pose;
calculating the gradient of the current random pose according to the gravitational field function;
updating and calculating the current random pose according to the gradient of the current random pose to obtain an updated random pose;
if the gradient of the current random pose does not meet the preset iteration termination condition, taking the updated random pose as a new current random pose, and returning to execute the step of calculating the gradient of the current random pose according to the gravitational field function and the subsequent steps;
and if the gradient of the current random pose meets the iteration termination condition, determining the updated random pose as an optimized second random pose.
4. The robotic arm motion planning method of claim 3, wherein the establishing the gravitational field function corresponding to the virtual optimal pose comprises:
establishing the gravitational field function according to:
wherein q is goal Q is the current random pose for the virtual optimal pose,
d (q, q) is a predetermined scale factor goal ) Is q and q goal The distance between the two or more of the two or more,
is a predetermined distance threshold, U att (q) is the gravitational field function.
5. The mechanical arm motion planning method according to claim 4, wherein the calculating the gradient of the current random pose according to the gravitational field function comprises:
calculating a gradient of the current random pose according to:
wherein,
and the gradient of the current random pose is obtained.
6. The mechanical arm motion planning method according to claim 3, wherein the updating the current random pose according to the gradient of the current random pose to obtain an updated random pose comprises:
updating and calculating the current random pose according to the following formula:
wherein q is the current randomThe position and the attitude of the robot are shown,
and (3) regarding the gradient of the current random pose, wherein eta is a preset updating step length, and q' is the updating random pose.
7. The method for planning the motion of a mechanical arm according to any one of claims 1 to 6, wherein the determining a virtual optimal motion planning path for the mechanical arm to move from a preset initial pose to a preset target pose comprises:
and determining a straight-line path connecting the initial pose and the target pose as the virtual optimal motion planning path.
8. A robot motion planning apparatus, comprising:
the virtual path determining module is used for determining a virtual optimal motion planning path from the movement of the mechanical arm from a preset initial pose to a preset target pose;
the random sampling module is used for carrying out random sampling in the free space of the mechanical arm to obtain a first random pose of the mechanical arm;
the iterative optimization module is used for taking the projection of the first random pose on the virtual optimal motion planning path as a virtual optimal pose; performing iterative optimization on the first random pose according to the virtual optimal pose to obtain an optimized second random pose;
and the motion planning module is used for performing motion planning on the mechanical arm by taking the second random pose as a random sampling result in a preset mechanical arm motion planning algorithm.
9. A computer-readable storage medium, in which a computer program is stored which, when being executed by a processor, carries out the steps of the robot arm motion planning method according to any one of claims 1 to 7.
10. A robot arm comprising a memory, a processor and a computer program stored in the memory and executable on the processor, wherein the processor when executing the computer program performs the steps of the robot arm motion planning method according to any of claims 1 to 7.
Priority Applications (2)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110301669.4A CN113119116B (en) | 2021-03-22 | 2021-03-22 | Mechanical arm motion planning method and device, readable storage medium and mechanical arm |
| PCT/CN2021/124620 WO2022198994A1 (en) | 2021-03-22 | 2021-10-19 | Robot arm motion planning method and apparatus, and readable storage medium and robot arm |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110301669.4A CN113119116B (en) | 2021-03-22 | 2021-03-22 | Mechanical arm motion planning method and device, readable storage medium and mechanical arm |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113119116A CN113119116A (en) | 2021-07-16 |
| CN113119116B true CN113119116B (en) | 2023-01-31 |
Family
ID=76773591
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110301669.4A Active CN113119116B (en) | 2021-03-22 | 2021-03-22 | Mechanical arm motion planning method and device, readable storage medium and mechanical arm |
Country Status (2)
| Country | Link |
|---|---|
| CN (1) | CN113119116B (en) |
| WO (1) | WO2022198994A1 (en) |
Families Citing this family (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113119116B (en) * | 2021-03-22 | 2023-01-31 | 深圳市优必选科技股份有限公司 | Mechanical arm motion planning method and device, readable storage medium and mechanical arm |
| CN113442144B (en) * | 2021-09-01 | 2021-11-19 | 北京柏惠维康科技有限公司 | Optimal pose determining method and device under constraint, storage medium and mechanical arm |
| CN113681565A (en) * | 2021-09-08 | 2021-11-23 | 浙江大学 | Man-machine cooperation method and device for realizing article transfer between robots |
| CN114237233B (en) * | 2021-11-30 | 2024-02-23 | 深圳市优必选科技股份有限公司 | Robot chess playing method and device, computer readable storage medium and robot |
| CN114310941B (en) * | 2021-12-21 | 2023-10-20 | 长三角哈特机器人产业技术研究院 | Robot path generation method for hub wheel hole deburring |
| CN116476041B (en) * | 2022-12-28 | 2024-01-30 | 深圳市人工智能与机器人研究院 | Force-position hybrid control method of nucleic acid sampling robot and robot |
| CN116690585B (en) * | 2023-07-25 | 2024-01-12 | 上海汇丰医疗器械股份有限公司 | Shadowless lamp path planning method and device based on automatic mechanical arm |
| CN117950323B (en) * | 2024-03-27 | 2024-05-31 | 苏州巴奈特机械设备有限公司 | Self-adaptive adjusting method and system based on mechanical arm processing control |
Family Cites Families (17)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR101554515B1 (en) * | 2009-01-07 | 2015-09-21 | 삼성전자 주식회사 | path planning apparatus of robot and method thereof |
| KR101667032B1 (en) * | 2009-10-30 | 2016-10-17 | 삼성전자 주식회사 | Path planning apparatus of robot and method thereof |
| KR102165437B1 (en) * | 2014-05-02 | 2020-10-14 | 한화디펜스 주식회사 | Path planning apparatus of mobile robot |
| CN106990777A (en) * | 2017-03-10 | 2017-07-28 | 江苏物联网研究发展中心 | Robot local paths planning method |
| CN106695802A (en) * | 2017-03-19 | 2017-05-24 | 北京工业大学 | Improved RRT<*> obstacle avoidance motion planning method based on multi-degree-of-freedom mechanical arm |
| US10878706B2 (en) * | 2018-10-12 | 2020-12-29 | Aurora Flight Sciences Corporation | Trajectory planner for a vehicle |
| CN109571466B (en) * | 2018-11-22 | 2021-01-26 | 浙江大学 | Seven-degree-of-freedom redundant mechanical arm dynamic obstacle avoidance path planning method based on rapid random search tree |
| CN109877836B (en) * | 2019-03-13 | 2021-06-08 | 浙江大华技术股份有限公司 | Path planning method and device, mechanical arm controller and readable storage medium |
| CN110744543B (en) * | 2019-10-25 | 2021-02-19 | 华南理工大学 | Improved PRM obstacle avoidance motion planning method based on UR3 mechanical arm |
| CN111168675B (en) * | 2020-01-08 | 2021-09-03 | 北京航空航天大学 | Dynamic obstacle avoidance motion planning method for mechanical arm of household service robot |
| CN111251306B (en) * | 2020-03-18 | 2022-11-29 | 广东省智能机器人研究院 | Mechanical arm path planning method with chassis error |
| CN111638725B (en) * | 2020-05-14 | 2021-08-10 | 西安电子科技大学 | Unmanned aerial vehicle formation reconstruction system and method based on ant colony algorithm and artificial potential field method |
| CN111506083A (en) * | 2020-05-19 | 2020-08-07 | 上海应用技术大学 | Industrial robot safety obstacle avoidance method based on artificial potential field method |
| CN111781948A (en) * | 2020-06-18 | 2020-10-16 | 南京非空航空科技有限公司 | Unmanned aerial vehicle formation shape transformation and online dynamic obstacle avoidance method |
| CN112327831A (en) * | 2020-10-20 | 2021-02-05 | 大连理工大学 | Factory AGV track planning method based on improved artificial potential field method |
| CN112379672B (en) * | 2020-11-24 | 2022-05-10 | 浙大宁波理工学院 | Intelligent unmanned ship path planning method based on improved artificial potential field |
| CN113119116B (en) * | 2021-03-22 | 2023-01-31 | 深圳市优必选科技股份有限公司 | Mechanical arm motion planning method and device, readable storage medium and mechanical arm |
-
2021
- 2021-03-22 CN CN202110301669.4A patent/CN113119116B/en active Active
- 2021-10-19 WO PCT/CN2021/124620 patent/WO2022198994A1/en active Application Filing
Also Published As
| Publication number | Publication date |
|---|---|
| CN113119116A (en) | 2021-07-16 |
| WO2022198994A1 (en) | 2022-09-29 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113119116B (en) | Mechanical arm motion planning method and device, readable storage medium and mechanical arm | |
| JP6443905B1 (en) | Robot motion path planning method, apparatus, storage medium, and terminal device | |
| US10852139B2 (en) | Positioning method, positioning device, and robot | |
| WO2022007350A1 (en) | Global path planning method and apparatus, terminal, and readable storage medium | |
| CN113119111A (en) | Mechanical arm and track planning method and device thereof | |
| CN110471409B (en) | Robot inspection method and device, computer readable storage medium and robot | |
| CN113119115A (en) | Mechanical arm motion planning method and device, readable storage medium and mechanical arm | |
| CN110969649B (en) | Matching evaluation method, medium, terminal and device for laser point cloud and map | |
| WO2022160787A1 (en) | Robot hand-eye calibration method and apparatus, readable storage medium, and robot | |
| WO2022110451A1 (en) | Method and apparatus for positioning robot, computer-readable storage medium, and robot | |
| CN112597610B (en) | Optimization method, device and equipment for lightweight design of mechanical arm structure | |
| WO2023066233A1 (en) | Program flashing method and apparatus for controller, and controller and storage medium | |
| CN113110423A (en) | Gait trajectory planning method and device, computer readable storage medium and robot | |
| CN117036422A (en) | Method, device, equipment and storage medium for tracking lane lines | |
| CN112016678A (en) | Training method and device for strategy generation network for reinforcement learning and electronic equipment | |
| CN111211585A (en) | Charging equipment distribution method and terminal equipment | |
| CN114283398A (en) | Method and device for processing lane line and electronic equipment | |
| CN113119083B (en) | Robot calibration method and device, robot and storage medium | |
| CN113119114B (en) | Mechanical arm motion planning method and device, readable storage medium and mechanical arm | |
| CN112085786B (en) | Pose information determining method and device | |
| CN116974283A (en) | Material transportation control method and device, electronic equipment and storage medium | |
| EP3610387B1 (en) | Distributed data structures for sliding window aggregation or similar applications | |
| CN112729349B (en) | Method and device for on-line calibration of odometer, electronic equipment and storage medium | |
| CN108572939B (en) | VI-SLAM optimization method, device, equipment and computer readable medium | |
| CN113927585A (en) | Robot balance control method and device, readable storage medium and robot |
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 |