Real-time motion planning method and system for robotic arms in human-machine collaborative scenarios

Abstract

The present invention discloses a real-time motion planning method and system for a manipulator facing a human-machine collaborative integration scenario. First, batch mixed sampling of the manipulator joint space is performed, and then batch adaptive path expansion of the manipulator joint space is performed on the path tree. After batch adaptive expansion of the path tree, in order to speed up the path planning time, batch adaptive bidirectional path expansion is performed on the path tree. Finally, a bidirectional connection attempt is made for the manipulator joint space path. After the connection is successful, a feasible manipulator joint space path is successfully found, and the manipulator motion planning process ends. Through key steps such as batch mixed sampling in joint space and batch adaptive bidirectional path expansion, the manipulator motion planning process is greatly accelerated, and the manipulator motion planning task can be adapted to different obstacle field environments. It has a strong generalization ability and is more adaptable to complex man-machine collaborative integration scenarios.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot real-time motion planning, and in particular relates to a method and system for real-time motion planning of a robotic arm for a human-machine collaborative integration scenario. Background Art

[0002] Real-time motion planning is a key technology to achieve efficient collaboration and ensure safety in human-robot collaboration scenarios. Unlike industrial robotic arms operating in structured environments, robotic arms in human-robot collaboration scenarios often need to perform motion planning in complex environments. This means that the robotic arm must adapt to environments with different numbers and shapes of obstacles. In addition, the motion planning time must be minimized to respond to dynamic tasks in a timely manner and ensure efficient human-robot collaboration to avoid emergency stops. Some researchers have proposed fast path planners accelerated by GPUs. However, these methods are mainly applied to mobile robots. Motion planning of robotic arms usually introduces additional constraints, and obstacles are difficult to represent in configuration space.

[0003] Compared to other motion planning algorithms such as artificial potential field (APF), A* algorithm, Dijkstra algorithm and particle swarm optimization (PSO), sampling-based motion planning algorithms such as RRT and PRM algorithms avoid the need to model the obstacle configuration space, making them effective for high-dimensional planning problems. These algorithms can directly plan joint paths without the need to pre-set terminal paths. For single query problems, RRT algorithms are generally more efficient and have lower time overhead than probabilistic PRM algorithms. Currently, researchers have proposed various improvements to the RRT algorithm to handle complex environments, resulting in the development of several new algorithms, including the bidirectional RRT algorithm (B-RRT), the target-biased bidirectional RRT* algorithm (GB-RRT*), and the novel potential field-guided bidirectional RRT* algorithm (NPB-RRT*-DC). Although these RRT variants have improved in planning time and path optimization, they still face challenges in meeting the needs of real-time motion planning.

[0004] In sampling-based motion planning algorithms, the main source of planning time consumption is the large number of collision checks performed during the path extension process. Existing sampling point collision detection methods usually rely on geometric model calculations, such as those found in the FCL library and bounding boxes, resulting in huge computational overhead. Current learning-based motion planning algorithms, such as the CN-RRT and Fastron methods, have made significant improvements in reducing the motion planning time of robotic arms. However, these motion planning methods usually operate in relatively fixed obstacle environments, and have limited generalization capabilities when applied to different or unseen obstacle scenes. In response to the above problems, the present invention proposes a real-time motion planning method for robotic arms in this article for human-machine collaborative integration scenarios, which is used to handle real-time motion planning tasks for robotic arms across different obstacle scenes. Summary of the invention

[0005] In view of the problem that the above-mentioned traditional robotic arm motion planning method has low planning time and is difficult to achieve efficient path planning of the robotic arm in complex cross-scenario situations, the present invention provides a robotic arm real-time motion planning method and system for human-machine collaborative integration scenarios.

[0006] The technical solution adopted by the present invention to solve the technical problem is:

[0007] A real-time motion planning method for a robotic arm in a human-machine collaborative integration scenario, the method comprising the following steps:

[0008] S100: Obtain real-time environmental information, divide the robot operation space, and initialize two path trees in the robot joint space and And respectively configure the initial nodes of the path tree, perform batch mixed sampling in the joint space, continuously obtain a preset number of sampling points, and find the path tree The closest to each sampling point Path nodes are formed candidate path edges;

[0009] S200: Perform equidistant interpolation on each of the M candidate edges to obtain interpolation points, based on the trained gap inference network model interpolation points for batch collision estimation, and we get After the collision results of the interpolation points are obtained, the last non-collision interpolation point among the interpolation points on each candidate path is selected as the new path extension node and added to the path tree. A collection of nodes;

[0010] S300: The new extension node in the path tree Batch sampling points, find the path tree The closest to each sampling point Path nodes are formed candidate path edges, each of the M candidate edges is interpolated with equal distances to obtain interpolation points for batch collision estimation, and we get After the collision results of the interpolation points are obtained, the last non-collision interpolation point among the interpolation points on each candidate path is selected as the new path extension node and added to the path tree. A collection of nodes;

[0011] S400: Determine two path trees and Whether the connection is successful. If the connection is successful, a feasible robot joint space path is found and the robot motion planning process ends.

[0012] Preferably, S100 includes:

[0013] S110: Divide the robot arm operation space into subspace units, each of which has the same size. Depending on whether there is an obstacle in the subspace unit, the state of the subspace unit is divided into a free state and an occupied state. A depth camera is used to obtain the state information matrix of all subspace units in the robot arm operation space. , for Dimensional matrix;

[0014] S120: Initialize 2 path trees in the robot joint space and , and The initial nodes are the starting configuration nodes of the robot arm. and the target configuration node , for the robot arm, its joint space is expressed as ,in represents the degrees of freedom of the robot arm, then the starting configuration node of the robot arm and the target configuration node In the joint space, they are expressed as and ;

[0015] S130: Determine if the robot arm is at the initial configuration node and the target configuration node Whether the spatial position will collide with environmental obstacles. If a collision occurs, modify the starting configuration node. and the target configuration node , otherwise proceed to the next step;

[0016] S140: Batch mixed sampling in joint space, continuous acquisition sampling points;

[0017] S150: For the above sampling points, find the path tree The closest to each sampling point Path Node ,form Candidate path edges , ,…, .

[0018] Preferably, S140 includes:

[0019] Before sampling, the numerical limit range of each joint angle of the robot arm is pre-set according to the actual hardware parameters of the robot arm. The value of each joint angle at the sampling point must be within the limited range.

[0020] Generate a random probability using a uniform random distribution function ,if , then a sampling point is randomly generated within the limited range of the robot joint angle; if , then from the path tree A random sampling point is selected from the joint space path point set as the sampling point; the above process is repeated until the Sampling points , at which point the batch sampling process ends.

[0021] Preferably, S200 includes:

[0022] S210: For M candidate edges , ,…, Each edge in is interpolated equidistantly, with an interpolation step length of , thus obtaining a total of Interpolation points ;

[0023] S220: Use the trained gap reasoning network model to Interpolation points Perform batch collision estimation;

[0024] S230: After obtaining After interpolating the collision results of the points, select each candidate path edge , ,…, The last collision-free interpolation point among the interpolation points is used as a new path extension node to obtain a total of New extension nodes , rather than The closest node Connect and add the new extension node Add path tree A collection of nodes.

[0025] Preferably, S220 includes:

[0026] S221: The interpolation point data is input into the trained gap inference network model at one time for batch data processing, and the The gap value matrix corresponding to the interpolation points ,in, for dimensional matrix, h=1,2,…, ;

[0027] S222: For the matrix For each element in , if its value is less than the safety threshold , then the element is set to 1, otherwise it is set to 0, where ;

[0028] S223: Each matrix and the state information matrix of the subspace unit Perform Hadamard product acquisition Collision Inference Matrix ; For any collision inference matrix ,matrix If all the elements are 0, it means that the robot arm joint configuration is When the interpolation point is reached, the robot arm does not collide with the obstacle. If the matrix If there is an element with a value of 1 in , it means that the robot arm collides with the obstacle. After performing the above operations, you can get Collision estimation results for interpolation points.

[0029] Preferably, the gap reasoning network model includes six parts that are sequentially forward connected. The first part connects the robot joint configuration nodes As initial input, and use the kernel function The joint angles are encoded to increase the frequency of the input values. The output of the first part will be used as the input of the second part. The second part consists of a fully connected layer, an activation function Sigmoid, and a DropOut layer. The output of the second part will be used as the input of the third part. The third part consists of a fully connected layer, an activation function Sigmoid, and a DropOut layer. The output features of the first part are horizontally spliced ​​on the output features of the second part as the input of the third part. The output of the third part will be used as the input of the fourth part. The fourth part consists of a fully connected layer, an activation function Sigmoid, and a DropOut layer. The output of the fourth part will be used as the input of the fifth part. The fifth part consists of a fully connected layer, an activation function Sigmoid, and a DropOut layer. The output features of the first part are horizontally spliced ​​on the output features of the fourth part as the input of the fifth part. The output of the fifth part will be used as the input of the sixth part. The sixth part consists of a fully connected layer, and the number of output features is , the purpose is to achieve dimensional gap distance mapping.

[0030] Preferably, S300 includes:

[0031] S310: Select New Extension Nodes As a path tree Batch sampling points, find the path tree The closest to each sampling point Path Node ,form Candidate path edges , ,…, ;

[0032] S320: For the aforementioned M candidate edges , ,…, Each edge in is interpolated equidistantly, with an interpolation step length of , thus obtaining a total of Interpolation points ;

[0033] S330: Use the trained gap reasoning network model to Interpolation points Perform batch collision estimation;

[0034] S340: After obtaining After interpolating the collision results of the points, select each candidate path edge , ,…, The last collision-free interpolation point among the interpolation points is used as a new path extension node to obtain a total of New extension nodes , rather than The closest node Connect and add the new extension node Add path tree A collection of nodes.

[0035] Preferably, S400 includes:

[0036] If the path tree and path tree There are overlapping nodes in the path nodes in and Successful connection means that a feasible robot joint space path is successfully found, and the robot motion planning process ends;

[0037] S400 also includes: if the path tree and path tree If there are no overlapping nodes in the path nodes, then swap the two path trees. and , and repeat S100-S400 until a feasible path is found or the planning time reaches a preset upper limit.

[0038] A real-time motion planning system for robotic arms in human-machine collaborative scenarios, including a candidate path edge determination module and a path tree Extension modules, path tree Extension modules and motion planning modules,

[0039] The candidate path edge determination module is used to obtain real-time environmental information, divide the robot operation space, and initialize two path trees in the robot joint space. and And respectively configure the initial nodes of the path tree, perform batch mixed sampling in the joint space, continuously obtain a preset number of sampling points, and find the path tree The closest to each sampling point Path nodes are formed candidate path edges;

[0040] Path Tree The extension module is used to perform equidistant interpolation on each of the M candidate edges to obtain interpolation points, based on the trained gap inference network model interpolation points for batch collision estimation, and we get After the collision results of the interpolation points are obtained, the last non-collision interpolation point among the interpolation points on each candidate path is selected as the new path extension node and added to the path tree. A collection of nodes;

[0041] Path Tree Extension modules are used to The new extension node in the path tree Batch sampling points, find the path tree The closest to each sampling point Path nodes are formed candidate path edges, each of the M candidate edges is interpolated with equal distances to obtain interpolation points for batch collision estimation, and we get After the collision results of the interpolation points are obtained, the last non-collision interpolation point among the interpolation points on each candidate path is selected as the new path extension node and added to the path tree. A collection of nodes;

[0042] The motion planning module is used to determine the two path trees and Whether the connection is successful. If the connection is successful, a feasible robot joint space path is found and the robot motion planning process ends.

[0043] The above-mentioned real-time motion planning method and system for the robotic arm for human-machine collaborative integration scenarios greatly accelerates the robotic arm motion planning process through key steps such as batch mixed sampling in joint space and batch adaptive bidirectional path expansion. It can adapt to the robotic arm motion planning tasks in different obstacle field environments, has strong generalization capabilities, and is more adaptable to complex human-machine collaborative integration scenarios. BRIEF DESCRIPTION OF THE DRAWINGS

[0044] Figure 1 It is a flow chart of a real-time motion planning method for a robotic arm for a human-machine collaborative integration scenario in one embodiment of the present invention;

[0045] Figure 2 is a schematic diagram of a 6-DOF manipulator and a discretized operating space in one embodiment of the present invention;

[0046] Figure 3 A path tree in one embodiment of the present invention Schematic diagram of batch adaptive path extension;

[0047] Figure 4 A path tree in one embodiment of the present invention Schematic diagram of batch adaptive bidirectional path extension;

[0048] Figure 5It is a robot arm motion planning simulation environment of obstacle scene 1 in one embodiment of the present invention;

[0049] Figure 6 It is a robot arm motion planning simulation environment of obstacle scene 2 in one embodiment of the present invention;

[0050] Figure 7 is an example of an arm motion planning trajectory in obstacle scene 1 in one embodiment of the present invention;

[0051] Figure 8 This is an example of the arm motion planning trajectory in obstacle scene 2 in one embodiment of the present invention. DETAILED DESCRIPTION

[0052] In order to enable those skilled in the art to better understand the technical solution of the present invention, the present invention is further described in detail below in conjunction with the accompanying drawings.

[0053] In one embodiment, Figure 1 As shown, a real-time motion planning method for a robotic arm in a human-machine collaborative integration scenario includes the following steps:

[0054] S100: Obtain real-time environmental information, divide the robot operation space, and initialize two path trees in the robot joint space and And respectively configure the initial nodes of the path tree, perform batch mixed sampling in the joint space, continuously obtain a preset number of sampling points, and find the path tree The closest to each sampling point Path nodes are formed candidate path edges;

[0055] S200: Perform equidistant interpolation on each of the M candidate edges to obtain interpolation points, based on the trained gap inference network model interpolation points for batch collision estimation, and we get After the collision results of the interpolation points are obtained, the last non-collision interpolation point among the interpolation points on each candidate path is selected as the new path extension node and added to the path tree. A collection of nodes;

[0056] S300: The new extension node in the path tree Batch sampling points, find the path tree The closest to each sampling point Path nodes are formed candidate path edges, each of the M candidate edges is interpolated with equal distances to obtain interpolation points for batch collision estimation, and we get After the collision results of the interpolation points are obtained, the last non-collision interpolation point among the interpolation points on each candidate path is selected as the new path extension node and added to the path tree. A collection of nodes;

[0057] S400: Determine two path trees and Whether the connection is successful. If the connection is successful, a feasible robot joint space path is found and the robot motion planning process ends.

[0058] In one embodiment, S100 includes:

[0059] S110: Divide the robot arm operation space into subspace units, each of which has the same size. Depending on whether there is an obstacle in the subspace unit, the state of the subspace unit is divided into a free state (value 0) and an occupied state (value 1). The depth camera is used to obtain the state information matrix of all subspace units in the robot arm operation space. , for dimensional matrix; further, Figure 2 As shown in the figure, a 6-DOF robot (UR5 robot) and a schematic diagram of the discretized operating space are shown: In the robot simulation software CoppeliaSim, the operating space of the UR5 robot is discretized into 3456 ( =3456) subspace units of the same volume, each cube unit has a side length of 0.045m. There are two obstacles in the operation space, occupying several subspace units;

[0060] S120: Initialize 2 path trees in the robot joint space and , and The initial nodes are the starting configuration nodes of the robot arm. and the target configuration node , for the robot arm, its joint space is expressed as ,in represents the degrees of freedom of the robot arm, then the starting configuration node of the robot arm and the target configuration node In the joint space, they are expressed as and ;

[0061] S130: Determine if the robot arm is at the initial configuration node and the target configuration node Whether the spatial position will collide with environmental obstacles. If a collision occurs, modify the starting configuration node. and the target configuration node , otherwise proceed to the next step;

[0062] S140: Batch mixed sampling in joint space, continuous acquisition sampling points;

[0063] S150: For the above sampling points, find the path tree The closest to each sampling point Path Node ,form Candidate path edges , ,…, .

[0064] In one embodiment, S140 includes:

[0065] Before sampling, the numerical limit range of each joint angle of the robot arm is pre-set according to the actual hardware parameters of the robot arm. The value of each joint angle at the sampling point must be within the limited range; for example, the first joint limit angle of the robot arm is , and so on, The joint limit angle is ;

[0066] Generate a random probability using a uniform random distribution function ,if , then a sampling point is randomly generated within the limited range of the robot joint angle; if , then from the path tree A random sampling point is selected from the joint space path point set as the sampling point; the above process is repeated until the Sampling points , at which point the batch sampling process ends.

[0067] In one embodiment, Figure 2 As shown, S200 includes:

[0068] S210: For M candidate edges , ,…, Each edge in is interpolated equidistantly, with an interpolation step length of , thus obtaining a total of Interpolation points ;

[0069] S220: Use the trained gap reasoning network model to Interpolation points Perform batch collision estimation;

[0070] S230: After obtaining After interpolating the collision results of the points, select each candidate path edge , ,…, The last collision-free interpolation point among the interpolation points is used as a new path extension node to obtain a total of New extension nodes , rather than The closest node Connect and add the new extension node Add path tree A collection of nodes.

[0071] In one embodiment, S220 includes:

[0072] S221: The interpolation point data is input into the trained gap inference network model at one time for batch data processing, and the The gap value matrix corresponding to the interpolation points ,in, for dimensional matrix, h=1,2,…, ;

[0073] S222: For the matrix For each element in , if its value is less than the safety threshold , then the element is set to 1, otherwise it is set to 0, where ;

[0074] S223: Each matrix and the state information matrix of the subspace unit Perform Hadamard product acquisition Collision Inference Matrix ; For any collision inference matrix ,matrix If all the elements are 0, it means that the robot arm joint configuration is When the interpolation point is reached, the robot arm does not collide with the obstacle. If the matrix If there is an element with a value of 1 in , it means that the robot arm collides with the obstacle. After performing the above operations, you can get Collision estimation results for interpolation points.

[0075] In one embodiment, the gap reasoning network model includes six parts that are forward connected in sequence. The first part connects the robot joint configuration nodes ( yes dimensional matrix) as the initial input and use the kernel function ( is an overclocking parameter) to encode the joint angle and increase the frequency of the input value. The output of the first part will be used as the input of the second part; the second part consists of a fully connected layer, an activation function Sigmoid, and a DropOut layer, and the output of the second part will be used as the input of the third part; the third part consists of a fully connected layer, an activation function Sigmoid, and a DropOut layer. The output features of the first part are horizontally spliced ​​on the output features of the second part as the input of the third part, and the output of the third part will be used as the input of the fourth part; the fourth part consists of a fully connected layer, an activation function Sigmoid, and a DropOut layer, and the output of the fourth part will be used as the input of the fifth part; the fifth part consists of a fully connected layer, an activation function Sigmoid, and a DropOut layer. The output features of the first part are horizontally spliced ​​on the output features of the fourth part as the input of the fifth part, and the output of the fifth part will be used as the input of the sixth part; the sixth part consists of a fully connected layer, and the number of output features is , the purpose is to achieve dimensional gap distance mapping.

[0076] Furthermore, for the path tree Perform batch adaptive bidirectional path expansion in the robot joint space. After batch adaptive expansion, in order to speed up the path planning time, the path tree Perform batch adaptive bidirectional path extension.

[0077] In one embodiment, Figure 3 As shown, S300 includes:

[0078] S310: For the path tree , which is different from the path tree Batch hybrid sampling process to guide the path tree Fast path tree Two-way expansion, select New Extension Nodes As a path tree Batch sampling points, find the path tree The closest to each sampling point Path Node ,form Candidate path edges , ,…, ;

[0079] S320: For the aforementioned M candidate edges , ,…, Each edge in is interpolated equidistantly, with an interpolation step length of , thus obtaining a total of Interpolation points ;

[0080] S330: Use the trained gap reasoning network model to Interpolation points Perform batch collision estimation;

[0081] S340: After obtaining After interpolating the collision results of the points, select each candidate path edge , ,…, The last collision-free interpolation point among the interpolation points is used as a new path extension node to obtain a total of New extension nodes , rather than The closest node Connect and add the new extension node Add path tree A collection of nodes.

[0082] In one embodiment, S330 specifically includes: The interpolation point data is input into the gap reasoning network model at one time for batch data processing, and the The gap value matrix corresponding to the interpolation points ( for dimensional matrix, =1,2,…, ). For the matrix For each element in , if its value is less than the safety threshold ( , which can be set by the user) then the element is set to 1, otherwise it is set to 0. Then each matrix ( =1,2,…, ) and the state information matrix of the subspace unit Perform Hadamard product (multiply corresponding elements) to obtain Collision Inference Matrix (h=1,2,…, ). For any collision inference matrix ,matrix If all the elements in the list are 0, it means that the robot arm joint configuration is When the interpolation point is reached, the robot arm does not collide with the obstacle. If the matrix If there is an element with a value of 1 in , it means that the robot arm collides with an obstacle. After performing the above operations, you can obtain Collision estimation results for interpolation points.

[0083] In one embodiment, S400 includes:

[0084] If the path tree and path tree There are overlapping nodes in the path nodes in and Successful connection means that a feasible robot joint space path is successfully found, and the robot motion planning process ends;

[0085] S400 also includes: if the path tree and path tree If there are no overlapping nodes in the path nodes in , then swap the two path trees. and , and repeat S100-S400 until a feasible path is found or the planning time reaches a preset upper limit.

[0086] The following is a specific example to perform simulation verification analysis of the proposed method by setting the following experimental parameters:

[0087] 1) Robotic arm motion planning simulation environment such as Figure 4 and Figure 5 As shown, the verification was performed using a UR5 robotic arm;

[0088] 2) Discretize the robot space into 3456 ( ), the batch sampling quantity M=10;

[0089] 3) Figure 4 The starting joint configuration of the robot arm in the environment is [-2.6708 rad, -0.7708 rad, 1 rad, 0rad, 0.5 rad, 0.5 rad], and the target joint configuration is [-0.5708 rad, -0.7708 rad, 1 rad, 0rad, 0.5 rad, 0.5 rad]; Figure 5 In the environment, the starting joint configuration of the robot arm is [-2.1808 rad, -1.0708 rad, 1.1 rad, -1.5708 rad, -1.4 rad, 0 rad], and the target joint configuration is [-1.2608 rad, -1.0708 rad, 1.1 rad, -1.5708 rad, -1.4 rad, 0 rad].

[0090] 50 simulation experiments were conducted in two obstacle scenarios. The average motion planning time of the proposed method was only 0.23s and 0.26s respectively. Figure 6 and Figure 7 The motion trajectory of the robot arm in two different obstacle scenarios is demonstrated. Compared with the traditional sampling-based robot arm motion planning method (NPB-RRT*-DC), the motion planning time of the proposed method is reduced by 90% without increasing the motion path length, and it is expected to be applied to human-machine collaborative scenarios where efficient motion planning is required.

[0091] A real-time motion planning system for robotic arms in human-machine collaborative scenarios, including a candidate path edge determination module and a path tree Extension modules, path tree Extension modules and motion planning modules,

[0092] The candidate path edge determination module is used to obtain real-time environmental information, divide the robot operation space, and initialize two path trees in the robot joint space. and And respectively configure the initial nodes of the path tree, perform batch mixed sampling in the joint space, continuously obtain a preset number of sampling points, and find the path tree The closest to each sampling point Path nodes are formed candidate path edges;

[0093] Path Tree The extension module is used to perform equidistant interpolation on each of the M candidate edges to obtain interpolation points, based on the trained gap inference network model interpolation points for batch collision estimation, and we get After the collision results of the interpolation points are obtained, the last non-collision interpolation point among the interpolation points on each candidate path is selected as the new path extension node and added to the path tree. A collection of nodes;

[0094] Path Tree Extension modules are used to The new extension node in the path tree Batch sampling points, find the path tree The closest to each sampling point Path nodes are formed candidate path edges, each of the M candidate edges is interpolated with equal distances to obtain interpolation points for batch collision estimation, and we get After the collision results of the interpolation points are obtained, the last non-collision interpolation point among the interpolation points on each candidate path is selected as the new path extension node and added to the path tree. A collection of nodes;

[0095] The motion planning module is used to determine the two path trees and Whether the connection is successful. If the connection is successful, a feasible robot joint space path is found and the robot motion planning process ends.

[0096] For the specific limitations of the real-time motion planning system for a robotic arm for a human-machine collaborative integration scenario, please refer to the limitations of the real-time motion planning method for a robotic arm for a human-machine collaborative integration scenario mentioned above, which will not be repeated here. Each module in the above-mentioned real-time motion planning system for a robotic arm for a human-machine collaborative integration scenario can be implemented in whole or in part through software, hardware, and a combination thereof. The above-mentioned modules can be embedded in or independent of the processor in a computer device in the form of hardware, or can be stored in a memory in a computer device in the form of software, so that the processor can call and execute the operations corresponding to the above modules.

[0097] Compared with the prior art, the present invention has the following beneficial effects:

[0098] (1) Compared with the traditional sampling-based robot motion planning method, which has the problems of long motion planning time and poor real-time performance, the motion planning method proposed in the present invention greatly accelerates the robot motion planning process by performing key steps such as batch hybrid sampling in joint space and batch adaptive bidirectional path expansion.

[0099] (2) Compared with the previous learning-based robot motion planning method which can only be used for robot motion planning in a fixed environment, the motion planning method proposed in the present invention can adapt to robot motion planning tasks in different obstacle field environments, has strong generalization ability, and is more adaptable to complex human-machine collaborative integration scenarios.

[0100] (3) Compared with the existing sampling-based robot arm motion planning method in which only one candidate path edge is expanded in each iteration, the present invention increases the probability of finding a feasible path by batch sampling multiple points and constructing multiple candidate path edges in each iteration of path planning, thereby further improving the planning efficiency.

[0101] (4) In terms of the expansion method, the present invention interpolates each candidate edge to obtain multiple interpolation points at one time, and uses the gap reasoning network model to batch process the collision detection task, and finds the last collision-free interpolation point of each candidate edge as the new extension node of the path tree. The above process greatly reduces the time of collision detection during the path expansion process by using the gap reasoning network model to batch process the collision detection task; at the same time, "finding the last collision-free interpolation point of each candidate edge as the new extension node of the path tree" can realize adaptive step size expansion, that is, expansion is performed with the maximum step size under the premise of no collision according to the environmental obstacle information, which also improves the expansion efficiency.

[0102] The above is a detailed introduction to the real-time motion planning method and system for a robotic arm for a human-machine collaborative integration scenario provided by the present invention. This article uses specific examples to illustrate the principles and implementation methods of the present invention. The description of the above embodiments is only used to help understand the core idea of ​​the present invention. It should be pointed out that for ordinary technicians in this technical field, without departing from the principles of the present invention, several improvements and modifications can be made to the present invention, and these improvements and modifications also fall within the scope of protection of the claims of the present invention.

Claims

1. A real-time motion planning method for a robotic arm for human-machine collaboration and integration scenarios, characterized in that: The method comprises the following steps: S100: Obtain real-time environmental information, divide the robot operation space, and initialize two path trees in the robot joint space and And respectively configure the initial nodes of the path tree, perform batch mixed sampling in the joint space, continuously obtain a preset number of sampling points, and find the path tree The closest to each sampling point Path nodes are formed candidate path edges; S200: Perform equidistant interpolation on each of the M candidate edges to obtain interpolation points, based on the trained gap inference network model interpolation points for batch collision estimation, and we get After the collision results of the interpolation points are obtained, the last non-collision interpolation point among the interpolation points on each candidate path is selected as the new path extension node and added to the path tree. A collection of nodes; S300: The new extension node in the path tree Batch sampling points, find the path tree The closest to each sampling point Path nodes are formed candidate path edges, each of the M candidate edges is interpolated with equal distances to obtain interpolation points for batch collision estimation, and we get After the collision results of the interpolation points are obtained, the last non-collision interpolation point among the interpolation points on each candidate path is selected as the new path extension node and added to the path tree. A collection of nodes; S400: Determine two path trees and Whether the connection is successful. If the connection is successful, a feasible robot joint space path is found and the robot motion planning process ends.

2. The method according to claim 1, characterized in that S100 includes: S110: Divide the robot arm operation space into subspace units, each of which has the same size. Depending on whether there are obstacles in the subspace unit, the state of the subspace unit is divided into a free state and an occupied state. A depth camera is used to obtain the state information matrix of all subspace units in the robot arm operation space. , for Dimensional matrix; S120: Initialize 2 path trees in the robot joint space and , and The initial nodes are the starting configuration nodes of the robot arm. and the target configuration node , for the robot arm, its joint space is represented as ,in represents the degrees of freedom of the robot arm, then the starting configuration node of the robot arm and the target configuration node In the joint space, they are expressed as and ; S130: Determine if the robot arm is at the initial configuration node and the target configuration node Whether the spatial position will collide with environmental obstacles. If a collision occurs, modify the starting configuration node. and the target configuration node , otherwise proceed to the next step; S140: Batch mixed sampling in joint space, continuous acquisition Sampling points ; S150: For the above sampling points, find the path tree The closest to each sampling point Path Node ,form Candidate path edges , ,…, .

3. The method according to claim 2, characterized in that S140 includes: Before sampling, the numerical limit range of each joint angle of the robot arm is pre-set according to the actual hardware parameters of the robot arm. The value of each joint angle at the sampling point must be within the limited range. Generate a random probability using a uniform random distribution function ,if , then a sampling point is randomly generated within the limited range of the robot joint angle; if , then from the path tree A random sampling point is selected from the joint space path point set as the sampling point; the above process is repeated until the Sampling points , at which point the batch sampling process ends.

4. The method according to claim 3, characterized in that S200 includes: S210: For M candidate edges , ,…, Each edge in is interpolated equidistantly, with an interpolation step length of , thus obtaining a total of Interpolation points ; S220: Use the trained gap reasoning network model to Interpolation points Perform batch collision estimation; S230: After obtaining After interpolating the collision results of the points, select each candidate path edge , ,…, The last collision-free interpolation point among the interpolation points is used as a new path extension node to obtain a total of New extension nodes , rather than The closest node Connect and add the new extension node Add path tree A collection of nodes.

5. The method according to claim 4, characterized in that S220 includes: S221: The interpolation point data is input into the trained gap inference network model at one time for batch data processing, and the The gap value matrix corresponding to the interpolation points ,in, for dimensional matrix, h=1,2,…, ; S222: For the matrix For each element in , if its value is less than the safety threshold , then the element is set to 1, otherwise it is set to 0, where ; S223: Each matrix and the state information matrix of the subspace unit Perform Hadamard product acquisition Collision Inference Matrix ; For any collision inference matrix ,matrix If all the elements are 0, it means that the robot arm joint configuration is When the interpolation point is reached, the robot arm does not collide with the obstacle. If the matrix If there is an element with a value of 1 in , it means that the robot arm collides with the obstacle. After performing the above operations, you can get Collision estimation results for interpolation points.

6. The method according to claim 5, characterized in that The gap reasoning network model consists of six parts that are forward-connected in sequence. The first part connects the robot joint configuration nodes. As initial input, and use the kernel function Encode the joint angles and increase the frequency of the input values. The output of the first part will be used as the input of the second part. The second part consists of a fully connected layer, an activation function Sigmoid, and a DropOut layer. The output of the second part will serve as the input of the third part. The third part consists of a fully connected layer, an activation function Sigmoid, and a DropOut layer. The output features of the first part are horizontally spliced ​​on the output features of the second part as the input of the third part, and the output of the third part will be used as the input of the fourth part; the fourth part consists of a fully connected layer, an activation function Sigmoid, and a DropOut layer, and the output of the fourth part will be used as the input of the fifth part; The fifth part consists of a fully connected layer, an activation function Sigmoid, and a DropOut layer. The output features of the first part are horizontally spliced ​​on the output features of the fourth part as the input of the fifth part. The output of the fifth part will be used as the input of the sixth part. The sixth part consists of a fully connected layer, and the output feature number is , the purpose is to achieve dimensional gap distance mapping.

7. The method according to claim 6, characterized in that S300 includes: S310: Select New Extension Nodes As a path tree Batch sampling points, find the path tree The closest to each sampling point Path Node ,form Candidate path edges , ,…, ; S320: For the aforementioned M candidate edges , ,…, Each edge in is interpolated equidistantly, with an interpolation step length of , thus obtaining a total of Interpolation points ; S330: Use the trained gap reasoning network model to Interpolation points Perform batch collision estimation; S340: After obtaining After interpolating the collision results of the points, select each candidate path edge , ,…, The last collision-free interpolation point among the interpolation points is used as a new path extension node to obtain a total of New extension nodes , rather than The closest node Connect and add the new extension node Add path tree A collection of nodes.

8. The method according to claim 7, characterized in that S400 includes: If the path tree and path tree There are overlapping nodes in the path nodes in and Successful connection means that a feasible robot joint space path is successfully found, and the robot motion planning process ends; S400 also includes: if the path tree and path tree If there are no overlapping nodes in the path nodes, then swap the two path trees. and , and repeat S100-S400 until a feasible path is found or the planning time reaches a preset upper limit.

9. A real-time motion planning system for robotic arms for human-machine collaboration and integration scenarios, characterized by: Including candidate path edge determination module, path tree Extension modules, path tree Extension modules and motion planning modules, The candidate path edge determination module is used to obtain real-time environmental information, divide the robot operation space, and initialize two path trees in the robot joint space. and And respectively configure the initial nodes of the path tree, perform batch mixed sampling in the joint space, continuously obtain a preset number of sampling points, and find the path tree The closest to each sampling point Path nodes are formed candidate path edges; Path Tree The extension module is used to perform equidistant interpolation on each of the M candidate edges to obtain interpolation points, based on the trained gap inference network model interpolation points for batch collision estimation, and we get After the collision results of the interpolation points are obtained, the last non-collision interpolation point among the interpolation points on each candidate path is selected as the new path extension node and added to the path tree. A collection of nodes; Path Tree Extension modules are used to The new extension node in the path tree Batch sampling points, find the path tree The closest to each sampling point Path nodes are formed candidate path edges, each of the M candidate edges is interpolated with equal distances to obtain interpolation points for batch collision estimation, and we get After the collision results of the interpolation points are obtained, the last non-collision interpolation point among the interpolation points on each candidate path is selected as the new path extension node and added to the path tree. A collection of nodes; The motion planning module is used to determine the two path trees and Whether the connection is successful. If the connection is successful, a feasible robot joint space path is found and the robot motion planning process ends.

Images