WO2022198993A1

WO2022198993A1 - Method and apparatus for manipulator motion planning, readable storage medium, and manipulator

Info

Publication number: WO2022198993A1
Application number: PCT/CN2021/124619
Authority: WO
Inventors: 陈金亮; 刘益彰; 熊友军
Original assignee: 深圳市优必选科技股份有限公司
Priority date: 2021-03-22
Filing date: 2021-10-19
Publication date: 2022-09-29
Also published as: CN113119115A

Abstract

The present application relates to the technical field of robots and specifically relates to a method and apparatus for manipulator motion planning, a computer-readable storage medium, and a manipulator. The method, by means of a first planning tree extending from an initial posture to a target posture and of a second planning tree extending from the target posture to the initial posture, implements the two-way parallel extension of a motion planning path, and introduces to a process of extension a path cost function to optimize the planning trees, thus allowing the motion planning path produced to approach the optimal path while increasing efficiency and reducing time consumption.

Description

A robotic arm motion planning method, device, readable storage medium, and robotic arm

This application claims the priority of the Chinese Patent Application No. 202110301129.6 filed with the Chinese Patent Office on March 22, 2021, the entire contents of which are incorporated herein by reference.

technical field

The present application belongs to the field of robotics technology, and in particular relates to a method, device, computer-readable storage medium and a robotic arm for motion planning of a robotic arm.

Background technique

When planning the motion of the manipulator in the prior art, a tree is generally constructed with the initial pose as the root node, and the search is randomly extended in the entire free space until the target pose is reached. Although this method can obtain the motion planning path of the manipulator, it is inefficient and time-consuming, and the obtained motion planning path is often far from the optimal path.

technical problem

In view of this, the embodiments of the present application provide a robotic arm motion planning method, device, computer-readable storage medium, and robotic arm, so as to solve the problem that the existing robotic arm motion planning method is relatively inefficient, takes a long time, and The obtained motion planning path often has a large gap with the optimal path.

technical solutions

A first aspect of the embodiments of the present application provides a method for planning a motion of a robotic arm, which may include:

Perform random sampling in the free space of the robotic arm to obtain the pose sampling points of the robotic arm;

A first expansion point corresponding to a first planning tree is determined in the free space according to the pose sampling points; the first planning tree is a plan extending from a preset initial pose to a preset target pose Tree;

adding the first extension point to the first planning tree according to a preset path cost function, and performing optimization processing on the first planning tree;

A second expansion point corresponding to the second planning tree is determined in the free space according to the first expansion point; the second planning tree is a planning tree expanded from the target pose to the initial pose;

adding the second extension point into the second planning tree according to the path cost function, and performing optimization processing on the second planning tree;

When the first planning tree and the second planning tree satisfy a preset coincidence condition, a motion planning path of the robotic arm is determined according to the first planning tree and the second planning tree.

Further, the adding the first extension point into the first planning tree according to a preset path cost function may include:

A node whose distance from the first expansion point in the first planning tree is less than a preset distance threshold is used as the adjacent point of the first expansion point;

Calculate the first path cost corresponding to each adjacent point according to the path cost function; the first path cost is the path cost from the initial pose to the adjacent point and the path cost from the adjacent point to the first extension point the sum of path costs;

The first extension point is added to the first planning tree by taking the adjacent point with the smallest first path cost as the parent node of the first extension point.

Further, the performing optimization processing on the first planning tree may include:

For each adjacent point except the parent node of the first extension point, calculate the second path cost and the third path cost corresponding to the adjacent point according to the path cost function; the second path cost is from The path cost from the initial pose to the adjacent point; the third path cost is the path cost from the initial pose to the first extension point and the path from the adjacent point to the first extension point the sum of the costs;

If the third path cost is less than the second path cost, the first extension point is used as the parent node of the adjacent point.

Optionally, the path cost function is:

where path is the target path, qn is the nth node on the target path, 1≤n≤N, N is the number of nodes on the target path, and similarity(qn,qn+1) is qn and qn+ Cosine similarity between 1, cost(path) is the path cost of the target path.

Optionally, the path cost function is:

Among them, dist(qn, qn+1) is the Euclidean distance between qn and qn+1.

Preferably, the path cost function is:

Among them, Weight1 and Weight2 are both preset weight coefficients.

Further, the determining the first expansion point corresponding to the first planning tree in the free space according to the pose sampling point may include:

Taking the node with the shortest distance from the pose sampling point in the first planning tree as the node to be expanded;

Calculate the distance between the pose sampling point and the node to be expanded;

If the distance between the pose sampling point and the node to be expanded is less than or equal to a preset step size, determining the pose sampling point as the first expansion point corresponding to the first planning tree;

If the distance between the pose sampling point and the node to be expanded is greater than the step size, then determine the point on the extension line whose distance from the node to be expanded is the step size as a candidate point ; Described extension line is the connection line between described pose sampling point and described to-be-expanded node;

If the candidate point is in the free space, the candidate point is determined as the first extension point corresponding to the first planning tree.

A second aspect of the embodiments of the present application provides a robotic arm motion planning device, which may include:

The random sampling module is used for random sampling in the free space of the manipulator to obtain the pose sampling points of the manipulator;

A first expansion point determination module, configured to determine a first expansion point corresponding to the first planning tree in the free space according to the pose sampling point; the first planning tree is a direction from a preset initial pose to The extended planning tree of the preset target pose;

a first planning tree optimization module, configured to add the first extension point into the first planning tree according to a preset path cost function, and perform optimization processing on the first planning tree;

A second expansion point determining module, configured to determine a second expansion point corresponding to the second planning tree in the free space according to the first expansion point; the second planning tree is a direction from the target pose to the Describe the planning tree of the initial pose expansion;

A second planning tree optimization module, configured to add the second expansion point into the second planning tree according to the path cost function, and perform optimization processing on the second planning tree;

A motion planning path determination module, configured to determine the motion of the robotic arm according to the first planning tree and the second planning tree when the first planning tree and the second planning tree satisfy a preset coincidence condition Motion planning path.

Further, the first planning tree optimization module may include:

an adjacent point determination unit, configured to use a node whose distance from the first expansion point in the first planning tree is less than a preset distance threshold as the adjacent point of the first expansion point;

The first calculation unit is used to calculate the first path cost corresponding to each adjacent point according to the path cost function; the first path cost is the path cost from the initial pose to the adjacent point and the path cost from the adjacent point to the adjacent point. the sum of the path costs of the first extension point;

An extension point adding unit, configured to add the first extension point to the first planning tree by taking the adjacent point with the smallest first path cost as the parent node of the first extension point.

Further, the first planning tree optimization module may also include:

The second calculation unit is configured to, for each adjacent point except the parent node of the first extension point, calculate the second path cost and the third path cost corresponding to the adjacent point according to the path cost function; The second path cost is the path cost from the initial pose to the adjacent point; the third path cost is the path cost from the initial pose to the first extension point and the path cost from the adjacent point to the adjacent point. The sum of the path costs of the first extension point;

A parent node changing unit, configured to use the first extension point as the parent node of the adjacent point if the third path cost is less than the second path cost.

Further, the first extension point determination module may include:

a node to be expanded determination unit, configured to use the node with the shortest distance from the pose sampling point in the first planning tree as the node to be expanded;

a distance calculation unit, configured to calculate the distance between the pose sampling point and the node to be expanded;

a first determining unit, configured to determine the pose sampling point as the first planning tree if the distance between the pose sampling point and the node to be expanded is less than or equal to a preset step size the corresponding first extension point;

A candidate point determination unit, configured to set the distance between the extended connection line and the node to be extended as the step if the distance between the pose sampling point and the node to be extended is greater than the step size The long point is determined as a candidate point; the extended connection is the connection between the pose sampling point and the node to be extended;

A second determining unit, configured to determine the candidate point as a first extension point corresponding to the first planning tree if the candidate point is in the free space.

A third aspect of the embodiments of the present application provides a computer-readable storage medium, where a computer program is stored in the computer-readable storage medium, and when the computer program is executed by a processor, any one of the above-mentioned robotic arm motion planning methods is implemented. step.

A fourth aspect of the embodiments of the present application provides a robotic arm, including a memory, a processor, and a computer program stored in the memory and executable on the processor, when the processor executes the computer program The steps of implementing any one of the above-mentioned robotic arm motion planning methods.

A fifth aspect of the embodiments of the present application provides a computer program product, which, when the computer program product runs on a robotic arm, causes the robotic arm to execute the steps of any of the above-mentioned methods for motion planning of a robotic arm.

beneficial effect

Compared with the prior art, the beneficial effects of the embodiments of the present application are: the first planning tree extended from the initial pose to the target pose and the second planning tree extended from the target pose to the initial pose are used in the embodiments of the present application. The bidirectional parallel expansion of the motion planning path is realized, and the path cost function is introduced in the expansion process to optimize the planning tree, so as to improve the efficiency and reduce the time-consuming, and make the obtained motion planning path closer to the optimal path. optimal path.

Description of drawings

In order to illustrate the technical solutions in the embodiments of the present application more clearly, the following briefly introduces the accompanying drawings that need to be used in the description of the embodiments or the prior art. Obviously, the drawings in the following description are only for the present application. In some embodiments, for those of ordinary skill in the art, other drawings can also be obtained according to these drawings without any creative effort.

1 is a flowchart of an embodiment of a method for planning a motion of a robotic arm in an embodiment of the present application;

Fig. 2 is the schematic flow chart of adding the first extension point into the first planning tree;

3 is a structural diagram of an embodiment of a robotic arm motion planning device in an embodiment of the present application;

FIG. 4 is a schematic block diagram of a robotic arm in an embodiment of the present application.

Embodiments of the present invention

In order to make the purpose, features and advantages of the invention of the present application more obvious and understandable, the technical solutions in the embodiments of the present application will be described clearly and completely below with reference to the accompanying drawings in the embodiments of the present application. Obviously, the following The described embodiments are only some, but not all, embodiments of the present application. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative work fall within the protection scope of the present application.

It is to be understood that, when used in this specification and the appended claims, the term "comprising" indicates the presence of the described feature, integer, step, operation, element and/or component, but does not exclude one or more other features , whole, step, operation, element, component and/or the presence or addition of a collection thereof.

It should also be understood that the terminology used in the specification of the application herein is for the purpose of describing particular embodiments only and is not intended to limit the application. As used in this specification and the appended claims, the singular forms "a," "an," and "the" are intended to include the plural unless the context clearly dictates otherwise.

It should also be further understood that, as used in this specification and the appended claims, the term "and/or" refers to and including 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 contextually interpreted as "when" or "once" or "in response to determining" or "in response to detecting" . Similarly, the phrases "if it is determined" or "if the [described condition or event] is detected" may be interpreted, depending on the context, to mean "once it is determined" or "in response to the determination" or "once the [described condition or event] is detected. ]" or "in response to detection of the [described condition or event]".

In addition, in the description of the present application, the terms "first", "second", "third", etc. are only used to distinguish the description, and cannot be understood as indicating or implying relative importance.

In this embodiment of the present application, the set composed of the poses of all joints of the manipulator can be described by the configuration space, that is, the C space, and when the manipulator and obstacles are considered, the C space will be divided into two spaces , namely the obstacle space C _Obs and the free space C _free .

Among them, C _Obs can be described by the following formula:

That is, C _Obs is a set of poses that meet the following conditions: the pose (ie q) belongs to the C space and the robot arm (ie Robot(q)) in this pose and the obstacle (ie Obs) intersect not empty, the two collide.

Correspondingly, C _free can be described by the following formula:

C _free = CC _Obs

That is, C _free is the complement of C _Obs .

C _free is an optional area in which the robotic arm can move and find the optimal trajectory and the shortest time. The basic idea of the manipulator motion planning algorithm is to establish a path diagram for solving the manipulator motion planning problem based on C _free . The specific process can include:

(1) Determine the workspace W of the robotic arm;

(2) Determine the obstacles Obs in the workspace W and the robotic arm Robot;

(3) Determine the C space corresponding to the manipulator, as well as the obstacle space C _Obs and the free space C _free ;

(4) Determine the initial pose q _init and the target pose q _en ^d of the robotic arm;

(5) The collision-free path τ of the manipulator is obtained by sampling according to the motion planning algorithm of the manipulator in the free space C _free , and the path must satisfy τ[0,1]→C _free , where 0 represents the starting time point of the motion planning , 1 represents the termination time point of the motion planning, the pose of the robot arm at any planning time point between the start time point and the termination time point of the path is within the range of the free space C _free , and the path The pose of the manipulator at the start time point is the initial pose, that is, τ[0]=q _init , and the pose of the manipulator at the end time point of the path is the target pose, that is, τ[1]=q _end .

Referring to FIG. 1, an embodiment of a robotic arm motion planning method in the embodiment of the present application may include:

In step S101, random sampling is performed in the free space of the manipulator to obtain the pose sampling points of the manipulator.

Step S102: Determine a first expansion point corresponding to the first planning tree in the free space according to the pose sampling points.

The first planning tree is a planning tree extended from a preset initial pose to a preset target pose, that is, a forward-expanding planning tree. In the initial state, the first planning tree has only one node, that is, the initial pose.

First, the node with the shortest distance from the pose sampling point in the first planning tree is used as the node to be expanded, and the distance between the pose sampling point and the node to be expanded is calculated.

If the distance between the pose sampling point and the node to be expanded is less than or equal to the preset step size, the pose sampling point is determined as the first expansion point corresponding to the first planning tree. The specific value of the step size may be set according to the actual situation, which is not specifically limited in this embodiment of the present application. If the distance between the pose sampling point and the node to be extended is greater than the step size, the point on the extension line whose distance from the node to be extended is the step size is determined as a candidate point, and the extension line here is The connection between the pose sampling point and the node to be expanded.

If the candidate point is in the obstacle space, return to step S101 to perform random sampling again; if the candidate point is in the free space, determine the candidate point as the first expansion point corresponding to the first planning tree.

Step S103: Add the first expansion point into the first planning tree according to the preset path cost function, and perform optimization processing on the first planning tree.

In this embodiment of the present application, different path cost functions may be used to evaluate the pros and cons of paths according to actual conditions.

Optionally, the path cost can be calculated based on cosine similarity in the path cost function, which is as follows:

where path is the target path, qn is the nth node on the target path, 1≤n≤N, N is the number of nodes on the target path, and similarity(qn,qn+1) is the difference between qn and qn+1 Cosine similarity, cost(path) is the path cost of the target path.

Optionally, the path cost can be calculated based on the Euclidean distance in the path cost function, which is as follows:

Among them, dist(qn, qn+1) is the Euclidean distance between qn and qn+1.

Preferably, the path cost can be calculated by combining cosine similarity and Euclidean distance in the path cost function, and the path cost function is as follows:

Wherein, Weight1 and Weight2 are both preset weight coefficients, and their specific values may be set according to actual conditions, which are not specifically limited in this embodiment of the present application.

As shown in Figure 2, the process of adding the first extension point to the first planning tree may specifically include the following steps:

Step S1031 , a node whose distance from the first expansion point in the first planning tree is less than a preset distance threshold is used as an adjacent point of the first expansion point.

The specific value of the distance threshold may be set according to the actual situation, which is not specifically limited in this embodiment of the present application. Generally, the distance threshold should be larger than the step size of each expansion.

Step S1032: Calculate the first path cost corresponding to each adjacent point according to the path cost function.

The first path cost is the sum of the path cost from the initial pose to the adjacent point and the path cost from the adjacent point to the first extension point.

Step S1033 , taking the adjacent point with the smallest first path cost as the parent node of the first extension point, and adding the first extension point into the first planning tree.

At this time, the first extension point becomes a new node of the first planning tree, and the connection between the first extension point and its parent node becomes a new edge of the first planning tree. Through such a node addition process, the path cost of each newly generated node can be reduced as much as possible.

Preferably, after the first extension point is added to the first planning tree, the first planning tree may be further optimized. Specifically, for each adjacent point except the parent node of the first extension point, the second path cost and the third path cost corresponding to the adjacent point are calculated according to the path cost function respectively. Among them, the second path cost is the path cost from the initial pose to the adjacent point, and the third path cost is the sum of the path cost from the initial pose to the first extension point and the path cost from the adjacent point to the first extension point and. If the third path cost is greater than or equal to the second path cost, the adjacent point does not need to be optimized; if the third path cost is less than the second path cost, the first extension point is used as the parent node of the adjacent point, that is, the adjacent point is deleted. The edge between the adjacent point and its original parent node, and the connection between the first extension point and the adjacent point becomes a new edge of the first planning tree.

Through such optimization processing, each time a new node is generated, path optimization can be performed to reduce the path cost of some nodes. From an overall perspective, not every node optimized for the path will appear in the final generated path, but in the process of generating the random tree, each path optimization creates an opportunity to reduce the final path cost as much as possible .

Step S104: Determine a second extension point corresponding to the second planning tree in the free space according to the first extension point.

The second planning tree is a planning tree extended from the target pose to the initial pose, that is, a planning tree extended in reverse. In the initial state, the second planning tree has only one node, that is, the target pose.

First, the node with the shortest distance from the first expansion point in the second planning tree is used as the node to be expanded, and the distance between the first expansion point and the node to be expanded is calculated.

If the distance between the first extension point and the node to be extended is less than or equal to the preset step size, the first extension point is determined as the second extension point corresponding to the second planning tree. If the distance between the first extension point and the node to be extended is greater than the step size, the point on the extension line whose distance from the node to be extended is the step size is determined as a candidate point, and the extension line here is The connection between the first expansion point and the node to be expanded.

If the candidate point is in the obstacle space, return to step S101 to perform random sampling again; if the candidate point is in the free space, the candidate point is determined as the second expansion point corresponding to the second planning tree.

Step S105: Add the second expansion point into the second planning tree according to the path cost function, and perform optimization processing on the second planning tree.

Step S105 is similar to step S103, and the specific process can refer to the detailed description in step S103, which will not be repeated here.

Step S106 , when the first planning tree and the second planning tree satisfy the preset coincidence condition, determine the motion planning path of the robotic arm according to the first planning tree and the second planning tree.

The coincidence condition is that the first planning tree and the second planning tree are connected, that is, the two have the same node. When the coincidence condition is not met, return to step S101 to re-sample randomly; when this condition is met, the first planning tree and the second planning tree are combined into a whole, because the first planning tree is from the initial pose to the target pose The second planning tree is extended from the target pose to the initial pose. After the two are combined during the expansion, a complete path from the initial pose to the target pose will be formed, which is the motion planning path of the robotic arm. .

To sum up, the embodiment of the present application realizes the bidirectional parallel expansion of the motion planning path through the first planning tree extending from the initial pose to the target pose and the second planning tree extending from the target pose to the initial pose, and In the expansion process, the path cost function is introduced to optimize the planning tree, so that the obtained motion planning path is closer to the optimal path while improving the efficiency and reducing the time-consuming.

It should be understood that the size of the sequence numbers of the steps in the above embodiments does not mean the sequence of execution, and the execution sequence of each process should be determined by its function and internal logic, and should not constitute any limitation to the implementation process of the embodiments of the present application.

Corresponding to the robotic arm motion planning method described in the above embodiment, FIG. 3 shows a structural diagram of an embodiment of a robotic arm motion planning apparatus provided in an embodiment of the present application.

In this embodiment, a robotic arm motion planning device may include:

The random sampling module 301 is used to perform random sampling in the free space of the manipulator to obtain the pose sampling points of the manipulator;

A first expansion point determination module 302, configured to determine a first expansion point corresponding to a first planning tree in the free space according to the pose sampling points; the first planning tree is a preset initial pose The planning tree extended to the preset target pose;

A first planning tree optimization module 303, configured to add the first extension point into the first planning tree according to a preset path cost function, and perform optimization processing on the first planning tree;

The second expansion point determining module 304 is configured to determine a second expansion point corresponding to the second planning tree in the free space according to the first expansion point; the second planning tree is a direction from the target pose to the the expanded planning tree of the initial pose;

A second planning tree optimization module 305, configured to add the second expansion point into the second planning tree according to the path cost function, and perform optimization processing on the second planning tree;

A motion planning path determination module 306, configured to determine the robotic arm according to the first planning tree and the second planning tree when the first planning tree and the second planning tree satisfy a preset coincidence condition motion planning path.

Further, the first planning tree optimization module may include:

Further, the first planning tree optimization module may also include:

Further, the first extension point determination module may include:

Those skilled in the art can clearly understand that, for the convenience and brevity of description, the specific working process of the above-described devices, modules and units can be referred to the corresponding processes in the foregoing method embodiments, which will not be repeated here.

In the foregoing embodiments, the description of each embodiment has its own emphasis. For parts that are not described or described in detail in a certain embodiment, reference may be made to the relevant descriptions of other embodiments.

FIG. 4 shows a schematic block diagram of a robotic arm provided by an embodiment of the present application. For convenience of description, only parts related to the embodiment of the present application are shown.

As shown in FIG. 4 , the robotic arm 4 of this embodiment includes: a processor 40 , a memory 41 , and a computer program 42 stored in the memory 41 and executable on the processor 40 . When the processor 40 executes the computer program 42 , the steps in each of the above embodiments of the robotic arm motion planning method are implemented, for example, steps S101 to S106 shown in FIG. 1 . Alternatively, when the processor 40 executes the computer program 42, the functions of the modules/units in the above device embodiments, for example, the functions of the modules 301 to 306 shown in FIG. 3 are implemented.

Exemplarily, the computer program 42 may be divided into one or more modules/units, and the one or more modules/units are stored in the memory 41 and executed by the processor 40 to complete the this application. The one or more modules/units may be a series of computer program instruction segments capable of performing specific functions, and the instruction segments are used to describe the execution process of the computer program 42 in the robotic arm 4 .

Those skilled in the art can understand that FIG. 4 is only an example of the robot arm 4, and does not constitute a limitation to the robot arm 4. It may include more or less components than the one shown, or combine some components, or different components For example, the robotic arm 4 may further include an input and output device, a network access device, a bus, and the like.

The processor 40 may be a central processing unit (Central Processing Unit, CPU), or other general-purpose processors, digital signal processors (Digital Signal Processor, DSP), application specific integrated circuits (Application Specific Integrated Circuit, ASIC), Field-Programmable Gate Array (FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. A general purpose processor may be a microprocessor or the processor may be any conventional processor or the like.

The storage 41 may be an internal storage unit of the robotic arm 4 , such as a hard disk or a memory of the robotic arm 4 . The memory 41 can also be an external storage device of the robotic arm 4, such as a plug-in hard disk, a smart memory card (Smart Media Card, SMC), a secure digital (Secure Digital, SD) equipped on the robotic arm 4 Card, Flash Card, etc. Further, the memory 41 may also include both an internal storage unit of the robotic arm 4 and an external storage device. The memory 41 is used to store the computer program and other programs and data required by the robotic arm 4 . The memory 41 can also be used to temporarily store data that has been output or will be output.

Those skilled in the art can clearly understand that, for the convenience and simplicity of description, only the division of the above-mentioned functional units and modules is used as an example. Module completion, that is, dividing the internal structure of the device into different functional units or modules to complete all or part of the functions described above. Each functional unit and module in the embodiment may be integrated in one processing unit, or each unit may exist physically alone, or two or more units may be integrated in one unit, and the above-mentioned integrated units may adopt hardware. It can also be realized in the form of software functional units. In addition, the specific names of the functional units and modules are only for the convenience of distinguishing from each other, and are not used to limit the protection scope of the present application. For the specific working processes of the units and modules in the above-mentioned system, reference may be made to the corresponding processes in the foregoing method embodiments, which will not be repeated here.

Those of ordinary skill in the art can realize that the units and algorithm steps of each example described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Skilled artisans may implement the described functionality using different methods for each particular application, but such implementations should not be considered beyond the scope of this application.

In the embodiments provided in this application, it should be understood that the disclosed apparatus/robot and method may be implemented in other ways. For example, the device/manipulator embodiments described above are only illustrative. For example, the division of modules or units is only a logical function division. In actual implementation, there may be other division methods, such as multiple units. Or components may be combined or may be integrated into another system, or some features may be omitted, or not implemented. On the other hand, the shown or discussed mutual coupling or direct coupling or communication connection may be through some interfaces, indirect coupling or communication connection of devices or units, and may be in electrical, mechanical or other forms.

The units described as separate components may or may not be physically separated, and components displayed as units may or may not be physical units, that is, may be located in one place, or may be distributed to multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution in this embodiment.

In addition, each functional unit in each embodiment of the present application may be integrated into one processing unit, or each unit may exist physically alone, or two or more units may be integrated into one unit. The above-mentioned integrated units may be implemented in the form of hardware, or may be implemented in the form of software functional units.

The integrated modules/units, if implemented in the form of software functional units and sold or used as independent products, may be stored in a computer-readable storage medium. Based on this understanding, the present application can implement all or part of the processes in the methods of the above embodiments, and can also be completed by instructing the relevant hardware through a computer program. The computer program can be stored in a computer-readable storage medium, and the computer When the program is executed by the processor, the steps of the foregoing method embodiments can be implemented. Wherein, the computer program includes computer program code, and the computer program code may be in the form of source code, object code, executable file or some intermediate form, and the like. The computer-readable storage medium may include: any entity or device capable of carrying the computer program code, a recording medium, a U disk, a removable hard disk, a magnetic disk, an optical disk, a computer memory, a read-only memory (ROM, Read-Only Memory) ), random access memory (RAM, Random Access Memory), electrical carrier signals, telecommunication signals, and software distribution media, etc. It should be noted that the content contained in the computer-readable storage medium may be appropriately increased or decreased according to the requirements of legislation and patent practice in the jurisdiction, for example, in some jurisdictions, according to legislation and patent practice, computer-readable Storage media exclude electrical carrier signals and telecommunications signals.

The above-mentioned embodiments are only used to illustrate the technical solutions of the present application, but not to limit them; although the present application has been described in detail with reference to the above-mentioned embodiments, those of ordinary skill in the art should understand that: it can still be used for the above-mentioned implementations. The technical solutions described in the examples are modified, or some technical features thereof are equivalently replaced; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions in the embodiments of the application, and should be included in the within the scope of protection of this application.

Claims

A method for motion planning of a robotic arm, comprising:

Perform random sampling in the free space of the robotic arm to obtain the pose sampling points of the robotic arm;

A first expansion point corresponding to a first planning tree is determined in the free space according to the pose sampling points; the first planning tree is a plan extending from a preset initial pose to a preset target pose Tree;

adding the first extension point to the first planning tree according to a preset path cost function, and performing optimization processing on the first planning tree;

A second expansion point corresponding to the second planning tree is determined in the free space according to the first expansion point; the second planning tree is a planning tree expanded from the target pose to the initial pose;

adding the second extension point into the second planning tree according to the path cost function, and performing optimization processing on the second planning tree;

When the first planning tree and the second planning tree satisfy a preset coincidence condition, a motion planning path of the robotic arm is determined according to the first planning tree and the second planning tree.
The robotic arm motion planning method according to claim 1, wherein the adding the first extension point into the first planning tree according to a preset path cost function comprises:

Taking a node whose distance from the first expansion point in the first planning tree is less than a preset distance threshold as the adjacent point of the first expansion point;

Calculate the first path cost corresponding to each adjacent point according to the path cost function; the first path cost is the path cost from the initial pose to the adjacent point and the path cost from the adjacent point to the first extension point the sum of path costs;

The first extension point is added to the first planning tree by taking the adjacent point with the smallest first path cost as the parent node of the first extension point.
The robotic arm motion planning method according to claim 2, wherein the optimizing the first planning tree comprises:

For each adjacent point except the parent node of the first extension point, calculate the second path cost and the third path cost corresponding to the adjacent point according to the path cost function; the second path cost is from The path cost from the initial pose to the adjacent point; the third path cost is the path cost from the initial pose to the first extension point and the path from the adjacent point to the first extension point the sum of the costs;

If the third path cost is less than the second path cost, the first extension point is used as the parent node of the adjacent point.
The robotic arm motion planning method according to claim 1, wherein the path cost function is:

where path is the target path, qn is the nth node on the target path, 1≤n≤N, N is the number of nodes on the target path, and similarity(qn,qn+1) is qn and qn+ Cosine similarity between 1, cost(path) is the path cost of the target path.
The robotic arm motion planning method according to claim 1, wherein the path cost function is:

where path is the target path, qn is the nth node on the target path, 1≤n≤N, N is the number of nodes on the target path, dist(qn,qn+1) is qn and qn+ Euclidean distance between 1, cost(path) is the path cost of the target path.
The robotic arm motion planning method according to claim 1, wherein the path cost function is:

where path is the target path, qn is the nth node on the target path, 1≤n≤N, N is the number of nodes on the target path, and similarity(qn,qn+1) is qn and qn+ The cosine similarity between 1, dist(qn, qn+1) is the Euclidean distance between qn and qn+1, Weight1 and Weight2 are both preset weight coefficients, and cost(path) is the target path. path cost.
The motion planning method for a robotic arm according to any one of claims 1 to 6, wherein the first expansion point corresponding to the first planning tree is determined in the free space according to the pose sampling point ,include:

Taking the node with the shortest distance from the pose sampling point in the first planning tree as the node to be expanded;

Calculate the distance between the pose sampling point and the node to be expanded;

If the distance between the pose sampling point and the node to be expanded is less than or equal to a preset step size, determining the pose sampling point as the first expansion point corresponding to the first planning tree;

If the distance between the pose sampling point and the node to be expanded is greater than the step size, then determine the point on the extension line whose distance from the node to be expanded is the step size as a candidate point ; Described extension line is the connection line between described pose sampling point and described to-be-expanded node;

If the candidate point is in the free space, the candidate point is determined as the first extension point corresponding to the first planning tree.
A robotic arm motion planning device, characterized in that it includes:

The random sampling module is used for random sampling in the free space of the manipulator to obtain the pose sampling points of the manipulator;

A first expansion point determination module, configured to determine a first expansion point corresponding to the first planning tree in the free space according to the pose sampling point; the first planning tree is a direction from a preset initial pose to The extended planning tree of the preset target pose;

a first planning tree optimization module, configured to add the first expansion point into the first planning tree according to a preset path cost function, and perform optimization processing on the first planning tree;

A second expansion point determining module, configured to determine a second expansion point corresponding to the second planning tree in the free space according to the first expansion point; the second planning tree is a direction from the target pose to the Describe the planning tree of the initial pose expansion;

A second planning tree optimization module, configured to add the second expansion point into the second planning tree according to the path cost function, and perform optimization processing on the second planning tree;

A motion planning path determination module, configured to determine the motion of the robotic arm according to the first planning tree and the second planning tree when the first planning tree and the second planning tree satisfy a preset coincidence condition Motion planning path.
A computer-readable storage medium storing a computer program, characterized in that, when the computer program is executed by a processor, the robotic arm motion according to any one of claims 1 to 7 is realized Steps of the planning method.
A robotic arm, comprising a memory, a processor, and a computer program stored in the memory and running on the processor, characterized in that, when the processor executes the computer program, the process according to claim 1 to Steps of the robotic arm motion planning method described in any one of 7.