Robot path planning method in man-machine cooperation environment

Abstract

The invention discloses a robot path planning method in a man-machine cooperation environment, and the method specifically comprises the steps: collecting the three-dimensional point cloud data of an assembly environment, calibrating the relation between coordinate systems in the assembly environment, and recognizing an obstacle region; a robot joint envelope body is constructed according to joint connecting rod parameters of the robot and the three-dimensional model, and robot size description and collision detection are simplified; spatial grid non-uniform discretization is carried out on the assembly environment according to the barrier distribution density, grids are coded, the assembly environment is divided into grid spaces with different coarse and fine granularities, and the path search efficiency is improved; a fitness function is designed, a target path search algorithm is established based on a genetic algorithm framework, crossover and mutation operations of adaptive probability are considered, rapid high-quality population iteration is realized, and robot path planning is completed through the target path search algorithm. The robot path planning difficulty in a complex environment is improved, and the quality and smoothness of robot path planning can be improved.

Description

Technical Field

[0001] The present invention relates to the technical field of robot path planning, and in particular to a robot path planning method in a human-machine collaborative environment. Background Art

[0002] Robot path planning is essential for robot operation. Traditional robot path planning relies on point-by-point teaching programming in an offline simulation environment, which is only suitable for simple scenarios. Human-robot collaboration scenarios often require maneuvers in complex environments, requiring adaptation to obstacles of varying numbers and shapes to ensure safe human-robot collaboration. Traditional teaching programming is no longer suitable for robot path planning in complex human-robot collaborative environments, necessitating the development of intelligent path search algorithms.

[0003] While sampling-based motion planning methods, such as the probabilistic PRM algorithm and the RRT algorithm, avoid modeling the obstacle space, their solution quality is dependent on the number of samples taken, and collision detection is time-consuming. While methods such as artificial potential field algorithms, A* algorithms, and genetic algorithms require building models of the obstacle space, their simple and efficient computations have led to their widespread application. However, the quality of current robot path planning still needs to be improved to achieve smoother motion. Summary of the Invention

[0004] Based on the above problems existing in the prior art, the present invention proposes a robot path planning method in a human-machine collaborative environment to improve the difficulty of robot path planning in complex environments and enhance the quality and smoothness of robot movement.

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

[0006] A robot path planning method in a human-machine collaborative environment, which specifically includes:

[0007] S1. Collect 3D point cloud data of the assembly environment, calibrate the relationship between the coordinate systems in the assembly environment, and identify obstacle areas;

[0008] S2. Constructing the robot joint envelope based on the robot's joint link parameters and three-dimensional model;

[0009] S3, non-uniformly discretize the spatial grid of the assembly environment and encode the grid;

[0010] S4. Design a fitness function and build a target path search algorithm based on the genetic algorithm framework to complete the robot path planning through the target path search algorithm.

[0011] Furthermore, step S1 is specifically as follows:

[0012] S11. Collect 3D point cloud data of the assembly environment using LiDAR and binocular cameras, and use the improved ICP algorithm to align the robot base coordinate system with the coordinate systems of each target object and obstacle in the environment;

[0013] S12. Perform obstacle recognition by calculating the spatial inclusion relationship between the center point of the grid and the three-dimensional coordinates of the obstacle point cloud, and marking the grid that completely falls within the obstacle coordinate range as the obstacle area.

[0014] Furthermore, step S2 is specifically as follows:

[0015] The robot's joint link parameters are combined with the DH method to establish a joint coordinate system. Based on the robot's three-dimensional model, each irregular joint model of the robot is regularized and enveloped to simplify the robot's size description. Among them, the rotational joint is enveloped by a sphere, and the linear link is enveloped by a capsule. By replacing the irregular joint model with a sphere and a capsule, collision detection is simplified to the calculation of the minimum Euclidean distance from the surface of a standard geometric body to the obstacle.

[0016] Furthermore, step S3 is specifically as follows:

[0017] A multi-level grid division strategy is implemented according to the density of obstacle distribution. Fine-grained grid division is used in areas with dense obstacles, and coarse-grained grid division is used in open areas. At the same time, real numbers are used to encode the grid, and the obstacle range is marked according to the point cloud data, thereby constructing assembly environments with different coarse and fine granularity.

[0018] Furthermore, the design fitness function in step S4 is specifically:

[0019] The fitness function f is designed to be composed of the sum of the inverse of the total length of the path segments, the sum of the steering angles of adjacent path segments, the penalty value accumulated by the number of collisions, and the minimum distance from the path node to the obstacle. The collision penalty term only counts one penalty for a single collision. The formula is expressed as:

[0020]

[0021] Where n is the number of grid nodes, L i is the length of the i-th path segment, α j is the angle between the jth path segments, a, b, c, d are the weights of each item, and satisfy a+b+c+d=1, N is the number of collisions, P is the penalty imposed for each collision, k l is the distance from the lth path node to the nearest obstacle.

[0022] Furthermore, the target path search algorithm in step S4 specifically includes:

[0023] In the framework of genetic algorithm, combined with the fitness function, the robot's path planning is subjected to genetic algorithm initialization encoding, decoding, selection, crossover, mutation and elite retention operations, specifically:

[0024] P1. Sequence the possible path points of the robot in the assembly environment, connect the path points for preliminary planning, obtain multiple possible paths, perform initialization coding, and calculate the fitness function;

[0025] P2 performs a decoding operation and performs discrete operations on the decoded path, detecting point by point whether the distance between the robot and the obstacle is less than the specified distance. If it is less than the specified distance, a penalty is added to the fitness function. A collision on a line segment is counted only once.

[0026] P3. Before performing crossover and mutation operations, calculate the minimum fitness of the two selected chromosomes and compare it with the average fitness of the current generation, and adaptively adjust the crossover and mutation probabilities. After each crossover and mutation operation, recalculate the fitness of the offspring path, and select the best individual of the parent with the largest fitness to compare with the best individual of the offspring. If the best individual of the offspring is better, retain the best individual of the offspring; otherwise, replace it with the best individual of the parent to participate in the next population iteration.

[0027] P4. Perform multiple population iterations and updates to eventually obtain a smooth and high-quality robot planning path, achieving optimal collision-free motion for the robot.

[0028] More specifically, the intersection probability P in the target path search algorithm in step S4 c and mutation probability P m The specific calculation formula is:

[0029]

[0030] Among them, P c0 is the original crossover probability, f avg is the average fitness of the parent generation, f max is the highest fitness value of the parent generation, P m0 is the original mutation probability, δ f is the variance of the parent fitness.

[0031] More specifically, the crossover operation in the target path search algorithm in step S4 is divided into three cases, specifically:

[0032] If there is a waypoint with the same sequence number in the two selected chromosomes, crossover is performed at the same sequence number;

[0033] If there are multiple path points with the same sequence number in the two selected chromosomes, one of the same path points is selected for crossover;

[0034] If there are no waypoints with the same sequence number in the two selected chromosomes, a waypoint is randomly selected for crossover.

[0035] More specifically, the mutation operation in the target path search algorithm in step S4 is as follows:

[0036] First, select a mutation interval in the current path. Depending on whether a collision occurs within the mutation interval, perform two mutation operations:

[0037] Adding path points: If a collision occurs in the selected variation interval, add path nodes in the variation interval so that the robot can bypass obstacles without collision.

[0038] Reduce path points: If the selected variation interval does not pass through obstacles, delete the redundant path nodes in the variation interval, and ensure that no collision occurs after deleting the redundant path nodes;

[0039] If the selected variation interval does not meet the above conditions, reselect it.

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

[0041] 1. When searching for a path, the present invention not only considers the length of the search path, but also the angle between any two adjacent paths. By adjusting the ratio of the two, the optimization target can comprehensively optimize the path, achieving smoother and more efficient path planning. In addition, a penalty is imposed on colliding nodes, taking into account infeasible paths caused by collisions, and maintaining path diversity.

[0042] 2. This invention uses spheres and capsules to simplify the robot joint model. The simple joint envelope model can effectively improve the collision detection speed, thereby improving planning efficiency and providing a basis for algorithm application.

[0043] 3. This method divides the assembly environment into grid spaces of different granularities based on the density of obstacles, which can effectively improve the algorithm's path search efficiency;

[0044] 4. The present invention designs adaptive crossover probability and mutation probability in the genetic algorithm; by designing a continuously decreasing crossover probability, the population is prevented from reaching the local optimum due to premature convergence, and by associating the fitness variance with the mutation probability, the mutation probability changes with the volatility of the overall fitness of the population, thereby improving the breakthrough of the local optimum to reach the global optimum. BRIEF DESCRIPTION OF THE DRAWINGS

[0045] Figure 1 This is a flow chart of a robot path planning in a human-machine collaborative environment proposed by the present invention;

[0046] Figure 2 Schematic diagram of the assembly environment of the simulation experiment in the embodiment of the present invention;

[0047] Figure 3 Schematic diagram of the path planning results of the simulation experiment in an embodiment of the present invention. DETAILED DESCRIPTION

[0048] In order to make the above-mentioned objects, features and advantages of the present invention more clearly understood, the following Figure 1-3 The present invention is further described in detail with specific implementation methods, so that the application can fully understand how to use technical means to solve technical problems and achieve technical effects and implement them accordingly.

[0049] Those skilled in the art will appreciate that all or part of the steps in the above-mentioned embodiment methods can be accomplished by instructing the relevant hardware through a program. Therefore, the present application may take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Furthermore, the present application may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0050] like Figure 1 As shown, the present invention proposes a robot path planning method in a human-machine collaborative environment, which specifically includes the following steps:

[0051] S1. Collect 3D point cloud data of the assembly environment, calibrate the relationship between the coordinate systems in the assembly environment, and identify obstacle areas;

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

[0053] S11. Collect 3D point cloud data of the assembly environment using LiDAR and binocular cameras, and use the improved ICP algorithm to align the robot base coordinate system with the coordinate systems of each target object and obstacle in the environment;

[0054] S12. Obstacle identification is performed by calculating the spatial inclusion relationship between the grid center point and the three-dimensional coordinates of the obstacle point cloud, and marking the grid that completely falls within the obstacle coordinate range as the obstacle area. In this embodiment, the scanned point cloud is the outer surface of the obstacle. When the grid center falls within the point cloud, the grid is considered to belong to the obstacle range and is considered an obstacle during subsequent collision detection and path planning.

[0055] S2. Constructing the robot joint envelope based on the robot's joint link parameters and three-dimensional model;

[0056] As a preferred embodiment, step S2 specifically includes:

[0057] The robot's joint link parameters are combined with the DH method to establish a joint coordinate system. Based on the robot's three-dimensional model, each irregular joint model of the robot is regularized and enveloped to simplify the robot's size description. Among them, the rotational joint is enveloped by a sphere, and the linear link is enveloped by a capsule. By replacing the irregular joint model with a sphere and a capsule, collision detection is simplified to the calculation of the minimum Euclidean distance from the surface of a standard geometric body to the obstacle.

[0058] S3, non-uniformly discretize the spatial grid of the assembly environment and encode the grid;

[0059] As a preferred embodiment, step S3 is specifically as follows:

[0060] A multi-level grid division strategy is implemented according to the density of obstacle distribution. Fine-grained grid division is used in areas with dense obstacles and coarse-grained grid division is used in open areas. At the same time, real numbers are used to encode the grids and the obstacle range is marked according to the point cloud data, thereby constructing an assembly environment with different coarse and fine granularity. The final assembly environment is as follows: Figure 2 It should be noted that the way to mark the obstacle range here is consistent with that in step S1. Based on the traditional uniformly discrete grid space, grids of different coarse and fine granularity are divided according to the distribution of obstacles. This makes it easier to filter out redundant path nodes and fault areas in open areas and perform more detailed path planning, thus taking into account both efficiency and safety.

[0061] S4. Design a fitness function and build a target path search algorithm based on the genetic algorithm framework to complete the robot path planning through the target path search algorithm;

[0062] As a preferred embodiment, the design fitness function in step S4 is specifically:

[0063] The fitness function f is designed to be composed of the sum of the inverse of the total length of the path segments, the sum of the steering angles of adjacent path segments, the penalty value accumulated by the number of collisions, and the minimum distance from the path node to the obstacle. The collision penalty term only counts one penalty for a single collision. The formula is expressed as:

[0064]

[0065] Where n is the number of grid nodes, L i is the length of the i-th path segment, α j is the angle between the jth path segments, a, b, c, d are the weights of each item, and satisfy a+b+c+d=1, N is the number of collisions, P is the penalty imposed for each collision, k l is the distance from the lth path node to the nearest obstacle.

[0066] As a preferred embodiment, the target path search algorithm in step S4 specifically includes:

[0067] In the framework of genetic algorithm, combined with the fitness function, the robot's path planning is subjected to genetic algorithm initialization encoding, decoding, selection, crossover, mutation and elite retention operations, specifically:

[0068] P1. Sequence the possible path points of the robot in the assembly environment, connect the path points for preliminary planning, obtain multiple possible paths, perform initialization coding, and calculate the fitness function;

[0069] P2 performs a decoding operation and performs discrete operations on the decoded path, detecting point by point whether the distance between the robot and the obstacle is less than the specified distance. If it is less than the specified distance, a penalty is added to the fitness function. A collision on a line segment is counted only once.

[0070] P3. Before performing crossover and mutation operations, calculate the minimum fitness of the two selected chromosomes and compare it with the average fitness of the current generation to adaptively adjust the crossover and mutation probabilities.

[0071] In this embodiment, the crossover probability P c and mutation probability P m The specific calculation formula is:

[0072]

[0073] Among them, P c0 is the original crossover probability, f avg is the average fitness of the parent generation, f max is the highest fitness value of the parent generation, P m0 is the original mutation probability, δ f is the variance of the parent fitness; in this embodiment, in the early stage of population iteration, a higher original crossover probability is designed to obtain a higher exploration ability and enrich the diversity of the population; as the iteration proceeds, it gradually approaches the optimal population, so the crossover probability needs to be appropriately reduced to obtain the optimal solution through fine search; however, as the iteration proceeds, it is easy to fall into the local optimum. At this time, increasing the mutation probability can improve the breakthrough of the local optimum to achieve the global optimum; when calculating the crossover probability, since the average fitness is definitely lower than the maximum fitness, this calculation is more appropriate; when calculating the mutation probability, if the ratio of the above average fitness to the maximum fitness is still used, it may lead to a larger mutation probability, causing the population to deviate from the global optimum; therefore, the ratio of the fitness variance to the average fitness is selected, and the volatility of the overall fitness is measured by the variance. Compared with the average fitness, the increase in the mutation probability can be determined according to the state of the population.

[0074] After each crossover and mutation operation, the fitness of the offspring path is recalculated, and the optimal individual of the parent with the largest fitness is selected for comparison with the optimal individual of the offspring. If the optimal individual of the offspring is better, the optimal individual of the offspring is retained, otherwise it is replaced by the optimal individual of the parent to participate in the next population iteration.

[0075] In this embodiment, the crossover operation is divided into three cases, specifically:

[0076] If there is a waypoint with the same sequence number in the two selected chromosomes, crossover is performed at the same sequence number;

[0077] If there are multiple path points with the same sequence number in the two selected chromosomes, one of the same path points is selected for crossover;

[0078] If there are no waypoints with the same sequence number in the two selected chromosomes, a random waypoint is selected for crossover;

[0079] The mutation operations are as follows:

[0080] First, select a mutation interval in the current path. Depending on whether a collision occurs within the mutation interval, perform two mutation operations:

[0081] Adding path points: If a collision occurs in the selected variation interval, add path nodes in the variation interval so that the robot can bypass obstacles without collision.

[0082] Reduce path points: If the selected variation interval does not pass through obstacles, delete the redundant path nodes in the variation interval, and ensure that no collision occurs after deleting the redundant path nodes;

[0083] If the selected mutation interval does not meet the above conditions (that is, there are no obstacles and no collisions in the selected interval, but there are no redundant nodes, and the interval needs to be reselected), then reselect.

[0084] P4. Perform multiple population iterations and updates to eventually obtain a smooth and high-quality robot planning path, achieving optimal collision-free motion for the robot.

[0085] Thus, the robot path planning method in a human-machine collaborative environment proposed by the present invention can obtain a smooth, collision-free planned path; this embodiment also provides the following simulation experiment:

[0086] According to the method proposed by the present invention, first obtain Figure 2 The obstacle density in the simulated assembly environment is 16.293%. To verify the effectiveness of the proposed path algorithm, the fitness function and target path search algorithm parameters are set as shown in Table 1:

[0087] Table 1 Parameter setting values

[0088]

[0089] A total of 10 simulations were performed, and the simulation results are shown in Table 2 below. The average simulation time was 0.369s;

[0090] Table 2 10 simulation results

[0091]

[0092] In addition, the optimal path was found after 100 population iterations, and the algorithm converged relatively stably. Figure 3 As shown, it shows that the invention can quickly search for high-quality feasible paths, verifying the effectiveness of the proposed path planning algorithm.

[0093] In summary, the robot path planning method in a human-machine collaborative environment proposed in the present invention can quickly obtain the robot's optimal collision-free path planning in a complex human-machine collaborative environment, thereby improving the difficulty of robot path planning in a complex environment and enhancing the quality and smoothness of the robot's movement.

[0094] In the description of this specification, the reference terms "one embodiment," "some embodiments," "example," "specific example," or "some examples" mean that the specific features, structures, materials, or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present application. Moreover, the specific features, structures, materials, or characteristics described may be combined in any appropriate manner in any one or more embodiments or examples. In addition, those skilled in the art may combine and combine different embodiments or examples described in this specification, as well as features of different embodiments or examples, unless they are mutually inconsistent.

[0095] The logic and / or steps represented in the flowchart or otherwise described herein may be considered, for example, as an ordered list of executable instructions for implementing logical functions, and may be embodied in any computer-readable medium for use by, or in conjunction with, an instruction execution system, apparatus, or device (such as a computer-based system, a system including a processor, or other system that can fetch and execute instructions from an instruction execution system, apparatus, or device).

[0096] The above embodiments provide a detailed introduction to the present invention. Specific examples are used herein to illustrate the principles and implementation methods of the present invention. The description of the above embodiments is only used to help understand the method of the present invention and its core ideas. At the same time, for those skilled in the art, according to the ideas of the present invention, there may be changes in the specific implementation methods and application scopes. In summary, the contents of this specification should not be understood as limiting the present invention.

Claims

1. A robot path planning method in a human-machine collaborative environment, characterized in that: The specific steps include: S1. Collect 3D point cloud data of the assembly environment, calibrate the relationship between the coordinate systems in the assembly environment, and identify obstacle areas; S2. Constructing the robot joint envelope based on the robot's joint link parameters and three-dimensional model; S3, non-uniformly discretize the spatial grid of the assembly environment and encode the grid; S4. Design a fitness function and build a target path search algorithm based on the genetic algorithm framework to complete the robot path planning through the target path search algorithm.

2. The robot path planning method in a human-machine collaborative environment according to claim 1, characterized in that: Step S1 is specifically as follows: S11. Collect 3D point cloud data of the assembly environment using LiDAR and binocular cameras, and use the improved ICP algorithm to align the robot base coordinate system with the coordinate systems of each target object and obstacle in the environment; S12. Perform obstacle recognition by calculating the spatial inclusion relationship between the center point of the grid and the three-dimensional coordinates of the obstacle point cloud, and marking the grid that completely falls within the obstacle coordinate range as the obstacle area.

3. The robot path planning method in a human-machine collaborative environment according to claim 1, characterized in that: Step S2 is specifically as follows: The robot's joint link parameters are combined with the DH method to establish a joint coordinate system. Based on the robot's three-dimensional model, each irregular joint model of the robot is regularized and enveloped to simplify the robot's size description. Among them, the rotational joint is enveloped by a sphere, and the linear link is enveloped by a capsule. By replacing the irregular joint model with a sphere and a capsule, collision detection is simplified to the calculation of the minimum Euclidean distance from the surface of a standard geometric body to the obstacle.

4. The robot path planning method in a human-machine collaborative environment according to claim 1, characterized in that: Step S3 is specifically as follows: A multi-level grid division strategy is implemented according to the density of obstacle distribution. Fine-grained grid division is used in areas with dense obstacles, and coarse-grained grid division is used in open areas. At the same time, real numbers are used to encode the grid, and the obstacle range is marked according to the point cloud data, thereby constructing assembly environments with different coarse and fine granularity.

5. The robot path planning method in a human-machine collaborative environment according to claim 1, characterized in that: The design fitness function in step S4 is specifically: The fitness function f is designed to be composed of the sum of the inverse of the total length of the path segments, the sum of the steering angles of adjacent path segments, the penalty value accumulated by the number of collisions, and the minimum distance from the path node to the obstacle. The collision penalty term only counts one penalty for a single collision. The formula is expressed as: Where n is the number of grid nodes, L i is the length of the i-th path segment, α j is the angle between the jth path segments, a, b, c, d are the weights of each item, and satisfy a+b+c+d=1, N is the number of collisions, P is the penalty imposed for each collision, k l is the distance from the lth path node to the nearest obstacle.

6. The robot path planning method in a human-machine collaborative environment according to claim 1, characterized in that: The target path search algorithm in step S4 specifically includes: In the framework of genetic algorithm, combined with the fitness function, the robot's path planning is subjected to genetic algorithm initialization encoding, decoding, selection, crossover, mutation and elite retention operations, specifically: P1. Sequence the possible path points of the robot in the assembly environment, connect the path points for preliminary planning, obtain multiple possible paths, perform initialization coding, and calculate the fitness function; P2 performs a decoding operation and performs discrete operations on the decoded path, detecting point by point whether the distance between the robot and the obstacle is less than the specified distance. If it is less than the specified distance, a penalty is added to the fitness function. A collision on a line segment is counted only once. P3. Before performing crossover and mutation operations, calculate the minimum fitness of the two selected chromosomes and compare it with the average fitness of the current generation, and adaptively adjust the crossover and mutation probabilities. After each crossover and mutation operation, recalculate the fitness of the offspring path, and select the best individual of the parent with the largest fitness to compare with the best individual of the offspring. If the best individual of the offspring is better, retain the best individual of the offspring; otherwise, replace it with the best individual of the parent to participate in the next population iteration. P4. Perform multiple population iterations and updates to eventually obtain a smooth and high-quality robot planning path, achieving optimal collision-free motion for the robot.

7. The robot path planning method in a human-machine collaborative environment according to claim 6, characterized in that: The crossover probability P in the target path search algorithm in step S4 c and mutation probability P m The specific calculation formula is: Among them, P c0 is the original crossover probability, f avg is the average fitness of the parent generation, f max is the highest fitness value of the parent generation, P m0 is the original mutation probability, δ f is the variance of the parent fitness.

8. The robot path planning method in a human-machine collaborative environment according to claim 6, characterized in that: The crossover operation in the target path search algorithm in step S4 is divided into three cases, specifically: If there is a waypoint with the same sequence number in the two selected chromosomes, crossover is performed at the same sequence number; If there are multiple path points with the same sequence number in the two selected chromosomes, one of the same path points is selected for crossover; If there are no waypoints with the same sequence number in the two selected chromosomes, a waypoint is randomly selected for crossover.

9. The robot path planning method in a human-machine collaborative environment according to claim 6, characterized in that: The mutation operation in the target path search algorithm in step S4 is specifically: First, select a mutation interval in the current path. Depending on whether a collision occurs within the mutation interval, perform two mutation operations: Adding path points: If a collision occurs in the selected variation interval, add path nodes in the variation interval so that the robot can bypass obstacles without collision. Reduce path points: If the selected variation interval does not pass through obstacles, delete the redundant path nodes in the variation interval, and ensure that no collision occurs after deleting the redundant path nodes; If the selected variation interval does not meet the above conditions, reselect the variation interval.

Images