A human-robot collaboration safety-oriented dynamic obstacle avoidance method for industrial robots

Abstract

The application discloses a kind of industrial robot dynamic obstacle avoidance methods for man-machine cooperation safety, specifically includes: by depth camera real-time acquisition human collision body information, and update synchronization to industrial robot coordinate system under;Design probability field RRT-Connect algorithm, fast planning feasible initial path;After generating initial path, fast path detection is carried out, whether there is obstacle shelter and trajectory shelter on the path;If there is no shelter, then carry out path optimization strategy;If it is detected that there is shelter, then on the basis of initial path, based on probability field RRT-Connect algorithm and path optimization strategy, path regeneration is carried out again;After path regeneration, repeat the above steps until robot moves according to path without collision.This application realizes online dynamic obstacle avoidance in the process of robot movement, guarantees man-machine safety and processing task continuity, and can be widely applied in man-machine cooperation safety obstacle avoidance task.

Description

Technical Field

[0001] The present invention belongs to the technical field of human-machine collaborative safety, and specifically relates to a dynamic obstacle avoidance method for an industrial robot oriented to human-machine collaborative safety. Background Art

[0002] With the continuous development of modern manufacturing, human-robot collaboration has gradually become the development direction of intelligent manufacturing. Human-robot collaboration places humans and robots in the same space and time, inevitably posing safety risks to humans during robot assembly and processing. Traditional safety measures that separate humans and robots in space and time are not applicable to human-robot collaboration scenarios. Existing safety assurance methods, such as emergency stops after collisions detected through torque monitoring and speed limits based on the distance between the human and the robot, prevent further harm to the human body. However, these measures interrupt the robot's processing tasks and reduce processing efficiency. Dynamic obstacle avoidance can plan a collision-free path based on current obstacle information while ensuring the safety of both humans and robots, driving the robot to the initial target position. It is an effective means to ensure human-robot safety and improve processing efficiency.

[0003] Currently, the main methods for robot path planning and obstacle avoidance include ant colony algorithms, genetic algorithms, A* algorithms, Dijkstra algorithms, RRT algorithms, artificial potential field methods, and DWA. Ant colony algorithms and genetic algorithms are computationally intensive and time-consuming, making them unsuitable for dynamic environments. A* and Dijkstra algorithms require a large amount of space to store environmental information, and while they have certain advantages for path planning in low-dimensional spaces, they are not suitable for industrial robots with six degrees of freedom. The DWA algorithm is commonly used for low-dimensional mobile robots, but the complexity of searching in the six-dimensional joint velocity space of industrial robots increases significantly. Artificial potential field methods are very effective for local path planning, but they suffer from local minima and unreachable targets. They are often used for single-target trajectory planning in dynamic environments. When applied to serial multi-joint industrial robots, potential field design and dynamics solutions are required for each joint, making them less suitable for trajectory avoidance involving coupled multi-joint targets. The RRT algorithm is a sampling-based path planning method that uses a collision detection module to determine path validity. It eliminates the need to explicitly construct the entire configuration space and boundaries. It is suitable for motion planning problems in high-dimensional spaces and complex constraints. However, the algorithm's randomness in high-dimensional spaces can reduce its efficiency. Existing RRT and its variants mostly focus on static environments. Dynamic planning algorithms, such as DRRT, while enhancing their adaptability to dynamic environments, also weaken their real-time response efficiency and path stability, making them difficult to meet the requirements of high-frequency dynamic obstacle avoidance and continuous human-machine obstacle avoidance in industrial collaboration scenarios. Summary of the Invention

[0004] In response to the problems existing in the above-mentioned prior art, the present invention proposes a dynamic obstacle avoidance method for industrial robots for human-machine collaborative safety, so as to realize dynamic obstacle avoidance of industrial robots, improve the real-time response efficiency and path stability of the algorithm, and meet the needs of high-frequency dynamic obstacles and continuous obstacle avoidance of humans and machines in industrial collaboration scenarios.

[0005] In order to achieve the above technical objectives, the present invention provides the following technical solutions:

[0006] A dynamic obstacle avoidance method for industrial robots oriented to human-machine collaboration safety, which specifically includes the following steps:

[0007] S1. Obtain human collision body information in real time through the depth camera and update it to the industrial robot coordinate system;

[0008] S2. Design the probability field RRT-Connect algorithm to quickly plan a feasible initial path;

[0009] S3. After generating the initial path, perform a quick path detection to check whether there are any obstacles or track obstructions on the path; if there are no obstacles, perform a path optimization strategy;

[0010] S4. If occlusion is detected, the path is regenerated based on the probability field RRT-Connect algorithm and path optimization strategy on the basis of the initial path;

[0011] S5. After the path is regenerated, steps S3-S4 are repeated until the robot moves along the path without collision.

[0012] Furthermore, step S1 specifically includes:

[0013] S11. Based on the color image obtained by the depth camera, use the OpenPose human detection network to detect key points of the human body in the color image and obtain the 2D coordinates of the key points of the human body in the color image;

[0014] S12, according to the depth map provided by the depth camera, the 2D coordinates of the key points of the human body are mapped to the depth map to obtain the 3D coordinates of the key points of the human body, which is expressed as follows:

[0015] ;

[0016] ;

[0017] in, is the 2D coordinate of the key point, is the value of the depth map on the corresponding pixel, is the internal parameter corresponding to the camera, 3D coordinates of key points of the human body;

[0018] S13, obtain the human key point 3D coordinates, and perform split assembly on the human body collision body, each joint of the human limbs, the torso, and the neck are taken as a cylindrical body for collision body modeling, and the head is taken as a spherical body for collision body modeling, and the whole human body collision body is obtained by assembly;

[0019] S14, the steps S11-S13 are circularly performed to obtain real-time human key point 3D coordinates by real-time detection on the real-time image collected by the depth camera.

[0020] Further, the probability field RRT-Connect algorithm in step S2 increases the probability field sampling on the basis of the RRT-Connect algorithm architecture, and changes the sampling strategy of the two trees in the original algorithm; the probability field sampling combines the gravitational field thought of the artificial potential field method, and converts the attractive force and the repulsive force into the gravitational probability field and the repulsive probability field, for a single tree, the gravitational probability field is generated in the joint value in the joint space corresponding to the terminal point, the tree is guided to explore the terminal point, and the repulsive probability field is generated in the existing node in the tree, and the growth of the tree is promoted to diverge.

[0021] Further, the sampling strategy of each tree in the probability field RRT-Connect algorithm is as follows:

[0022] P1, the initial path terminal point is recorded as , the initial path starting point is recorded as , the first path node is recorded as i , and the first existing node in the tree growth process is recorded as ; i Each node is represented as:

[0023] ;

[0024] ;

[0025] ;

[0026] Wherein, , , are six joint angles of the target point, the starting point, and the first existing node, respectively. i

[0027] P2, taking the first joint angle as an example, a gravitational probability field is generated, and the probability density function of the gravitational probability field is a normal distribution, and the formula is represented as:

[0028] ;

[0029] ​​;

[0030] in, is the angle value of the first joint of the robot; is the mean value, and in the gravitational probability field its value is The first joint angle value of is the standard deviation;

[0031] P3, also taking the first joint angle as an example, the starting point With existing nodes Generate a repulsive probability field, and the probability density function of the repulsive probability field is set to an inverted normal distribution, which is expressed as follows:

[0032] ;

[0033] ;

[0034] ;

[0035] ;

[0036] in, is the probability density function of the repulsive force probability field, is the normal distribution function, which is used Substitute the calculation into ; is the mean, for , ;for , ; is the standard deviation;

[0037] P4. Added a uniformly distributed probability density function within the range of joint angles. , increase the sampling probability of the end point and reduce the sampling probability of existing nodes. The formula is expressed as:

[0038] ;

[0039] in, is the minimum joint value within the reachable range of the first joint angle, is the maximum joint value within the reachable range of the first joint angle;

[0040] P5, probability density function of weighted mixed normal distribution, inverted normal distribution and uniform distribution, to obtain the overall sampling probability density in the first joint angle , the formula is:

[0041] ;

[0042] in, is the weight of the normal distribution, is a uniformly distributed weight, 、 For the i , j The weights of the inverted normal distribution, is the number of inverted normal distributions;

[0043] Normalize the overall sampling probability density to ensure that the integral is 1. The formula is:

[0044] ;

[0045] in, is the overall sampling probability density after normalization of the first joint angle;

[0046] P6. Repeat steps P2-P5 to obtain the normalized overall adoption probability density of the remaining joint angles. Use the rejection sampling method to generate six randomly sampled joint values ​​according to the reachable range of the joints, merge them into the final sampling point, and use it as a node in the tree.

[0047] Furthermore, the fast path detection in step S3 includes forward path detection and reverse path detection, the forward path is the path of the robot moving from the starting point to the end point, and the reverse path is the path of the robot moving from the end point to the starting point.

[0048] More specifically, the fast path detection in step S3 is as follows:

[0049] After obtaining the initial path, a virtual robot is built based on the CAD file using robot inverse solution, trajectory interpolation, and collision detection algorithms;

[0050] First, perform forward path detection, drive the virtual robot to move at high speed from the starting point to the end point along the initial path, and record the angles of each joint of the robot in real time during the movement. According to the obstacle information, when it is determined that the initial path will be blocked by an obstacle and a collision will occur, record the last forward collision-free path point in the initial path. ;

[0051] When the forward path detection determines that the robot will collide when moving along the initial path, the reverse path detection is triggered, and the virtual robot is driven to move at high speed from the end point to the starting point along the initial path. During the movement, the joint angles of the robot are also recorded in real time. According to the obstacle information, the last reverse collision-free path point in the initial path is recorded. ;

[0052] Finally, the fast path detection determines that 、 The initial paths between them will collide and path replanning is required.

[0053] Further, the path optimization strategy in step S3 is specifically:

[0054] First, the path between two interval path points , is subjected to fast collision detection, if the robot can directly move from to without collision, the intermediate path point is deleted; then the process is continued from , until all the movements between the interval path points will cause collision, the path with the deleted intermediate path point is reserved, and the optimization is completed.

[0055] Further, step S4 is specifically:

[0056] After obtaining the forward collision-free path point and the reverse collision-free path point , the original path from the initial path starting point to and the initial path ending point to is reserved, and and are taken as new starting point and ending point, and a new trajectory from to is planned based on the probabilistic field RRT-Connect;

[0057] Meanwhile, during the path regeneration, the following mechanism is established: when the fast path detection judges that collision will occur, but the real robot has not collided, on the one hand, the path regeneration is triggered, and on the other hand, the real robot is set to move only according to the trajectory from to , until a new path is generated, if the real robot has reached the point , the real robot waits in place for a new path.

[0058] Based on the above technical solution, the present application has at least the following beneficial effects:

[0059] The dynamic obstacle avoidance method for human-robot collaborative safety of the industrial robot realizes online dynamic obstacle avoidance in the robot movement process, and guarantees the safety of human and robot and the continuity of the processing task.

[0060] The present application designs a probabilistic field RRT-Connect method for robot joint space, which can quickly generate a feasible path between two points in space by applying target point attractive probability field and repulsive probability field of existing nodes to guide the fast exploration of the search tree.

[0061] The present invention establishes a mechanism for rapid path detection and path regeneration, realizes dynamic path generation, establishes a path optimization strategy, shortens the robot's movement time, and ultimately achieves online obstacle avoidance for the robot under dynamic obstacles.

[0062] The dynamic obstacle avoidance method for industrial robots proposed in the present invention can ensure the safety of humans and machines while enabling the robot to avoid obstacles and reach the target task point. It can be applied to human-machine collaborative safety obstacle avoidance tasks. BRIEF DESCRIPTION OF THE DRAWINGS

[0063] Figure 1 This is a flow chart of a dynamic obstacle avoidance method for industrial robots oriented to human-machine collaboration safety proposed by the present invention;

[0064] Figure 2 Schematic diagram of tree growth in the probability field RRT-Connect algorithm designed by the present invention;

[0065] Figure 3 Schematic diagram of fast path detection and path regeneration in the method proposed by the present invention. DETAILED DESCRIPTION

[0066] In order to make the purpose, technical solutions and advantages of the present invention more clear, the following Figure 1-Figure 3 It should be understood that the specific embodiments described herein are only used to illustrate the present invention and are not intended to limit the present invention.

[0067] Although the steps in the present invention are arranged with numbers, they are not intended to limit the order of the steps. Unless the order of the steps is clearly stated or the execution of a step requires other steps as a basis, the relative order of the steps can be adjusted. It is understood that the term "and / or" used herein refers to and covers any and all possible combinations of one or more of the associated listed items.

[0068] like Figure 1 As shown, the present invention proposes a dynamic obstacle avoidance method for industrial robots oriented to human-machine collaboration safety, which specifically includes the following steps:

[0069] S1. Obtain human collision body information in real time through the depth camera and update it to the industrial robot coordinate system;

[0070] As a preferred embodiment, step S1 specifically includes:

[0071] S11. Based on the color image obtained by the depth camera, use the OpenPose human detection network to detect key points of the human body in the color image and obtain the 2D coordinates of the key points of the human body in the color image;

[0072] S12, according to the depth map provided by the depth camera, the 2D coordinates of the key points of the human body are mapped to the depth map to obtain the 3D coordinates of the key points of the human body, which is expressed as follows:

[0073] ;

[0074] ;

[0075] in, is the 2D coordinate of the key point, is the value of the depth map on the corresponding pixel, is the internal parameter corresponding to the camera, 3D coordinates of key points of the human body;

[0076] S13, using the obtained 3D coordinates of the key points of the human body, assembling the human body collision body in a split manner, modeling the collision body by taking the joints of the human limbs, the torso, and the neck as cylinders, and modeling the collision body by taking the head as a sphere, and assembling the whole collision body of the human body;

[0077] S14, looping through steps S11-S13, performing real-time detection on the real-time image captured by the depth camera, and obtaining real-time 3D coordinates of key points of the human body.

[0078] S2. Design a probability field RRT-Connect algorithm to quickly plan a feasible initial path; in this application, the probability field RRT-Connect algorithm adds probability field sampling on the basis of the RRT-Connect algorithm architecture, changes the sampling strategy of the two trees in the original algorithm, and can quickly plan a collision-free path between two points; the probability field sampling integrates the gravitational field idea of ​​the artificial potential field method, converts the attraction and repulsion into gravitational probability field and repulsion probability field, and for a single tree, a gravitational probability field is generated at the joint value in the joint space corresponding to its end point, guiding the tree to explore the end point, and the existing nodes in the tree generate a repulsion probability field, which promotes the divergence of the tree's growth.

[0079] More specifically, the sampling strategy for each tree in the probability field RRT-Connect algorithm is as follows:

[0080] P1, record the end point of the initial path is the target point for tree growth, and the starting point of the initial path As the starting point of tree growth, record i Path nodes The first i There are nodes; each node is represented by:

[0081] ;

[0082] ;

[0083] ;

[0084] in, 、 、 The target point, the starting point, i Six joint angles of existing nodes;

[0085] P2, such as Figure 2 As shown, taking the first joint angle as an example, Generate gravitational probability field, probability density function of gravitational probability field For normal distribution, the formula is:

[0086] ;

[0087] ;

[0088] in, is the angle value of the first joint of the robot; is the mean value, and in the gravitational probability field its value is The first joint angle value of is the standard deviation;

[0089] P3, also taking the first joint angle as an example, the starting point With existing nodes Generate a repulsive probability field, and the probability density function of the repulsive probability field is set to an inverted normal distribution, which is expressed as follows:

[0090] ;

[0091] ;

[0092] ;

[0093] ;

[0094] in, is the probability density function of the repulsive force probability field, is the normal distribution function, which is used Substitute the calculation into ; is the mean, for , ;for , ; is the standard deviation, in this application 、 According to the experience setting, the probability density is controlled to be dispersed or concentrated according to the needs.

[0095] P4. Added a uniformly distributed probability density function within the range of joint angles. , increase the sampling probability of the end point and reduce the sampling probability of existing nodes. The formula is expressed as:

[0096] ;

[0097] in, is the minimum joint value within the reachable range of the first joint angle, is the maximum joint value within the reachable range of the first joint angle;

[0098] P5, probability density function of weighted mixed normal distribution, inverted normal distribution and uniform distribution, to obtain the overall sampling probability density in the first joint angle , the formula is:

[0099] ;

[0100] in, is the weight of the normal distribution, is a uniformly distributed weight, 、 For the i , j The weights of an inverted normal distribution, is the number of inverted normal distributions;

[0101] It should be noted that in this application, the number of inverted normal distributions is equal to the number of existing nodes plus the number of nodes, that is, the i-th inverted normal distribution corresponds to the i-th existing node. At the same time, for the convenience of formula expression, setting i=0 represents the starting point. .

[0102] Normalize the overall sampling probability density to ensure that the integral is 1. The formula is:

[0103] ;

[0104] in, is the overall sampling probability density after normalization of the first joint angle;

[0105] P6. Repeat steps P2-P5 to obtain the normalized overall adoption probability density of the remaining joint angles. Use the rejection sampling method to generate six randomly sampled joint values ​​according to the reachable range of the joints and merge them into the final sampling point.

[0106] Each sampling point in the tree corresponds to a path node; six random joint values ​​combined together specify a path node (the robot's six joint values ​​correspond to a unique point in space).

[0107] S3, after generating the initial path, rapid path detection is performed to detect whether there is an obstacle block and a trajectory block on the path; the rapid path detection includes forward path detection and reverse path detection, the forward path is a path in which the robot moves from a starting point to an end point, and the reverse path is a path in which the robot moves from the end point to the starting point.

[0108] As a preferred embodiment, as shown in Figure 3 , the rapid path detection specifically includes:

[0109] After obtaining the initial path, a virtual robot is established according to a CAD file by using a robot inverse solution, trajectory interpolation and a collision detection algorithm;

[0110] First, forward path detection is performed, the virtual robot is driven to move at a high speed from the starting point to the end point according to the initial path, the joint angles of the robot are recorded in real time during the movement, and when it is judged that the initial path will be blocked by an obstacle and collide, the last forward non-collision path point in the initial path is recorded ; in this application, the obstacle information is the human collision body information obtained in step S1;

[0111] When the forward path detection judges that the robot will collide when moving according to the initial path, reverse path detection is triggered, the virtual robot is driven to move at a high speed from the end point to the starting point according to the initial path, the joint angles of the robot are also recorded in real time during the movement, and according to the obstacle information, the last reverse non-collision path point in the initial path is recorded ;

[0112] Finally, the rapid path detection judges that the initial path between , will collide, and path re-planning is required.

[0113] If there is no block, a path optimization strategy is performed; specifically, the path optimization strategy includes:

[0114] First, rapid collision detection is performed on the path between two interval path points , , if the robot can directly move from to without collision, the intermediate path point is deleted; then the process is continued from , and the process is repeated until all interval path points between the path points will collide, the path with the intermediate path point deleted at this time is retained, and the optimization is completed.

[0115] S4, if it is detected that there is a block, path re-generation is performed again on the basis of the initial path based on a probability field RRT-Connect algorithm and a path optimization strategy;

[0116] As a preferred embodiment, step S4 is specifically:

[0117] get forward collision-free path point and reverse collision-free path point After that, the original path from the starting point to the end point to is retained, and and are taken as the new starting point and end point, and a new trajectory from to is planned based on the probabilistic field RRT-Connect;

[0118] Meanwhile, during path regeneration, the following mechanism is established: when the fast path detection determines that a collision will occur, but the real robot has not yet collided, on the one hand, path regeneration is triggered, and on the other hand, the real robot is set to move only according to the trajectory from to until a new path is generated, and if the real robot has reached the point , the real robot waits in place for a new path. This mechanism processes the real robot movement and path regeneration in parallel, and the planning algorithm reduces the real robot waiting time as much as possible, effectively improving the planning efficiency.

[0119] S5, repeat steps S3-S4 after path regeneration until the robot moves according to the path without collision.

[0120] It is apparent to those skilled in the art that the present application is not limited to the details of the above exemplary embodiments, but can be implemented in other specific forms without departing from the spirit or essential characteristics of the present application. Therefore, the embodiments should be considered in all respects as illustrative and not restrictive, and the scope of the present application is defined by the appended claims rather than the above description, and it is intended to encompass all changes falling within the meaning and range of equivalents of the elements of the claims. Any reference signs in the claims should not be considered as limiting the claims to which they relate.

[0121] Furthermore, it should be understood that although the present specification is described in terms of embodiments, not every embodiment contains only one independent technical solution, and the description of the specification is only for the sake of clarity, and those skilled in the art should consider the specification as a whole, and the technical solutions in each embodiment can be appropriately combined to form other embodiments that those skilled in the art can understand.

Claims

1. A dynamic obstacle avoidance method for industrial robots oriented to human-machine collaboration safety, characterized by: The specific steps include: S1. Obtain human collision body information in real time through the depth camera and update and synchronize it to the industrial robot coordinate system; Step S1 specifically includes: S11. Based on the color image obtained by the depth camera, use the OpenPose human detection network to detect key points of the human body in the color image and obtain the 2D coordinates of the key points of the human body in the color image; S12, according to the depth map provided by the depth camera, the 2D coordinates of the key points of the human body are mapped to the depth map to obtain the 3D coordinates of the key points of the human body, which is expressed as follows: ； ； in, is the 2D coordinate of the key point, is the value of the depth map on the corresponding pixel, is the internal parameter corresponding to the camera, 3D coordinates of key points of the human body; S13, using the obtained 3D coordinates of the key points of the human body, assembling the human body collision body in a split manner, modeling the collision body by taking the joints of the human limbs, the torso, and the neck as cylinders, and modeling the collision body by taking the head as a sphere, and assembling the whole collision body of the human body; S14, looping through steps S11-S13, performing real-time detection on the real-time image captured by the depth camera to obtain real-time 3D coordinates of key points of the human body; S2. Design a probability field RRT-Connect algorithm to quickly plan a feasible initial path. The probability field RRT-Connect algorithm adds probability field sampling to the RRT-Connect algorithm architecture, changing the sampling strategy of the two trees in the original algorithm. The probability field sampling integrates the gravitational field concept of the artificial potential field method, converting the attractive and repulsive forces into gravitational probability fields and repulsive probability fields. For a single tree, a gravitational probability field is generated at the joint value in the joint space corresponding to its endpoint, guiding the tree to explore the endpoint. Existing nodes in the tree generate repulsive probability fields, prompting the tree to grow and diverge. S3. After generating the initial path, perform a quick path detection to check whether there are obstacles or trajectory obstructions on the path. If there are no obstacles, perform a path optimization strategy. The quick path detection includes forward path detection and reverse path detection. The forward path is the path of the robot moving from the starting point to the end point, and the reverse path is the path of the robot moving from the end point to the starting point. Specifically: After obtaining the initial path, a virtual robot is built based on the CAD file using robot inverse solution, trajectory interpolation, and collision detection algorithms; First, perform forward path detection, drive the virtual robot to move at high speed from the starting point to the end point along the initial path, and record the angles of each joint of the robot in real time during the movement. According to the obstacle information, when it is determined that the initial path will be blocked by an obstacle and a collision will occur, record the last forward collision-free path point in the initial path. ; When the forward path detection determines that the robot will collide when moving along the initial path, the reverse path detection is triggered, and the virtual robot is driven to move at high speed from the end point to the starting point along the initial path. During the movement, the joint angles of the robot are also recorded in real time. According to the obstacle information, the last reverse collision-free path point in the initial path is recorded. ; Finally, the fast path detection determines that 、 The initial paths between them will collide and need to be replanned; S4. If occlusion is detected, the path is regenerated based on the probability field RRT-Connect algorithm and path optimization strategy on the basis of the initial path; S5. After the path is regenerated, steps S3-S4 are repeated until the robot moves along the path without collision.

2. The method for industrial robot dynamic obstacle avoidance for human-machine collaboration safety according to claim 1, characterized in that: The sampling strategy for each tree in the probability field RRT-Connect algorithm is as follows: P1, record the end point of the initial path is the target point for tree growth, and the starting point of the initial path As the starting point of tree growth, record i Path nodes The first i There are nodes; each node is represented by: ； ； ； in, 、 、 The target point, the starting point, i Six joint angles of existing nodes; P2, taking the first joint angle as an example, Generate gravitational probability field, probability density function of gravitational probability field For normal distribution, the formula is: ； ； in, is the angle value of the first joint of the robot; is the mean value, and in the gravitational probability field its value is The first joint angle value of is the standard deviation; P3, also taking the first joint angle as an example, the starting point With existing nodes Generate a repulsive probability field, and the probability density function of the repulsive probability field is set to an inverted normal distribution, which is expressed as follows: ； ； ； ； in, is the probability density function of the repulsive force probability field, is the normal distribution function, which is used Substitute the calculation into ; is the mean, for , ;for , ; is the standard deviation; P4. Added a uniformly distributed probability density function within the range of joint angles. , increase the sampling probability of the end point and reduce the sampling probability of existing nodes. The formula is expressed as: ； in, is the minimum joint value within the reachable range of the first joint angle, is the maximum joint value within the reachable range of the first joint angle; P5, probability density function of weighted mixed normal distribution, inverted normal distribution and uniform distribution, to obtain the overall sampling probability density in the first joint angle , the formula is: ； in, is the weight of the normal distribution, is a uniformly distributed weight, 、 For the i , j The weights of the inverted normal distribution, is the number of inverted normal distributions; Normalize the overall sampling probability density to ensure that the integral is 1. The formula is: ； in, is the overall sampling probability density after normalization of the first joint angle; P6. Repeat steps P2-P5 to obtain the normalized overall adoption probability density of the remaining joint angles. Use the rejection sampling method to generate six randomly sampled joint values ​​according to the reachable range of the joints, merge them into the final sampling point, and use it as a node in the tree.

3. The method for industrial robot dynamic obstacle avoidance for human-machine collaboration safety according to claim 1, characterized in that: The path optimization strategy in step S3 is specifically as follows: First, for two separated path points 、 The path between them is quickly collided. If the robot can directly Exercise to If no collision occurs, delete the intermediate path points ; Afterwards This process is continued until all paths between the paths collide with each other. The path with the intermediate paths removed is retained to complete the optimization.

4. The method for industrial robot dynamic obstacle avoidance for human-machine collaboration safety according to claim 1, characterized in that: Step S4 is specifically as follows: Get the forward collision-free path point and reverse collision-free pathpoints After that, keep the initial path starting point arrive , the initial path end point arrive The original path of and As the new starting point and end point, RRT-Connect planning based on probability field arrive new trajectory; At the same time, during the path regeneration, the following mechanism is established: when the fast path detection determines that a collision will occur, but the real robot has not collided, on the one hand, the path regeneration is triggered, and on the other hand, the real robot is set to only press to The trajectory moves until a new path is generated. If the real robot has reached point, the real robot waits for the new path in place.

Images