CN113119116A - 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 PDF

Info

Publication number
CN113119116A
CN113119116A CN202110301669.4A CN202110301669A CN113119116A CN 113119116 A CN113119116 A CN 113119116A CN 202110301669 A CN202110301669 A CN 202110301669A CN 113119116 A CN113119116 A CN 113119116A
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.)
Granted
Application number
CN202110301669.4A
Other languages
Chinese (zh)
Other versions
CN113119116B (en
Inventor
陈金亮
刘益彰
熊友军
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shenzhen Ubtech Technology Co ltd
Original Assignee
Shenzhen Ubtech Technology Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Shenzhen Ubtech Technology Co ltd filed Critical Shenzhen Ubtech Technology Co ltd
Priority to CN202110301669.4A priority Critical patent/CN113119116B/en
Publication of CN113119116A publication Critical patent/CN113119116A/en
Priority to PCT/CN2021/124620 priority patent/WO2022198994A1/en
Application granted granted Critical
Publication of CN113119116B publication Critical patent/CN113119116B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • 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 plan the motion of the mechanical arm. By the method and the device, the virtual optimal motion planning path is used as a guide basis in 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

Mechanical arm motion planning method and device, readable storage medium and mechanical arm
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;
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 plan the motion of the mechanical arm.
Further, the performing iterative optimization on the first random pose according to the virtual optimal motion planning path to obtain an optimized second random pose may include:
mapping the first random pose to the virtual optimal motion planning path to obtain a virtual optimal pose corresponding to the first random pose;
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 a gravitational field function corresponding to the virtual optimal pose may include:
establishing the gravitational field function according to:
Figure BDA0002986510180000021
wherein q isgoalFor the virtual optimal pose, q is the current random pose,
Figure BDA0002986510180000022
d (q, q) is a predetermined scale factorgoal) Is q and qgoalThe distance between the two or more of the two or more,
Figure BDA0002986510180000023
is a predetermined distance threshold, Uatt(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:
Figure BDA0002986510180000031
wherein the content of the first and second substances,
Figure BDA0002986510180000032
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:
Figure BDA0002986510180000033
wherein q is the current random pose,
Figure BDA0002986510180000034
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 performing iterative optimization on the first random pose according to the virtual optimal motion planning path 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:
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:
Figure BDA0002986510180000041
wherein q isgoalFor the virtual optimal pose, q is the current random pose,
Figure BDA0002986510180000042
d (q, q) is a predetermined scale factorgoal) Is q and qgoalThe distance between the two or more of the two or more,
Figure BDA0002986510180000043
is a predetermined distance threshold, Uatt(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:
Figure BDA0002986510180000051
wherein the content of the first and second substances,
Figure BDA0002986510180000052
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 formula:
Figure BDA0002986510180000053
wherein q is the current random pose,
Figure BDA0002986510180000054
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 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.
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 an embodiment 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 a 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 plan the motion of the mechanical arm. According to the method and the device, the virtual optimal motion planning path in an ideal state is used as a guide basis during random sampling, iterative optimization is carried out on an original random pose (namely a first random pose) obtained by random sampling, the original random pose is pulled upwards in the direction of the virtual optimal motion planning path, and the optimized random pose (namely a second random pose) is obtained.
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 CObsAnd free space Cfree。
Wherein, CObsThis can be described by the following formula:
Figure BDA0002986510180000071
i.e. CObsA set of poses satisfying the following conditions: this pose (i.e., q) belongs to C space and the robotic arm in this pose (i.e., robot (q)) does not intersect the obstacle (i.e., Obs) empty, and both collide.
Accordingly, CfreeThis can be described by the following formula:
Cfree＝C-CObs
i.e. CfreeIs CObsThe complement of (c).
CfreeThe 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 CfreeEstablishing 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 space C corresponding to the mechanical arm and obstacle space CObsAnd free space Cfree；
(4) Determining an initial pose q of a robotic arminitAnd object pose qend；
(5) In free space CfreeThe 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%]→CfreeWherein 0 represents a starting time point of the motion plan, 1 represents an ending time point of the motion plan, 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 a free space CfreeAnd the pose of the robot arm at the starting time point of the path is the initial pose, i.e., τ [0 ]]＝qinitThe pose of the mechanical arm at the termination time point of the path is the target pose, namely tau 1]＝qend。
In the embodiment of the application, a straight path connecting the initial pose and the target pose can 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 in free space may be performed in any manner in the prior art. For the sake of easy distinction, the result obtained by performing random sampling in 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:
Figure BDA0002986510180000091
wherein q isgoalA virtual optimal pose, q a current random pose,
Figure BDA0002986510180000092
is a predetermined scale factorThe value of the body can be set according to the actual situation, d (q, q)goal) Is q and qgoalThe distance between the two or more of the two or more,
Figure BDA0002986510180000093
for a predetermined distance threshold, the specific value thereof may be set according to the actual situation, Uatt(q) is a gravitational field function. As can be seen from the above equation, the gravitational field function is a piecewise function when
Figure BDA0002986510180000094
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 in
Figure BDA0002986510180000095
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:
Figure BDA0002986510180000101
wherein the content of the first and second substances,
Figure BDA0002986510180000102
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:
Figure BDA0002986510180000103
wherein q is the current random pose,
Figure BDA0002986510180000104
for the gradient of the current random pose, η is a preset updating step length, the specific value of η 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 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: 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 of step S10332 and the subsequent steps is returned to.
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 performs iterative optimization on 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, and an optimized random pose (namely, a second random pose) is obtained, so that 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:
Figure BDA0002986510180000131
wherein q isgoalFor the virtual optimal pose, q is the current random pose,
Figure BDA0002986510180000132
d (q, q) is a predetermined scale factorgoal) Is q and qgoalThe distance between the two or more of the two or more,
Figure BDA0002986510180000133
is a predetermined distance threshold, Uatt(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:
Figure BDA0002986510180000134
wherein the content of the first and second substances,
Figure BDA0002986510180000135
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 formula:
Figure BDA0002986510180000136
wherein q is the current random pose,
Figure BDA0002986510180000137
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 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 embodiments of the robot arm motion planning method, such as the 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 the above-mentioned device embodiments, 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 a 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), or 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 that does not include electrical carrier signals and telecommunications signals in accordance with legislation and patent practice.
The above-mentioned embodiments are only used for illustrating the technical solutions of the present application, and not for limiting 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 substantially depart from the spirit and scope of the embodiments of the present application and are intended to be included within the scope of 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;
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 plan the motion of 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 motion planning path to obtain an optimized second random pose comprises:
mapping the first random pose to the virtual optimal motion planning path to obtain a virtual optimal pose corresponding to the first random pose;
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 mechanical arm motion planning method according to claim 2, wherein the performing iterative optimization on 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 the 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:
Figure FDA0002986510170000021
wherein q isgoalFor the virtual optimal pose, q is the current random pose,
Figure FDA0002986510170000022
d (q, q) is a predetermined scale factorgoal) Is q and qgoalThe distance between the two or more of the two or more,
Figure FDA0002986510170000023
is a predetermined distance threshold, Uatt(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:
Figure FDA0002986510170000024
wherein the content of the first and second substances,
Figure FDA0002986510170000025
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:
Figure FDA0002986510170000031
wherein q is the current random pose,
Figure FDA0002986510170000032
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 mechanical arm motion planning method according to any one of claims 1 to 6, wherein the determining of the virtual optimal motion planning path for the mechanical arm to move from the preset initial pose to the 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 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 performing iterative optimization on the first random pose according to the virtual optimal motion planning path 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, characterized in that the processor, when executing the computer program, carries out the steps of the robot arm motion planning method according to any of claims 1 to 7.
CN202110301669.4A 2021-03-22 2021-03-22 Mechanical arm motion planning method and device, readable storage medium and mechanical arm Active CN113119116B (en)

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 true CN113119116A (en) 2021-07-16
CN113119116B 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)

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113442144A (en) * 2021-09-01 2021-09-28 北京柏惠维康科技有限公司 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
CN114237233A (en) * 2021-11-30 2022-03-25 深圳市优必选科技股份有限公司 Robot chess playing method and device, computer readable storage medium and robot
CN114310941A (en) * 2021-12-21 2022-04-12 哈尔滨工业大学芜湖机器人产业技术研究院 Robot path generation method for hub wheel hole deburring machining
WO2022198994A1 (en) * 2021-03-22 2022-09-29 深圳市优必选科技股份有限公司 Robot arm motion planning method and apparatus, and readable storage medium and robot arm
CN116476041A (en) * 2022-12-28 2023-07-25 深圳市人工智能与机器人研究院 Force-position hybrid control method of nucleic acid sampling robot and robot

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
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

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20100174435A1 (en) * 2009-01-07 2010-07-08 Samsung Electronics Co., Ltd. Path planning apparatus of robot and method thereof
CN109877836A (en) * 2019-03-13 2019-06-14 浙江大华技术股份有限公司 Paths planning method, device, mechanical arm controller and readable storage medium storing program for executing
CN110744543A (en) * 2019-10-25 2020-02-04 华南理工大学 Improved PRM obstacle avoidance motion planning method based on UR3 mechanical arm
US20200118446A1 (en) * 2018-10-12 2020-04-16 Aurora Flight Sciences Corporation Trajectory Planner for a Vehicle
CN111251306A (en) * 2020-03-18 2020-06-09 广东省智能机器人研究院 Mechanical arm path planning method with chassis error
CN111506083A (en) * 2020-05-19 2020-08-07 上海应用技术大学 Industrial robot safety obstacle avoidance method based on artificial potential field method
CN111638725A (en) * 2020-05-14 2020-09-08 西安电子科技大学 Unmanned aerial vehicle formation reconstruction system and method based on ant colony algorithm and 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
CN112379672A (en) * 2020-11-24 2021-02-19 浙大宁波理工学院 Intelligent unmanned ship path planning method based on improved artificial potential field

Family Cites Families (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
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
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
CN111168675B (en) * 2020-01-08 2021-09-03 北京航空航天大学 Dynamic obstacle avoidance motion planning method for mechanical arm of household service robot
CN113119116B (en) * 2021-03-22 2023-01-31 深圳市优必选科技股份有限公司 Mechanical arm motion planning method and device, readable storage medium and mechanical arm

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20100174435A1 (en) * 2009-01-07 2010-07-08 Samsung Electronics Co., Ltd. Path planning apparatus of robot and method thereof
US20200118446A1 (en) * 2018-10-12 2020-04-16 Aurora Flight Sciences Corporation Trajectory Planner for a Vehicle
CN109877836A (en) * 2019-03-13 2019-06-14 浙江大华技术股份有限公司 Paths planning method, device, mechanical arm controller and readable storage medium storing program for executing
CN110744543A (en) * 2019-10-25 2020-02-04 华南理工大学 Improved PRM obstacle avoidance motion planning method based on UR3 mechanical arm
CN111251306A (en) * 2020-03-18 2020-06-09 广东省智能机器人研究院 Mechanical arm path planning method with chassis error
CN111638725A (en) * 2020-05-14 2020-09-08 西安电子科技大学 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
CN112379672A (en) * 2020-11-24 2021-02-19 浙大宁波理工学院 Intelligent unmanned ship path planning method based on improved artificial potential field

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
任子玉: "智能车自主避障路径规划研究综述", 《软件导刊》 *
刘海恩: "改进的RRT路径规划算法", 《计算机工程与设计》 *

Cited By (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2022198994A1 (en) * 2021-03-22 2022-09-29 深圳市优必选科技股份有限公司 Robot arm motion planning method and apparatus, and readable storage medium and robot arm
CN113442144A (en) * 2021-09-01 2021-09-28 北京柏惠维康科技有限公司 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
CN114237233A (en) * 2021-11-30 2022-03-25 深圳市优必选科技股份有限公司 Robot chess playing method and device, computer readable storage medium and robot
CN114237233B (en) * 2021-11-30 2024-02-23 深圳市优必选科技股份有限公司 Robot chess playing method and device, computer readable storage medium and robot
CN114310941A (en) * 2021-12-21 2022-04-12 哈尔滨工业大学芜湖机器人产业技术研究院 Robot path generation method for hub wheel hole deburring machining
CN114310941B (en) * 2021-12-21 2023-10-20 长三角哈特机器人产业技术研究院 Robot path generation method for hub wheel hole deburring
CN116476041A (en) * 2022-12-28 2023-07-25 深圳市人工智能与机器人研究院 Force-position hybrid control method of nucleic acid sampling robot and robot
CN116476041B (en) * 2022-12-28 2024-01-30 深圳市人工智能与机器人研究院 Force-position hybrid control method of nucleic acid sampling robot and robot

Also Published As

Publication number Publication date
CN113119116B (en) 2023-01-31
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
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
CN113119111A (en) Mechanical arm and track planning method and device thereof
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
CN113283082B (en) Centroid track generation method, centroid track generation device, computer readable storage medium and robot
WO2023066233A1 (en) Program flashing method and apparatus for controller, and controller and storage medium
CN113110423B (en) Gait track planning method and device, computer readable storage medium and robot
CN110720096A (en) Multi-sensor state estimation method and device and terminal equipment
CN114003613A (en) High-precision map lane line updating method and device, electronic equipment and storage medium
CN111211585A (en) Charging equipment distribution method and terminal equipment
CN110470310A (en) Automatic map generates
CN112097772B (en) Robot and map construction method and device thereof
CN113119114B (en) Mechanical arm motion planning method and device, readable storage medium and mechanical arm
CN116974283A (en) Material transportation control method and device, electronic equipment and storage medium
CN112085786B (en) Pose information determining method and device
CN109344877A (en) A kind of sample data processing method, sample data processing unit and electronic equipment
CN109416748B (en) SVM-based sample data updating method, classification system and storage device
CN112669196B (en) Method and equipment for optimizing data by factor graph in hardware acceleration engine
CN108572939B (en) VI-SLAM optimization method, device, equipment and computer readable medium
CN109693233B (en) Robot posture detection method and device, terminal equipment and computer storage medium
CN112904837A (en) Data processing method, device and computer readable storage medium

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