Obstacle avoidance path planning method, device, storage medium and electronic device

Abstract

The present invention provides an obstacle avoidance path planning method, device, storage medium, and electronic device. The method includes: discretely sampling the joint space of a robotic arm to obtain a number of candidate path nodes; constructing a sparse path map based on the candidate path nodes; updating the sparse path map based on the positions of obstacles detected in real time; and searching for an obstacle avoidance path based on the updated sparse path map, the current position of the robotic arm, and the desired position of the robotic arm. The present invention uses a sparse path map to quickly search for an obstacle avoidance path.

Description

Technical Field

[0001] The embodiments of the present application relate to the field of robotic arms, and in particular to an obstacle avoidance path planning method, device, storage medium, and electronic device. Background Art

[0002] As people's living standards gradually improve, the service industry has experienced rapid development, and the demand for robotic arm operational capabilities has become increasingly clear. Dynamic environments are inevitable in service scenarios. Successfully avoiding dynamic obstacles in dynamic environments while completing tasks is one of the skills that robotic arms must possess to achieve human-machine collaboration.

[0003] In related technologies, the obstacle avoidance path of a robotic arm planned for dynamic obstacles has low path update efficiency and a low obstacle avoidance success rate. Summary of the Invention

[0004] In order to overcome the problems existing in the related art, the present application provides an obstacle avoidance path planning method, device, storage and electronic equipment, which can improve the obstacle avoidance path update efficiency of the robotic arm and improve the obstacle avoidance success rate of the robotic arm.

[0005] According to a first aspect of an embodiment of the present application, a method for obstacle avoidance path planning is provided, comprising the following steps:

[0006] Discretely sampling the joint space of the manipulator to obtain a number of candidate path nodes;

[0007] Constructing a sparse route map based on a plurality of candidate route nodes;

[0008] Update the sparse roadmap based on the locations of obstacles detected in real time;

[0009] An obstacle avoidance path is searched and obtained according to the updated sparse roadmap, the current posture of the robotic arm, and the expected posture of the robotic arm.

[0010] According to a second aspect of an embodiment of the present application, there is provided an obstacle avoidance path planning device, comprising:

[0011] A candidate path node acquisition module is used to discretely sample the joint space of the manipulator to obtain a number of candidate path nodes;

[0012] A mapping relationship determination module, configured to construct a sparse route map based on a plurality of candidate route nodes;

[0013] A roadmap update module is used to update the sparse roadmap based on the locations of obstacles detected in real time;

[0014] The obstacle avoidance path updating module is used to search for an obstacle avoidance path based on the updated sparse roadmap, the current posture of the robotic arm, and the expected posture of the robotic arm.

[0015] According to a third aspect of an embodiment of the present application, an electronic device is provided, comprising a processor and a memory; the memory stores a computer program, and the computer program is suitable for being loaded by the processor and executing the obstacle avoidance path planning method as described above.

[0016] According to a fourth aspect of an embodiment of the present application, a computer-readable storage medium is provided, on which a computer program is stored, characterized in that when the computer program is executed by a processor, the obstacle avoidance path planning method as described above is implemented.

[0017] The embodiment of the present application constructs a sparse roadmap and then quickly updates the sparse roadmap based on the positions of obstacles detected in real time, so as to quickly search and obtain an obstacle avoidance path based on the updated sparse roadmap, thereby improving the search efficiency of the obstacle avoidance path and improving the accuracy of the obstacle avoidance path search.

[0018] It should be understood that the foregoing general description and the following detailed description are exemplary and explanatory only and are not restrictive of the present application.

[0019] For better understanding and implementation, the present invention is described in detail below with reference to the accompanying drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0020] In order to more clearly illustrate the embodiments of the present application or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without any creative work.

[0021] Figure 1 A schematic diagram of an application scenario of the obstacle avoidance path planning method according to an embodiment of the present application;

[0022] Figure 2 This is a flow chart of the obstacle avoidance path planning method shown in the first embodiment of the present application;

[0023] Figure 3 This is a flowchart of a method for obtaining candidate path nodes according to one embodiment of the present application;

[0024] Figure 4 A flowchart of a method for obtaining a sparse roadmap according to an embodiment of the present application;

[0025] Figure 5A schematic diagram of the structure of four points for constructing a sparse roadmap according to an embodiment of the present application;

[0026] Figure 6 A flowchart of a method for updating a sparse roadmap according to an embodiment of the present application is shown;

[0027] Figure 7 A schematic diagram of the mapping relationship between a sparse roadmap and a robotic arm workspace according to an embodiment of the present application;

[0028] Figure 8 This is a flowchart of a method for updating a sparse roadmap based on a mapping relationship between the sparse roadmap and a robot workspace, according to one embodiment of the present application;

[0029] Figure 9 A schematic diagram illustrating the principle of updating a sparse roadmap according to an embodiment of the present application;

[0030] Figure 10 This is a flowchart of a method for obtaining an obstacle avoidance path for a robotic arm, shown in one embodiment of the present application;

[0031] Figure 11 This is a schematic block diagram of an obstacle avoidance path planning device according to a second embodiment of the present application;

[0032] Figure 12 This is a schematic structural diagram of an electronic device according to the third embodiment of the present application. DETAILED DESCRIPTION

[0033] To make the objectives, technical solutions, and advantages of this application more clear, the embodiments of this application will be further described in detail below with reference to the accompanying drawings. In the following description, when referring to the accompanying drawings, unless otherwise indicated, the same numbers in different drawings represent the same or similar elements.

[0034] It should be understood that the embodiments described in the following examples do not represent all embodiments consistent with this application. Rather, they are merely examples of devices and methods consistent with certain aspects of this application, as detailed in the appended claims. All other embodiments derived by persons of ordinary skill in the art based on the embodiments of this application without inventive effort are intended to fall within the scope of protection of this application.

[0035] The terms used in this application are for the purpose of describing specific embodiments only and are not intended to limit this application. The singular forms of "a", "the" and "the" used in this application are also intended to include plural forms, unless the context clearly indicates otherwise. In addition, in the description of this application, unless otherwise stated, "a plurality" refers to two or more. It should also be understood that the term "and / or" used herein refers to and includes any or all possible combinations of one or more associated listed items, for example, A and / or B can represent: A exists alone, A and B exist at the same time, and B exists alone; the character " / " generally indicates that the objects associated before and after are in an "or" relationship.

[0036] It should be understood that although the terms first, second, third, etc. may be used in this application to describe various information, this information should not be limited to these terms. Moreover, these terms are only used to distinguish similar objects, and are not necessarily used to describe a specific order or sequence, nor can they be understood to indicate or imply relative importance. For those of ordinary skill in the art, the specific meanings of the above terms in this application can be understood according to the specific circumstances. Depending on the context, the words "if" / "if" used in this application can be interpreted as "at the time of" or "when" or "in response to determining".

[0037] See also Figure 1 , which is a schematic diagram of an application scenario of the obstacle avoidance path planning method of an embodiment of the present application. The application scenario of the obstacle avoidance path planning method of the embodiment of the present application includes a robotic arm 10; the robotic arm 10 can be fixed on a base and work in collaboration with a person 20. Optionally, the base can be movable, and the robotic arm 10 can move with the movement of the base in addition to its own movement; optionally, the base can also be fixed, and the robotic arm 10 only has its own movement. The embodiment of the present application takes the case where the base is fixed and only the robotic arm 10 moves as an example to illustrate the obstacle avoidance path planning method of the present application.

[0038] The robotic arm 10 includes multiple joints. A joint is a device that connects two components. These connections are not fixed, but rather allow for limited relative motion. Optionally, the motion may include rotation and translation. The robotic arm 10 achieves its own motion by controlling the movement of each joint.

[0039] The robotic arm 10 also includes one or more processors; the processor can be used to execute the obstacle avoidance path planning method of the present application, control the movement of each joint, and thereby drive the movement of the robotic arm 10.

[0040] Alternatively, the processor can be built into the robotic arm 10 and function as a whole with the robotic arm 10; or the processor can be external to the robotic arm 10 to independently control the movement of the robotic arm 10. Alternatively, the processor can simply control the movement of each joint. In other words, the obstacle avoidance path planning method of the embodiment of the present application can also be executed by another processing center connected to the processor, which then transmits the planned obstacle avoidance path to the processor, which then further controls the movement of each joint.

[0041] The following will be combined with the Figures 2 to 9 , the obstacle avoidance path planning method provided in the embodiment of the present application is introduced in detail.

[0042] See also Figure 2 The obstacle avoidance path planning method provided in the embodiment of the present application includes the following steps:

[0043] Step S101: discretely sample the joint space of the robot arm to obtain several candidate path nodes.

[0044] A robotic arm can consist of n joints, where n is an integer greater than or equal to 1, and each joint has one degree of freedom. The end-of-arm pose is determined by the joint states of these n joints, which can be specifically expressed as n-dimensional joint vector parameters. The space formed by all these joint vector parameters is called the joint space of the robotic arm.

[0045] The joint vector parameters can include joint angles. This means that the joint angles of each joint are used as the joint states of each joint, and are recorded as a set of joint states. The combination of these joint states can determine the end-of-arm pose. All possible combinations of the joint angles of each joint, or all possible combinations of joint states, constitute the joint space of the manipulator.

[0046] For example, for a robotic arm with 6 joints and n=6, the end pose of the robotic arm can be determined by the combination of the joint angles of the 6 joints. The joint angle combination can be configured as a vector J=(j1, j2, j3, j4, j5, j6), where j1 is the joint angle of the 1st joint, j2 is the joint angle of the 2nd joint, j3 is the joint angle of the 3rd joint, j4 is the joint angle of the 4th joint, j5 is the joint angle of the 5th joint, and j6 is the joint angle of the 6th joint. It can be understood that J=(j1, j2, j3, j4, j5, j6) can determine the end pose of the robotic arm, and for all possible combinations of the 6 joint angles, it is the joint space of the robotic arm.

[0047] It is understood that the space formed by all joint vector parameters in the embodiments of the present application can be all possible combinations of the joint angles of each joint; and all possible combinations of the joint angles of each joint can correspond to multiple pose nodes. In an optional embodiment, the joint space of the manipulator is discretely sampled to obtain a number of candidate path nodes. This can be achieved by discretely sampling all possible combinations of the joint angles of each joint, that is, discretely sampling the corresponding multiple pose nodes, and then using the points corresponding to the discrete sampling as the number of candidate path nodes.

[0048] Step S102: construct a sparse route map based on a number of candidate route nodes.

[0049] Optionally, a method for constructing a sparse route map based on a number of candidate path nodes may be to use a sparse asymptotic optimal method or other sparse route construction methods.

[0050] Among them, the sparse asymptotic optimal method is a method for determining the optimal route map by considering the position of each sparse candidate path node and various connection structures. Specifically, the sparse asymptotic optimal method is: traversing the candidate path nodes one by one, judging whether to further select the current candidate path node and the corresponding path based on the candidate paths and paths that have been determined to be selected, and thus constructing a sparse route map based on the candidate path nodes and the corresponding paths that have been determined to be selected. Among them, constructing a sparse route map in accordance with the sparse asymptotic optimal method can achieve the completeness, sparsity and asymptotic approach to optimality of the sparse route map, thereby further improving the search efficiency of the obstacle avoidance path and improving the accuracy of the obstacle avoidance path search.

[0051] Step S103: Update the sparse roadmap according to the positions of obstacles detected by the robotic arm in real time.

[0052] In an optional embodiment, several depth cameras can be set around the robot arm. Based on the images of the robot arm's periphery taken in real time by the several depth cameras, the position of obstacles can be determined in real time, and then the paths that interfere with the obstacles in the sparse route map can be eliminated to update the sparse route map.

[0053] Step S104: Search and obtain an obstacle avoidance path based on the updated sparse roadmap, the current posture of the robotic arm, and the expected posture of the robotic arm.

[0054] The posture is used to indicate the position of the robot arm and the joint state of each joint of the robot arm. It is understandable that the current posture of the robot arm in the embodiment of the present application is the current position of the current robot arm and the joint state of each joint of the current robot arm. The expected posture of the robot arm is the final position of the robot arm and the final joint state of each joint of the robot arm when performing a task action based on the task that the robot arm needs to perform. It should be understood that for a task with reciprocating work, the starting posture and ending posture of each reciprocating motion are used as the expected posture of the robot arm.

[0055] Optionally, when the robotic arm is in an offline state, discrete sampling is performed on the joint space of the robotic arm to obtain several candidate path nodes, and a sparse roadmap is constructed based on the several candidate path nodes; when the robotic arm is in an online state, the sparse roadmap is updated according to the positions of obstacles detected by the robotic arm in real time, and an obstacle avoidance path is searched and obtained based on the updated sparse roadmap, the current posture of the robotic arm, and the expected posture of the robotic arm.

[0056] It can be understood that the offline state is the state when the robotic arm is not performing related tasks, and the online state is the state when the robotic arm is performing related tasks.

[0057] In the embodiment of the present application, a sparse roadmap is constructed when the robotic arm is in an offline state, and the sparse roadmap is updated when the robotic arm is in an online state to search for an obstacle avoidance path. This can improve the working efficiency of the robotic arm and avoid the situation where the robotic arm cannot work in time because a sparse roadmap needs to be constructed first after it is turned on.

[0058] The embodiment of the present application constructs a sparse roadmap and then quickly updates the sparse roadmap based on the positions of obstacles detected in real time, so as to quickly search and obtain an obstacle avoidance path based on the updated sparse roadmap, thereby improving the search efficiency of the obstacle avoidance path and improving the accuracy of the obstacle avoidance path search.

[0059] See also Figure 3 In an optional embodiment, the step of discretely sampling the joint space of the manipulator to obtain a plurality of candidate path nodes in step S101 includes:

[0060] Step S1011: Obtain a discretization step size according to a preset observation distance of a path node and a penetration distance of an adjacent path node.

[0061] Among them, the observation distance of the path node is the distance calculated by Euclidean distance. Specifically, in two-dimensional space, the observation distance of the path node is used to indicate a circular observation area generated with the path node as the center and a preset length as the radius; in three-dimensional space, the observation distance of the path node is used to indicate a spherical observation area generated with the path node as the center and a preset length as the radius. Among them, the radius, that is, the preset length, can be selected according to actual needs. It can be understood that the larger the radius, the larger the discretization step size, and the sparser the obtained roadmap. Conversely, the smaller the radius, the smaller the discretization step size, and the denser the obtained sparse roadmap.

[0062] The penetration distance between adjacent path nodes is calculated as the Euclidean distance. Specifically, in two-dimensional space, two path nodes correspond to two circular observation areas. When the distance between two path nodes is less than twice the observation distance, their two circular observation areas overlap. The Euclidean length of the overlapping area is the penetration distance. Specifically, the penetration distance is calculated as: penetration distance = 2 * observation distance - Euclidean distance between two path nodes. It is understood that the definition and calculation of the penetration distance in three-dimensional space are the same as those in two-dimensional space and will not be elaborated here.

[0063] Specifically, for the sparse path graph to be constructed, the observation distance of each path node in the sparse path graph is preset to Δ, and the area within Δ is regarded as the visible area of ​​the path node. Considering that according to the sparse asymptotic optimal method, when constructing the sparse path graph, the visible areas of two path nodes that can form a connecting path must have mutual penetration, then the maximum distance d between two path nodes that can form a connecting path is max For: d max =2*Δ.

[0064] The discretization operation divides the joint space of the three-dimensional robot arm into small cubes. The eight vertices of each small cube are recorded as the same discrete area. The maximum distance d′ of the path nodes in the same discrete area can be obtained by calculating the Manhattan distance. max :d′ max =n*β. β is the discretization step size; n represents the degree of freedom of the robot arm, that is, the number of joints of the robot arm.

[0065] In order to improve the roadmap coverage, the embodiment of the present application considers that each path node in the same discrete area can establish an effective connection. Combined with the penetration distance ψ, the discrete step length β can be obtained as:

[0066] n*(β+ψ)=2*Δ

[0067]

[0068] Step S1012: discretely sample the joint space of the robotic arm according to the discretization step size to obtain a number of candidate path nodes.

[0069] The embodiment of the present application obtains a discretized step size based on the observation distance of a preset path node and the penetration distance of an adjacent path node; based on the discretized step size, the joint space of the robotic arm is discretely sampled to obtain several candidate path nodes, which can improve the coverage rate of the sparse path graph.

[0070] In an optional embodiment, after the step of discretely sampling the joint space of the robot arm according to the discretization step size to obtain a number of candidate path nodes in step S1012, the method includes: randomly sampling the joint space of the robot arm, and adding the randomly sampled nodes as candidate path nodes.

[0071] Considering the candidate path nodes obtained by discretization, constructing a sparse roadmap can effectively improve its coverage. However, the density of the constructed sparse roadmap is low near obstacles, especially in narrow channels. Therefore, the embodiment of the present application combines random sampling to sample several nodes in the joint space to enter the candidate path nodes to construct a sparse roadmap, thereby further improving the coverage of the sparse route.

[0072] See also Figure 4 In an optional embodiment, step S102 of constructing a sparse roadmap based on a plurality of candidate path nodes includes:

[0073] Step S1021: Initialize the route map and traverse each candidate path node.

[0074] For an initial roadmap G S (V S ,E S ), where V S represents the set of all nodes in the graph, each node represents a set of joint states of the robotic arm, E S Represents all edges, that is, the set of paths. d(q1,q2) represents the distance between nodes q1 and q2, L(q1,q2) represents the local path from q1 to q2, and in the embodiment of the present application, the local path is marked by a straight line connection. In order to ensure the sparsity of the map, the observation distance Δ of the path node is set, and the area within Δ is regarded as the visible area of ​​the node. It can be understood that the node observation distance is determined by the sparsity. Note C free For free space, meet The state of the robotic arm corresponding to node q does not interfere with the obstacle.

[0075] Step S1022: If there is no point in the route map whose distance to the current candidate path node is less than the observed distance of the path node, the current candidate path node is added to the route map as a path node.

[0076] Specifically, Figure 5 As shown in (a), for the current candidate path node q, the search point set on the roadmap GS satisfy: L(q,v)∈C free And d(q,v)<Δ. At this time, if W=Φ, then the current candidate path node q is an isolated point, and the current candidate path node q is added to V S .

[0077] Step S1023: If in the route map, there are two points that are not in the same connected area and the distance between them and the current candidate path node is less than the observation distance of the path node, and there is a connecting path connecting the current candidate path node with the two points that are not in the same connected area, then the current candidate path node is used as the path node, and the connecting path between the current candidate path node and the two points that are not in the same connected area is used as the route path and added to the route map.

[0078] Figure 5 As shown in (b), for the current candidate path node q, if the corresponding set W If v and v′ are not in the same connected area (v and v′ have no direct or indirect connection), then the current candidate path node q is used as the connection point and the current candidate path node q is added to V S , construct a straight line connection between nodes q, v, v′ (corresponding to all nodes in non-same connected areas that have been stored) and add them to E S middle.

[0079] Step S1024: If in the route map, for the two points closest to the current candidate path node, when the two points are in the same connected area, the distances from the current candidate path node are less than the observation distance of the path node, and the connection path of the two points is valid, and the connection path of the two points has not been added to the route map, the connection path of the two points is used as the route path and added to the route map; when the two points are not in the same connected area, but there is a connection path connecting the current candidate path node with the two points, the candidate path node is used as the path node, and the connection path between the candidate path node and the two points is used as the route path and added to the route map.

[0080] The connection path of the two points effectively indicates that the two points are non-interfering and are not joint limited.

[0081] Figure 5 As shown in (c), the condition for constructing the existing node set W is relaxed, and satisfy: d(q,n)<Δ. Select two nodes v and v′ closest to node q from N. If L(v,v′)∈C free and Then add L(v,v′) to E S ; On the contrary, if L(q,v)∈C free And L(q,v′)∈C free , then the current candidate path node q is taken as the interface point, and the current candidate path node q is added to V S , construct the paths L(q,v) and L(q,v′) of nodes q, v, v′ and add them to E S middle.

[0082] Step S1025: If in the route map, if the current candidate path node connects two points that do not have a direct connection, or if the current candidate path node achieves a better connection between the two points, the current candidate path node is used as a candidate path point, and the connection path between the current candidate path node and the two points that do not have a direct connection is used as a route path and added to the route map.

[0083] Optionally, a better connection is used to indicate a shorter path between two points.

[0084] Figure 5 As shown in (d), V S The path node v has an intersection with the visible areas of v′ and v″, and However, there is no direct connection between v′ and v″. If the current candidate path node can achieve a direct connection between path nodes v′ and v″ or a better connection, then the current candidate path node is used as the optimization point and added to V S , take the direct connection or better connection path between the current candidate path node and the path nodes v′ and v″ as the route path and add it to E S .

[0085] Step S1026: Obtain a sparse route map based on the path nodes and line paths in the route map.

[0086] The embodiment of the present application constructs a sparse roadmap by constructing four path node introduction methods, so that each time a new current candidate path node is added, the relationship between the new current candidate path node and the previously added candidate path node is fully considered to determine whether to introduce the current candidate path node to construct the sparse roadmap, thereby improving the completeness, sparsity and asymptotic approach to optimality of the sparse roadmap.

[0087] See also Figure 6In an optional embodiment, the step of updating the sparse roadmap according to the positions of obstacles detected in real time in step S103 includes:

[0088] Step S1031: establishing a mapping relationship between the sparse roadmap and the robot arm workspace;

[0089] Step S1032: updating the sparse roadmap according to the position of the obstacle in the manipulator workspace detected in real time and the mapping relationship between the sparse roadmap and the manipulator workspace.

[0090] In a dynamic environment, the sparse roadmap constructed offline is not always valid due to the possible presence of obstacles. Therefore, an online query is required to update the sparse roadmap constructed offline. In the sparse roadmap, the position of obstacles in the sparse roadmap can be determined by traversing the sparse roadmap, but this method is time-consuming. Therefore, in order to quickly obtain invalid paths that interfere with obstacles, the embodiment of the present application establishes a mapping relationship between the sparse roadmap and the robot arm workspace.

[0091] The robot's workspace is the area the end of the robot can reach. By quickly and accurately identifying obstacles in a dynamic environment within the robot's workspace, the sparse pathmap is mapped to the robot's workspace, eliminating paths that interfere with obstacles in the sparse pathmap to update the sparse pathmap.

[0092] Specifically, the robot workspace is rasterized to obtain the robot grid map, and then a multi-map (multi-segment map) method is used to establish the mapping relationship between each grid point in the robot grid map and each path in the sparse route map. Figure 7 As shown, the path v in the sparse roadmap on the left is i and v j Corresponding right Figure 1 A gray swept area.

[0093] In an optional embodiment, several depth cameras can be set around the robot arm. According to the real-time images of the robot arm's periphery taken by the several depth cameras and the rasterized robot arm workspace, the position of the detected obstacle in the robot arm space can be determined in real time, and then the mapping relationship between the sparse roadmap and the robot arm workspace is used to determine the position of the obstacle in the sparse roadmap. Then, through the mapping relationship between the sparse roadmap and the robot arm workspace, the paths that interfere with the obstacles in the sparse roadmap are excluded to update the sparse roadmap.

[0094] See also Figure 8In an optional embodiment, the step of updating the sparse roadmap in step S1032 according to the real-time detected position of the obstacle in the manipulator workspace and the mapping relationship between the sparse roadmap and the manipulator workspace includes:

[0095] Step S10321: According to the real-time detected position of the obstacle in the manipulator workspace and the mapping relationship between the sparse pathmap and the manipulator workspace, determine the paths and path nodes in the sparse pathmap that interfere with the obstacle.

[0096] Step S10322: Delete the paths and path nodes that interfere with obstacles in the sparse route map, and update the sparse route map.

[0097] like Figure 9 As shown, for the area where there are obstacles in the grid map, the path node v corresponding to the sparse road map i and v j Therefore, the invalid path nodes v that interfere with obstacles in the sparse roadmap can be i and v j and path node v i and v j The paths between them are deleted, thereby updating the sparse route map.

[0098] The embodiment of the present application determines the paths and path nodes in the sparse roadmap that interfere with obstacles based on the real-time detected positions of obstacles in the workspace of the robotic arm and the mapping relationship between the sparse roadmap and the workspace of the robotic arm, and then updates the sparse roadmap, which can improve the updating efficiency of the sparse roadmap and thus improve the updating efficiency of the obstacle avoidance path.

[0099] See also Figure 10 In an optional embodiment, the step of searching for an obstacle avoidance path based on the updated sparse roadmap, the current position of the manipulator, and the desired position of the manipulator in step S104 includes:

[0100] Step S1041: In the updated sparse roadmap, according to the current posture of the robotic arm and the expected posture of the robotic arm, a first path search algorithm is used through the first thread to search for an obstacle avoidance path for the robotic arm.

[0101] Step S1042: In the updated sparse roadmap, according to the current posture of the robotic arm and the expected posture of the robotic arm, a second path search algorithm is used through the second thread to search for an obstacle avoidance path for the robotic arm.

[0102] Step S1043: In the first thread and the second thread, when one of the threads searches for and obtains the robot arm obstacle avoidance path, the operation of the other thread is stopped, and the robot arm obstacle avoidance path searched and obtained by one of the threads is used as the planned robot arm obstacle avoidance path.

[0103] The first path search algorithm may be an A* algorithm to search for an effective shortest path, and the second path search algorithm may be a sampling-based algorithm, for example, an RRT_Connect algorithm. Figure 8 As shown, at the path node q start and path node q goal The bold black line between them is the obstacle avoidance path obtained through the search. This embodiment of the application uses two threads to synchronously plan the robot's obstacle avoidance path. If either thread prioritizes planning a path, the other thread is stopped. This application improves planning efficiency and probability completeness based on the robot's current position and the robot's desired position.

[0104] Optionally, in order to further achieve the accuracy of the obstacle avoidance path, after searching and obtaining the robot arm obstacle avoidance path, the searched robot arm obstacle avoidance path is preprocessed by pruning and smoothing, and then output as the planned robot arm obstacle avoidance path.

[0105] See also Figure 11 , which is a schematic diagram of the structure of the obstacle avoidance planning device provided in the second embodiment of the present application. The device 200 includes:

[0106] The candidate path node acquisition module 201 is used to perform discrete sampling on the joint space of the manipulator to obtain a number of candidate path nodes;

[0107] A mapping relationship determination module 202 is configured to construct a sparse route map based on a number of candidate route nodes;

[0108] A roadmap updating module 203 is configured to update the sparse roadmap according to the positions of obstacles detected in real time;

[0109] The obstacle avoidance path acquisition module 204 is used to search for and obtain an obstacle avoidance path based on the updated sparse roadmap, the current position of the robotic arm, and the desired position of the robotic arm.

[0110] In an optional embodiment, the mapping relationship determination module 202 includes:

[0111] A discretization step length acquisition module is used to obtain a discretization step length according to a preset observation distance of a path node and a penetration distance of a visible area of ​​an adjacent path node;

[0112] The node determination module is used to perform discrete sampling on the joint space of the robot arm according to the discretization step size to obtain several candidate path nodes.

[0113] In an optional embodiment, the mapping relationship determination module 202 includes:

[0114] The roadmap initialization module initializes the roadmap and traverses each candidate path node.

[0115] The first path node determination module is used to add the current candidate path node as a path node to the route map if there is no point in the route map whose distance to the current candidate path node is less than the observed distance of the path node.

[0116] The second path node determination module is used to determine the current candidate path node as a path node and the connection path between the current candidate path node and the two points that are not in the same connected area as a route path, and add them to the route map if the distances between the two points in the route map and the current candidate path node are both less than the observation distance of the path node, and there is a connection path connecting the current candidate path node with the two points that are not in the same connected area.

[0117] The third path node determination module is used to, if in the route map, for the two points closest to the current candidate path node, when the two points are in the same connected area, the distances from the current candidate path node are both less than the observation distance of the path node, and the connection path of the two points is valid, and the connection path of the two points has not been added to the route map, take the connection path of the two points as the route path and add it to the route map; when the two points are not in the same connected area, but there is a connection path connecting the current candidate path node with the two points, take the candidate path node as the path node, and take the connection path between the candidate path node and the two points as the route path and add it to the route map.

[0118] The fourth path node determination module is used to, if in the route map, if the current candidate path node connects two points that do not have a direct connection, or the current candidate path node achieves a better connection between the two points, use the current candidate path node as a candidate path point, and use the connection path between the current candidate path node and the two points that do not have a direct connection as a route path, and add it to the route map.

[0119] The sparse roadmap construction module is used to obtain a sparse roadmap according to the path nodes and line paths in the roadmap.

[0120] In an optional embodiment, the roadmap updating module 203 includes:

[0121] A mapping relationship determination module, used to establish a mapping relationship between the sparse roadmap and the robotic arm workspace;

[0122] The roadmap acquisition module is used to update the sparse roadmap according to the position of the obstacle in the working space of the manipulator detected in real time and the mapping relationship between the sparse roadmap and the working space of the manipulator.

[0123] In an optional embodiment, the roadmap acquisition module includes:

[0124] The obstacle determination module is used to determine the paths and path nodes in the sparse roadmap that interfere with obstacles based on the real-time detected positions of obstacles in the robot arm workspace and the mapping relationship between the sparse roadmap and the robot arm workspace.

[0125] The route update module is used to delete the paths and path nodes that interfere with obstacles in the sparse route map and update the sparse route map.

[0126] In an optional embodiment, the obstacle avoidance path acquisition module 204:

[0127] The first path acquisition module is used to search for an obstacle avoidance path for the robotic arm by using a first path search algorithm through a first thread according to the current posture of the robotic arm and the expected posture of the robotic arm in the updated sparse roadmap.

[0128] The second path acquisition module is used to search for an obstacle avoidance path for the robotic arm by using a second path search algorithm through a second thread according to the current posture of the robotic arm and the expected posture of the robotic arm in the updated sparse roadmap.

[0129] The obstacle avoidance path determination module is used to stop the work of the other thread in the first thread and the second thread when one of the threads searches for the robot arm obstacle avoidance path, and use the robot arm obstacle avoidance path searched by one of the threads as the planned robot arm obstacle avoidance path.

[0130] It should be noted that the obstacle avoidance path planning device provided in the second embodiment of the present application only uses the division of the above-mentioned functional modules as an example when executing the obstacle avoidance path planning method. In actual applications, the above-mentioned functions can be assigned to different functional modules as needed, that is, the internal structure of the device can be divided into different functional modules to complete all or part of the functions described above. In addition, the obstacle avoidance path planning device provided in the second embodiment of the present application and the obstacle avoidance path planning method of the first embodiment of the present application are of the same concept. The implementation process thereof is detailed in the method embodiment and will not be repeated here.

[0131] See also Figure 12 , is a schematic diagram of the structure of a computer device provided in the third embodiment of this application. Figure 12As shown, the electronic device 300 can be specifically a computer, a mobile phone, a tablet computer, an interactive tablet or a processing device in a robotic arm, etc. The electronic device 300 may include: at least one processor 301, at least one memory 302, at least one display 303, at least one network interface 304, a user interface 305 and at least one communication bus 306.

[0132] The communication bus 306 is used to implement the connection and communication between these components.

[0133] The user interface 305 may include a display screen and a camera; the user interface 305 may also include a standard wired interface and a wireless interface.

[0134] The network interface 304 may optionally include a standard wired interface and a wireless interface (such as a WI-FI interface).

[0135] The processor 301 may include one or more processing cores. The processor 301 utilizes various interfaces and circuits to connect the various components within the entire electronic device 300. By running or executing instructions, programs, code sets, or instruction sets stored in the memory 302, and calling data stored in the memory 302, the processor 301 performs various functions of the electronic device 300 and processes data. Optionally, the processor 301 may be implemented in the form of at least one hardware component selected from the group consisting of a digital signal processing (DSP), a field-programmable gate array (FPGA), and a programmable logic array (PLA). The processor 301 may integrate one or a combination of a central processing unit (CPU), a graphics processing unit (GPU), and a modem. The CPU primarily processes the operating system, user interface, and application programs; the GPU is responsible for rendering and drawing the content required to be displayed by the display layer; and the modem is used to handle wireless communications. It is understood that the modem may not be integrated into the processor 301 and may be implemented separately on a single chip.

[0136] Among them, the memory 302 may include a random access memory (RAM) or a read-only memory (Read-Only Memory). Optionally, the memory 302 includes a non-transitory computer-readable storage medium. The memory 302 can be used to store instructions, programs, codes, code sets or instruction sets. The memory 302 may include a program storage area and a data storage area, wherein the program storage area may store instructions for implementing an operating system, instructions for at least one function (such as a touch function, a sound playback function, an image playback function, etc.), instructions for implementing the above-mentioned various method embodiments, etc.; the data storage area may store data involved in the above-mentioned various method embodiments, etc. The memory 302 may also be optionally at least one storage device located away from the aforementioned processor 301. As Figure 12 As shown, the memory 302 as a computer storage medium may include an operating system, a network communication module, and a user.

[0137] exist Figure 12 In the electronic device 300 shown, the user interface 305 is mainly used to provide an input interface for the user and obtain data input by the user; and the processor 301 can be used to call the operating application stored in the memory 302, such as: the application of the obstacle avoidance planning method; and execute the relevant operations of any obstacle avoidance planning method in the above-mentioned embodiments, with corresponding functions and beneficial effects.

[0138] A fourth embodiment of the present application further provides a computer-readable storage medium storing a computer program, which contains instructions suitable for being loaded by a processor and executing the steps of the obstacle avoidance planning method described above. The specific execution process can be referred to the specific description of the embodiment and is not further described here. The device containing the storage medium can be a personal computer, laptop computer, smartphone, tablet computer, or other electronic device.

[0139] For the device embodiments, since they basically correspond to the method embodiments, the relevant parts can be referred to the partial description of the method embodiments. The device embodiments described above are merely illustrative, wherein the components described as separate parts may or may not be physically separated, and the parts shown as units may or may not be physical units, that is, they may be located in one place, or they may be distributed on multiple network units. Some or all of the modules can be selected according to actual needs to achieve the purpose of the present application scheme. A person of ordinary skill in the art can understand and implement it without paying any creative work.

[0140] Those skilled in the art will appreciate that the embodiments of the present application can be provided as methods, systems, or computer program products. Therefore, the present application can adopt the form of a complete hardware embodiment, a complete software embodiment, or an embodiment in combination with software and hardware. Moreover, the present application can adopt the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to magnetic disk storage, CD-ROM, optical storage, etc.) that contain computer-usable program code.

[0141] The present application is described with reference to the flowcharts and / or block diagrams of the methods, devices (systems), and computer program products according to the embodiments of the present application. It should be understood that each process and / or box in the flowchart and / or block diagram, as well as the combination of the processes and / or boxes in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the steps in the process. Figure 1 a process or multiple processes and / or boxes Figure 1 These computer program instructions can also be stored in a computer-readable memory that can guide a computer or other programmable data processing device to work in a specific way, so that the instructions stored in the computer-readable memory produce a product including an instruction device, which implements the functions selected in the process. Figure 1 a process or multiple processes and / or boxes Figure 1 function selected in a box or multiple boxes.

[0142] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operational steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing the instructions executed on the computer or other programmable device for implementing the process. Figure 1 a process or multiple processes and / or boxes Figure 1 steps for the function selected in a box or multiple boxes.

[0143] In a typical configuration, a computing device includes one or more processors (CPUs), input / output interfaces, network interfaces, and memory.

[0144] The memory may include non-permanent memory in a computer-readable medium, random access memory (RAM) and / or non-volatile memory in the form of read-only memory (ROM) or flash RAM. The memory is an example of a computer-readable medium.

[0145] Computer-readable media includes permanent and non-permanent, removable and non-removable media that can be implemented by any method or technology to store information. The information can be computer-readable instructions, data structures, program modules or other data. Examples of computer storage media include, but are not limited to, phase change memory (PRAM), static random access memory (SRAM), dynamic random access memory (DRAM), other types of random access memory (RAM), read-only memory (ROM), electrically erasable programmable read-only memory (EEPROM), flash memory or other memory technology, compact disc read-only memory (CD-ROM), digital versatile disc (DVD) or other optical storage, magnetic cassettes, magnetic tape, magnetic disk storage or other magnetic storage devices or any other non-transmission media that can be used to store information that can be accessed by a computing device. As defined herein, computer-readable media does not include transitory computer-readable media (transitory media), such as modulated data signals and carrier waves.

[0146] It should also be noted that the terms "comprises," "includes," or any other variations thereof are intended to encompass non-exclusive inclusion, such that a process, method, commodity, or apparatus that includes a series of elements includes not only those elements but also other elements not explicitly listed, or includes elements inherent to such process, method, commodity, or apparatus. In the absence of further limitations, an element defined by the phrase "comprises a ..." does not exclude the presence of other identical elements in the process, method, commodity, or apparatus that includes the element.

[0147] The above are merely embodiments of the present application and are not intended to limit the present application. For those skilled in the art, the present application may have various changes and variations. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principles of the present application should all be included within the scope of the claims of the present application.

Claims

1. An obstacle avoidance path planning method, characterized in that: The steps include: Discretely sample the joint space of the robot arm to obtain several candidate path nodes; Constructing a sparse route map based on a plurality of candidate route nodes; Update the sparse roadmap based on the locations of obstacles detected in real time; Searching for an obstacle avoidance path based on the updated sparse roadmap, the current posture of the robotic arm, and the desired posture of the robotic arm; The step of discretely sampling the joint space of the manipulator to obtain a plurality of candidate path nodes includes: The discretization step size is obtained according to the preset observation distance of the path node and the penetration distance of the adjacent path nodes; According to the discretization step size, the joint space of the manipulator is discretely sampled to obtain a number of candidate path nodes.

2. The obstacle avoidance path planning method according to claim 1, characterized in that: When the robotic arm is in an offline state, discrete sampling is performed on the joint space of the robotic arm to obtain a plurality of candidate path nodes; and a sparse roadmap is constructed based on the plurality of candidate path nodes; When the robotic arm is in an online state, updating a sparse roadmap according to the positions of obstacles detected in real time; An obstacle avoidance path is searched and obtained according to the updated sparse roadmap, the current posture of the robotic arm, and the expected posture of the robotic arm.

3. The obstacle avoidance path planning method according to claim 1 or 2, characterized in that: After the step of discretely sampling the joint space of the manipulator according to the discretization step size to obtain a plurality of candidate path nodes, the method further includes: The joint space of the robotic arm is randomly sampled, and the nodes obtained by random sampling are added as the candidate path nodes.

4. The obstacle avoidance path planning method according to claim 1 or 2, characterized in that: The step of constructing a sparse roadmap based on a plurality of candidate path nodes includes: Initialize the route map and traverse each of the candidate path nodes; If there is no point in the route map whose distance to the current candidate path node is less than the observed distance of the path node, then the current candidate path node is added to the route map as a path node; If, in the route map, the distances between two points that are not in the same connected area and the current candidate path node are both smaller than the observed distance of the path node, and there is a connecting path connecting the current candidate path node and the two points that are not in the same connected area, then the current candidate path node is taken as the path node, and the connecting path between the current candidate path node and the two points that are not in the same connected area is taken as the route path and added to the route map; If, in the route map, for the two points closest to the current candidate path node, when the two points are in the same connected area, the distances to the current candidate path node are both less than the observed distance of the path node, and the connection path between the two points is valid, and the connection path between the two points has not been added to the route map, the connection path between the two points is used as the route path and added to the route map; if the two points are not in the same connected area, but there is a connection path connecting the current candidate path node and the two points, the candidate path node is used as the path node, and the connection path between the candidate path node and the two points is used as the route path and added to the route map; If, in the route map, the current candidate path node connects two points that are not directly connected, or the current candidate path node achieves a better connection between the two points, the current candidate path node is used as a candidate path point, and the connection path between the current candidate path node and the two points that are not directly connected is used as a route path and added to the route map; A sparse route map is obtained according to the route nodes and line paths in the route map.

5. The obstacle avoidance path planning method according to claim 1 or 2, characterized in that: The step of updating the sparse roadmap according to the positions of obstacles detected in real time includes: Establishing a mapping relationship between the sparse roadmap and the robotic arm workspace; The sparse roadmap is updated according to the position of the obstacle detected in real time in the workspace of the manipulator and the mapping relationship between the sparse roadmap and the workspace of the manipulator.

6. The obstacle avoidance path planning method according to claim 5, characterized in that: The step of updating the sparse roadmap according to the position of the obstacle detected in real time in the manipulator workspace and the mapping relationship between the sparse roadmap and the manipulator workspace includes: Determine, based on the real-time detected position of the obstacle in the manipulator workspace and the mapping relationship between the sparse pathmap and the manipulator workspace, the paths and path nodes in the sparse pathmap that interfere with the obstacle; The paths and path nodes that interfere with obstacles are deleted from the sparse route map, and the sparse route map is updated.

7. The obstacle avoidance path planning method according to claim 1 or 2, characterized in that: The step of searching for an obstacle avoidance path based on the updated sparse roadmap, the current posture of the robotic arm, and the desired posture of the robotic arm comprises: In the updated sparse roadmap, searching for an obstacle avoidance path for the robotic arm using a first path search algorithm through a first thread according to the current posture of the robotic arm and the desired posture of the robotic arm; In the updated sparse roadmap, searching for an obstacle avoidance path for the robotic arm using a second path search algorithm through a second thread according to the current posture of the robotic arm and the desired posture of the robotic arm; In the first thread and the second thread, when one of the threads searches and obtains the robot arm obstacle avoidance path, the operation of the other thread is stopped, and the robot arm obstacle avoidance path searched and obtained by one of the threads is used as the planned robot arm obstacle avoidance path.

8. An obstacle avoidance path planning device, characterized in that: include: The candidate path node acquisition module is used to perform discrete sampling on the joint space of the manipulator to obtain several candidate path nodes; A mapping relationship determination module, configured to construct a sparse route map based on a plurality of candidate route nodes; A roadmap update module is used to update the sparse roadmap based on the locations of obstacles detected in real time; An obstacle avoidance path acquisition module, configured to search for an obstacle avoidance path based on the updated sparse roadmap, the current position of the robotic arm, and the desired position of the robotic arm; The candidate path node acquisition module includes: A discretization step length acquisition module is used to obtain a discretization step length according to a preset observation distance of a path node and a penetration distance of a visible area of ​​an adjacent path node; The node determination module is used to perform discrete sampling on the joint space of the robot arm according to the discretization step size to obtain several candidate path nodes.

9. An electronic device comprising a processor and a memory; characterized in that: The memory stores a computer program, and the computer program is suitable for being loaded by the processor and executing the obstacle avoidance path planning method according to any one of claims 1 to 7.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the obstacle avoidance path planning method according to any one of claims 1 to 7 is implemented.

Images