Master-slave dual-robot path planning method based on key point optimization genetic algorithm

Abstract

The invention discloses a master-slave dual-robot path planning method based on a key point optimization genetic algorithm, and the method is based on the genetic algorithm, and selects key points representing path characteristics according to angle conditions, distance conditions and the like for solving the problem that the redundancy of paths generated by a conventional path planning method is too high. Therefore, the redundancy in genetic optimization is reduced; aiming at the problem of poor path coordination caused by independent generation of robot paths in path planning of a double-mobile robot system, a master-slave framework is adopted, slave robot tracks are generated according to a master robot track and combined to serve as an initial feasible solution of a genetic algorithm, and a comprehensive fitness function considering coordination of master and slave robots is designed according to the framework. And meanwhile, a nonlinear crossover and mutation operator is introduced, so that the exploration capability of the optimization algorithm is further improved, and the algorithm is guided to iteratively optimize a safe and coordinated dual-robot collaborative path.

Description

Technical Field

[0001] The present invention belongs to the technical field of mobile robot path planning, and particularly relates to a master-slave dual-robot path planning method based on key-point optimized genetic algorithm. Background Technique

[0002] In the fields of modern industry, logistics, and warehousing management, dual-robot collaborative handling is a widely used technical means. By having two robots work together, efficient handling and precise placement of complex goods can be achieved. In this process, dual-robot collaborative path planning is one of the key technologies, and its purpose is to generate safe, efficient, and coordinated motion paths for two collaborating robots to complete the handling task.

[0003] In the research of traditional path planning, traditional planning algorithms such as the A* algorithm, Dijkstra algorithm, RRT algorithm, etc. These methods can generate feasible paths for a single robot to avoid obstacles. However, when these methods are applied to a dual-robot system, the generated paths often contain a large number of redundant base points, resulting in a high path redundancy. At the same time, the paths of the two robots are often generated independently, lacking effective relevance and coordination. This lack of coordination may lead to motion conflicts or disconnections between the robots, thus affecting the smooth completion of the task. Summary of the Invention

[0004] The purpose of the present invention is to provide a master-slave dual-robot path planning method based on key-point optimized genetic algorithm for the problems existing in the above-mentioned prior art.

[0005] The technical solution to achieve the purpose of the present invention is: A master-slave dual-robot path planning method based on key-point optimized genetic algorithm, the method comprising the following steps:

[0006] Step 1, using the grid method to model the environment to be planned, and the model includes the starting position, ending position of the master robot, and obstacle information;

[0007] Step 2, determining the obstacle expansion radius of the grid map according to the size of the master robot itself;

[0008] Step 3, extracting the key points of the master-slave robot paths and initializing the population; specifically including:

[0009] Obtaining the feasible path of the master robot, taking out the key points from the set of feasible path points, and encoding them in order as the initial solution of the master robot path;

[0010] Calculating the vertical offset vector between every two adjacent key points in the feasible path of the master robot, and calculating the key points of the slave robot path through the given offset between the master and slave robots, and encoding them in order as the initial solution of the slave robot path;

[0011] Concatenate the initial solution of the master robot path and the initial solution of the slave robot path into the initial solution of the population, and then repeat the above process until the required number of initialized populations is obtained;

[0012] Step 4: Select some individuals from the population as the parents for crossover and mutation operations according to the fitness function of the individuals;

[0013] Step 5: Perform improved crossover and mutation operations on the initialized population to obtain offspring;

[0014] Step 6: Use the deletion operator to perform pruning operations on the feasible solutions of the population;

[0015] Step 7: Combine the offspring and the parent feasible solutions generated by crossover, mutation, and deletion operations, and select the new generation of feasible solutions in descending order according to the population fitness function;

[0016] Step 8: Determine whether the iteration termination condition of the population is reached. If so, decode and output the optimal key points of the robot path. If not, jump to Step 5 and continue to perform the iteration operation;

[0017] Step 9: Connect the key points of the master and slave robots in sequence and perform smoothing processing to obtain the globally optimal path of the master and slave robots.

[0018] Furthermore, the calculation formula for the expansion radius of the grid map obstacle in Step 2 is:

[0019]

[0020] In the formula, L and W are the maximum vertical distance and horizontal distance from the centroid of the master robot to the outer contour respectively, R safe is the safety margin, and R di is the expansion radius of the grid map obstacle.

[0021] Furthermore, in Step 3, taking out the key points from the set of feasible path points specifically includes:

[0022] Take the path points in the set of feasible path points that satisfy at least one of the following conditions as the key point kp i :

[0023] x i = x 0 , y i = y 0

[0024] x i = x f , y i = y f

[0025]

[0026] Wherein, x i , y i are respectively the horizontal and vertical coordinates of the i-th key point kp i , x i+1 , y i+1 are respectively the horizontal and vertical coordinates of the (i + 1)-th key point kp i+1 , x i-1 , y i-1 are respectively the horizontal and vertical coordinates of the (i - 1)-th key point kp i-1 , x 0 , y 0 are respectively the horizontal and vertical coordinates of the initial position kp 0 , that is, the starting position of the master robot, x f , y f are respectively the horizontal and vertical coordinates of the termination position kp f , that is, the termination position of the master robot, θ 0 is the preset angle threshold, d 0 is the preset distance threshold, x ob , y ob are the horizontal and vertical coordinates of the obstacle closest to the path point.

[0027] Furthermore, in step 3, calculate the vertical offset vector between every two adjacent key points in the feasible path of the master robot, and calculate the path key points of the slave robot through the given offset between the master and slave robots. The specific calculation formula is:

[0028] (x′ i , y′ i ) = (x i , y i ) + d off ·v i

[0029] Wherein, x′ i , y′ i are respectively the horizontal and vertical coordinates of the path key point of the slave robot corresponding to the i-th key point kp i , d off is the given offset between the master and slave robots, v i is the vertical offset vector between every two adjacent key points in the feasible path of the master robot.

[0030] Furthermore, the fitness function of the individual described in step 4 is comprehensively determined by the robot path length function, the smoothness function of the robot path, the safety function of the robot path, and the coordination function of the master and slave robots. Specifically:

[0031] fit = μ1 fit 1 +μ 2 fit 2 +μ 3 P co

[0032] Among them,

[0033] fit 1 = ω 1 P L + ω 2 P c + ω 3 P s

[0034] fit 2 = ω′ 1 P′ L + ω′ 2 P′ c + ω′ 3 P′ s

[0035] In the formula, fit is the fitness function of the population, fit 1 is the fitness function of the master robot, fit 2 is the fitness function of the slave robot, ω 1 ~ ω 3 , ω′ 1 ~ ω′ 3 , μ 1 ~ μ 3 are all weight coefficients, P L , P′ L are the path length functions of the master and slave robots respectively, P c , P′ c are the path smoothness functions of the master and slave robots respectively, P s , P′ s are the path safety functions of the master and slave robots respectively, P co is the coordination function of the master and slave robots.

[0036] Furthermore, the path length function of the master-slave robot is defined as the sum of the distances between all consecutive two key points in the path:

[0037]

[0038] In the formula, (x i , y i ), (x i+1 , y i+1 ) represent two consecutive key points respectively, and n represents the total number of key points;

[0039] The path smoothness function of the master-slave robot is defined as the sum of the squares of the second-order differences of all key points in the path:

[0040]

[0041] In the formula, ||Δ 2 kp i || 2 represents the second-order difference of the key point kp i ;

[0042] The path safety function of the master-slave robot is:

[0043] P s =-λ 1 P s1 +λ 2 P s2

[0044] Among them, P s1 is the robot path collision penalty term, P s2 is the safety gap term, λ 1 , λ 2 are the weight coefficients of P s1 and P s2 respectively;

[0045] The coordination function of the master-slave robot is:

[0046]

[0047] In the formula, D i,min is the distance between the key point kp′ i in the path of the slave robot and the key point in the path of the master robot closest to it, and d off is the given offset between the master-slave robots.

[0048] Furthermore, the robot path collision penalty term P s1 is:

[0049]

[0050] Among them,

[0051]

[0052] In the formula, φ i is the collision penalty term of the key point kp i , d(kp i ,ob mi ) is the distance from kp i to the nearest obstacle ob mi , and d safe is the predefined safety distance;

[0053] The safety clearance term P s2 is:

[0054]

[0055] Furthermore, for the improved crossover in step 5, the improved crossover operator specifically includes:

[0056] The first crossover operator:

[0057] Generate the offspring by combining the parent path base points through a non - linear function, as shown in the following formula:

[0058]

[0059] In the formula, and are the abscissa and ordinate of the key points of two parents respectively, x of , y of are the abscissa and ordinate of the offspring respectively, ξ 1 , ξ 2 , ξ 3 , ξ 4 are randomly generated weights that satisfy the normalization condition ξ 1 +ξ 2 +ξ 3 +ξ 4 = 1;

[0060] The second crossover operator:

[0061] Combine two parents in a random manner, as shown in the following formula:

[0062]

[0063] In the formula, η follows a uniform distribution on [- 1,1].

[0064] Furthermore, for the improved mutation operation in step 5, the improved mutation operator specifically includes:

[0065] The first mutation operator:

[0066] By randomly selecting a key point in the parent and moving the key point a certain distance in the direction of the end point to obtain an offspring, as shown in the following formula:

[0067] (x of ,y of )=(x pa ,y pa )+γ·v pa,g

[0068] In the formula, v pa,gis the direction vector from the parent to the end point, γ is a coefficient, and x pa and y pa are the abscissa and ordinate of the key points of the parent respectively, and x of and y of are the abscissa and ordinate of the offspring respectively;

[0069] The second mutation operator:

[0070] By randomly selecting a finite number of consecutive key points between a certain key point as the starting point and another key point as the ending point in the parent, a feasible path is reconstructed, and the key points of the new feasible path are obtained by the method of taking key points from the set of feasible path points in step 3, and the original key points are replaced to obtain an offspring.

[0071] Furthermore, the deletion operator described in step 6 is specifically: evaluate the key points of the individuals in the feasible solution set in order, and if deleting a certain key point can enhance the fitness of the individual, remove it.

[0072] Compared with the prior art, the significant advantages of the present invention are:

[0073] (1) Aiming at the problem of excessive redundancy of the paths generated by the traditional path planning method, key points representing the path characteristics are selected according to angle conditions, distance conditions, etc., so as to reduce the redundancy in genetic optimization.

[0074] (2) Aiming at the problem of poor path coordination caused by the independent generation of robot paths in the path planning of a dual mobile robot system, a master-slave framework is adopted. The trajectory of the slave robot is generated according to the trajectory of the master robot and combined as the initial feasible solution of the genetic algorithm, and a comprehensive fitness function considering the coordination of the master and slave robots is designed based on this framework. At the same time, non-linear crossover and mutation operators are introduced to further improve the exploration ability of the optimization algorithm, so as to guide the algorithm to iteratively optimize a safe and coordinated cooperative path for the dual robots.

[0075] (3) Ensure that the starting point and the ending point of the optimized path are the given targets. Secondly, maximize the smoothness and safety of the path. Finally, a large number of redundant path points generated by random sampling are removed, simplifying the initial path population and improving the calculation efficiency.

[0076] (4) Key points for representing the geometric characteristics of the path are proposed, including the starting and ending points of the path, points with large direction changes, points close to obstacles, etc.; at the same time, a fitness function and non-linear genetic operators suitable for the key point path optimization of master-slave robots are designed, which can efficiently represent path characteristics, improve path planning efficiency, strengthen the master-slave coordination of path planning, and improve the global search ability of the algorithm, etc.

[0077] The present invention will be further described in detail below with reference to the accompanying drawings. Brief Description of the Drawings

[0078] Figure 1 It is a flowchart of the improved genetic algorithm proposed in the present invention in an embodiment.

[0079] Figure 2 It is a diagram of the robot size parameters.

[0080] Figure 3 It is a schematic diagram of the path planning process of the present invention in an embodiment. Detailed Embodiment

[0081] In order to make the objectives, technical solutions and advantages of the present application clearer, the present application will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application and are not used to limit the present application.

[0082] It should be noted that if there are directional indications (such as up, down, left, right, front, back...) involved in the embodiments of the present invention, the directional indications are only used to explain the relative positional relationship and movement conditions between components in a specific posture (as shown in the accompanying drawings). If the specific posture changes, the directional indications will also change accordingly.

[0083] In addition, if there are descriptions such as "first" and "second" involved in the embodiments of the present invention, the descriptions of "first" and "second" are only for descriptive purposes and cannot be understood as indicating or implying their relative importance or implicitly indicating the quantity of the indicated technical features. Thus, the features defined with "first" and "second" may explicitly or implicitly include at least one of such features. In addition, the technical solutions between various embodiments can be combined with each other, but it must be based on the fact that those skilled in the art can implement it. When the combination of technical solutions is contradictory or cannot be implemented, it should be considered that such a combination of technical solutions does not exist and is not within the protection scope required by the present invention.

[0084] In one embodiment, in combination with Figure 1 , a master-slave dual-robot path planning method based on key-point optimized genetic algorithm is provided, and the method includes the following steps:

[0085] Step 1, using the grid method to model the environment to be planned, and the model includes the starting position, the ending position of the master robot, and obstacle information;

[0086] Step 2, determining the obstacle inflation radius of the grid map according to the size of the master robot itself;

[0087] Step 3, extracting the path key points of the master-slave robots and initializing the population; specifically including:

[0088] Obtain the feasible path of the master robot, extract the key points from the set of feasible path points, and encode them in order as the initial solution of the master robot path;

[0089] Calculate the vertical offset vector between every two adjacent key points in the feasible path of the master robot, and calculate the key points of the slave robot path through the given offset between the master and slave robots, and encode them in order as the initial solution of the slave robot path;

[0090] Concatenate the initial solution of the master robot path and the initial solution of the slave robot path into the initial solution of the population, and then repeat the above process until the required number of initialized populations is obtained;

[0091] Step 4, according to the fitness function of the individuals, select some individuals from the population as the parents for crossover and mutation operations;

[0092] Step 5, perform improved crossover and mutation operations on the initialized population to obtain offspring;

[0093] Step 6, use the deletion operator to prune the feasible solutions of the population;

[0094] Step 7, merge the offspring and the parent feasible solutions generated by crossover, mutation and deletion operations, and select the new generation of feasible solutions in descending order according to the population fitness function;

[0095] Step 8, determine whether the iteration termination condition of the population is reached. If so, decode and output the optimal key points of the robot path (the path with the highest fitness, decode it and output the optimal KP of the robot path, that is, the key points). If not, jump to Step 5 and continue to perform the iteration operation;

[0096] Step 9, connect the key points of the master and slave robots in order in sequence and perform smoothing processing, and then obtain the global optimal path of the master and slave robots. Here, preferably, the smoothing processing is performed based on the cubic spline method.

[0097] Figure 3 It is a schematic diagram of the global path of the master and slave robots generated according to this method.

[0098] Furthermore, in one of the embodiments, the calculation formula for the expansion radius of the grid map obstacle in Step 2 is:

[0099]

[0100] In the formula, L and W are respectively the maximum vertical distance and the horizontal distance from the centroid of the master robot to the outer contour (as Figure 2 shown), R safe is the safety margin, and R di is the expansion radius of the grid map obstacle.

[0101] Further, in one of the embodiments, step 3 specifically includes:

[0102] Step 3-1: Use the RRT method to obtain the feasible path of the master robot, and sequentially extract the key point sequence KP from the feasible path point set to form the initial solution of the master robot path;

[0103] For the path points that satisfy at least one of the following conditions are used as the key points kp i :

[0104] x i = x 0 , y i = y 0

[0105] x i = x f , y i = y f

[0106]

[0107] In the formula, x i , y i are respectively the horizontal and vertical coordinates of the i-th key point kp i , x i+1 , y i+1 are respectively the horizontal and vertical coordinates of the (i + 1)-th key point kp i+1 , x i-1 , y i-1 are respectively the horizontal and vertical coordinates of the (i - 1)-th key point kp i-1 , x 0 , y 0 are respectively the horizontal and vertical coordinates of the initial position kp 0 , that is, the starting position of the master robot, x f , y f are respectively the horizontal and vertical coordinates of the termination position kp f , that is, the termination position of the master robot, θ 0 is the preset angle threshold, d 0 is the preset distance threshold, x ob , y ob are the horizontal and vertical coordinates of the obstacle closest to the path point.

[0108] Step 3-2: Given the offset d off between the master and slave robots, calculate the vertical offset vector v i between every two adjacent key points in the master robot path, and obtain the corresponding key point kp' i of the slave robot path according to the following formula, and use kp'i Sequentially coded as the initial solution KP' of the robot path:

[0109] (x' i , y' i ) = (x i , y i ) + d off ·v i

[0110] In the formula, x' i , y' i are the horizontal and vertical coordinates of the key point kp' of the robot path respectively; i

[0111] Step 3-3, splice KP and KP' into the coded feasible solution K f , and place it into the population POP. Repeat Step 3.1 and Step 3.2 until an initialized population with a sufficient number is obtained.

[0112] Furthermore, in Step 4, according to the fitness function of the individual, use the roulette wheel selection method to select some individuals from the population as the parents for the crossover and mutation operations. The selection probability of each individual is proportional to its fitness. Therefore, better individuals have more chances to be selected. In addition, construct the POP of the i-th iteration based on the ranking from the pool composed of the population of the (i - 1)-th iteration and the new offspring obtained by the genetic operator. The fitness function of the population needs to meet the requirements of the master-slave robot path length, smoothness, and safety, and at the same time ensure the motion coordination between the master-slave robot trajectories.

[0113] The fitness function of the said individual is comprehensively determined by the robot path length function, the smoothness function of the robot path, the robot path safety function, and the master-slave robot coordination function, specifically as:

[0114] fit = μ 1 fit 1 + μ 2 fit 2 + μ 3 P co

[0115] Among them,

[0116] fit 1 = ω 1 P L + ω 2 P c + ω 3 P s

[0117] fit 2 = ω' 1 P'​L +ω′ 2 P′ c +ω′ 3 P′ s

[0118] where fit is the fitness function of the population, fit 1 is the fitness function of the master robot, fit 2 is the fitness function of the slave robot, ω 1 ~ω 3 ,ω′ 1 ~ω′ 3 ,μ 1 ~μ 3 are all weight coefficients, P L 、P′ L are the path length functions of the master and slave robots respectively, P c 、P′ c are the path smoothness functions of the master and slave robots respectively, P s 、P′ s are the path safety functions of the master and slave robots respectively, P co is the coordination function of the master and slave robots.

[0119] Preferably, in some embodiments, the path length function of the master and slave robots is defined as the sum of the distances between all consecutive two key points in the path:

[0120]

[0121] where (x i , y i ), (x i+1 , y i+1 ) represent two consecutive key points respectively, and n represents the total number of key points;

[0122] The path smoothness function of the master and slave robots is defined as the sum of the squares of the second-order differences of all key points in the path:

[0123]

[0124] where ||Δ 2 kp i || 2 represents the second-order difference of the key point kp i .

[0125] Preferably, in some embodiments, the path safety function of the master and slave robots is:

[0126] P s =-λ 1 P s1 +λ 2 Ps2

[0127] Among them, P s1 is the robot path collision penalty term, and P s2 is the safety gap term, and λ 1 , and λ 2 are the weight coefficients of P s1 and P s2 respectively.

[0128] Among them, the robot path collision penalty term P s1 is:

[0129]

[0130] Among them,

[0131]

[0132] In the formula, φ i is the collision penalty term of the key point kp i , d(kp i , ob mi ) is the distance from kp i to the nearest obstacle ob mi , and d safe is the predefined safety distance;

[0133] The safety gap term P s2 is:

[0134]

[0135] Preferably, in some embodiments, in order to ensure as much as possible that the slave robot always follows the master robot and maintains a reasonable and safe spatial distance from it, avoiding both collisions caused by being too close and loss of cooperation caused by being too far away, based on the spatial distance constraint, the coordination function of the master-slave robots is designed as:

[0136]

[0137] In the formula, D i,min is the distance between the key point kp' i in the path of the slave robot and the key point in the path of the master robot closest to it, and d off is the given offset between the master and slave robots.

[0138] Furthermore, in one of the embodiments, step 5 specifically includes:

[0139] Step 5-1, perform a crossover operation with an appropriate probability. The crossover operator combines two parent Ks with the same or different numbers of KPs fCombined to produce a new offspring. The crossover operators adopted include two types:

[0140] The first crossover operator:

[0141] The base points of the parental paths are combined through a non - linear function to generate an offspring as shown in the following formula:

[0142]

[0143] In the formula, and are the horizontal and vertical coordinates of the key points of two parents respectively, x of , y of are the horizontal and vertical coordinates of the offspring respectively, ξ 1 , ξ 2 , ξ 3 , ξ 4 are randomly generated weights that satisfy the normalization condition ξ 1 + ξ 2 + ξ 3 + ξ 4 =1;

[0144] The second crossover operator:

[0145] Two parents are combined in a random way as shown in the following formula:

[0146]

[0147] In the formula, η follows a uniform distribution on [-1, 1].

[0148] The first crossover operator can generate a path with better smoothness to a certain extent. The second crossover operator improves the search ability of the algorithm and reduces the possibility of premature convergence. It should be noted that when the number of key points of the two parents to be crossed is different, all the key points of the parent with fewer key points are selected and combined with the key points closest in distance in the parent with more key points to generate an offspring.

[0149] Step 5 - 2, perform a mutation operation with an appropriate probability. The mutation operator changes one or some of the key points in a K f to produce a new offspring. The improved mutation operators adopted specifically include:

[0150] The first mutation operator:

[0151] By randomly selecting a key point in the parent and moving the key point a certain distance towards the end point to obtain an offspring as shown in the following formula:

[0152] (x of , y of ) = (x pa , ypa ) + γ·v pa,g

[0153] In the formula, v pa,g is the direction vector from the parent to the end point, γ is a coefficient, x pa , y pa are respectively the horizontal and vertical coordinates of the key points of the parent, x of , y of are respectively the horizontal and vertical coordinates of the offspring;

[0154] The second mutation operator:

[0155] By randomly selecting a finite number of consecutive key points between a certain key point as the starting point and another key point as the ending point in the parent, the RRT method is used to reconstruct a feasible path, and the key points of the new feasible path are obtained by the method of taking out the key points from the set of feasible path points in step 3, and the original key points are replaced to obtain an offspring.

[0156] The two mutation operators further improve the global exploration and local search capabilities of the algorithm and the diversity of the population, and avoid falling into local optimal solutions.

[0157] Furthermore, in one embodiment, the deletion operator described in step 6 is specifically: the key points of the individuals in the feasible solution set are evaluated in order, and if deleting a certain key point can enhance the fitness of the individual, it is removed.

[0158] In one embodiment, a master-slave dual-robot path planning system based on key-point optimized genetic algorithm is provided, and the system includes:

[0159] The first module is used to model the environment to be planned by the grid method, and the model includes the starting position, the ending position and the obstacle information of the master robot;

[0160] The second module is used to determine the obstacle inflation radius of the grid map according to the size of the master robot itself;

[0161] The third module is used to extract the key points of the master-slave robot path and perform population initialization; specifically including:

[0162] Obtain the feasible path of the master robot, take out the key points from the set of feasible path points, and encode them in order as the initial solution of the master robot path;

[0163] Calculate the vertical offset vector between every two adjacent key points in the feasible path of the master robot, and calculate the key points of the slave robot path through the given offset between the master and slave robots, and encode them in order as the initial solution of the slave robot path;

[0164] The initial solution of the master robot path and the initial solution of the slave robot path are spliced into the initial solution of the population, and then the above process is repeated until the required number of initialized populations is obtained;

[0165] The fourth module is used to select some individuals from the population as the parents for crossover and mutation operations according to the fitness function of the individuals;

[0166] The fifth module is used to perform improved crossover and mutation operations on the initialized population to obtain offspring;

[0167] The sixth module is used to perform pruning operations on the feasible solutions of the population using the deletion operator;

[0168] The seventh module is used to merge the offspring and the parent feasible solutions generated by crossover, mutation, and deletion operations, and select the new generation of feasible solutions in descending order according to the population fitness function;

[0169] The eighth module is used to determine whether the iteration termination condition of the population is reached. If so, the optimal key points of the robot path are decoded and output. If not, it jumps to the fifth module to continue the iteration operation;

[0170] The ninth module is used to sequentially connect the key points of the master and slave robots in order and perform smoothing processing, and then obtain the globally optimal paths of the master and slave robots.

[0171] For the specific limitations of the master-slave dual-robot path planning system based on the key-point optimization genetic algorithm, reference can be made to the limitations of the master-slave dual-robot path planning method based on the key-point optimization genetic algorithm in the above text, which will not be elaborated here. Each module in the above master-slave dual-robot path planning system based on the key-point optimization genetic algorithm can be implemented in whole or in part by software, hardware, and their combination. The above modules can be embedded in the processor of the computer device in hardware form or independent of it, or stored in the memory of the computer device in software form, so that the processor can call and execute the operations corresponding to the above modules.

[0172] In one embodiment, a computer device is provided, including a memory, a processor, and a computer program stored on the memory and executable on the processor. When the processor executes the computer program, it realizes:

[0173] Step 1, use the grid method to model the environment to be planned. The model includes the starting position, the ending position of the master robot, and obstacle information;

[0174] Step 2, determine the obstacle expansion radius of the grid map according to the size of the master robot itself;

[0175] Step 3, extract the key points of the master-slave robot path and perform population initialization; specifically including:

[0176] Obtain the feasible path of the master robot, extract the key points from the set of feasible path points, and encode them in order as the initial solution of the master robot path;

[0177] Calculate the vertical offset vector between every two adjacent key points in the feasible path of the master robot, and calculate the key points of the slave robot path through the given offset between the master and slave robots, and encode them in order as the initial solution of the slave robot path;

[0178] Concatenate the initial solution of the master robot path and the initial solution of the slave robot path into the initial solution of the population, and then repeat the above process until the required number of initialized populations is obtained;

[0179] Step 4, select some individuals from the population according to the fitness function of the individuals as the parents for crossover and mutation operations;

[0180] Step 5, perform improved crossover and mutation operations on the initialized population to obtain offspring;

[0181] Step 6, use the deletion operator to prune the feasible solutions of the population;

[0182] Step 7, merge the offspring and the parent feasible solutions generated by crossover, mutation and deletion operations, and select the new generation of feasible solutions in descending order according to the population fitness function;

[0183] Step 8, determine whether the iteration termination condition of the population is reached. If so, decode and output the optimal key points of the robot path. If not, jump to Step 5 to continue the iteration operation;

[0184] Step 9, connect the key points of the master and slave robots in order respectively, and perform smoothing processing, and then obtain the globally optimal paths of the master and slave robots.

[0185] For the specific limitations of each step, reference can be made to the limitations of the master-slave dual-robot path planning method based on key-point optimization genetic algorithm in the above text, which will not be elaborated here.

[0186] In one embodiment, a computer-readable storage medium is provided, on which a computer program is stored. When the computer program is executed by a processor, it realizes:

[0187] Step 1, use the grid method to model the environment to be planned. The model includes the starting position, the ending position of the master robot and the obstacle information;

[0188] Step 2, determine the obstacle inflation radius of the grid map according to the size of the master robot itself;

[0189] Step 3: Extract the key points of the master-slave robot paths and initialize the population. Specifically, it includes:

[0190] Obtain the feasible path of the master robot, extract the key points from the set of feasible path points, and encode them in order as the initial solution of the master robot path.

[0191] Calculate the vertical offset vectors between every two adjacent key points in the feasible path of the master robot, and calculate the key points of the slave robot path through the given offset between the master and slave robots, and encode them in order as the initial solution of the slave robot path.

[0192] Concatenate the initial solution of the master robot path and the initial solution of the slave robot path into the initial solution of the population, and then repeat the above process until the required number of initialized populations is obtained.

[0193] Step 4: Select some individuals from the population as the parents for crossover and mutation operations according to the fitness function of the individuals.

[0194] Step 5: Perform improved crossover and mutation operations on the initialized population to obtain the offspring.

[0195] Step 6: Use the deletion operator to prune the feasible solutions of the population.

[0196] Step 7: Combine the offspring and the parent feasible solutions generated by crossover, mutation, and deletion operations, and select the new generation of feasible solutions in descending order according to the population fitness function.

[0197] Step 8: Determine whether the iteration termination condition of the population is reached. If so, decode and output the optimal key points of the robot path. If not, jump to Step 5 and continue the iteration operation.

[0198] Step 9: Connect the key points of the master and slave robots in order and perform smoothing processing to obtain the globally optimal paths of the master and slave robots.

[0199] For the specific limitations of each step, reference can be made to the limitations of the master-slave dual-robot path planning method based on key-point optimization genetic algorithm in the above text, which will not be elaborated here.

[0200] The above shows and describes the basic principles, main features, and advantages of the present invention. Those skilled in the art should understand that the present invention is not limited by the above embodiments. What is described in the above embodiments and the specification only illustrates the principles of the present invention. Without departing from the spirit and scope of the present invention, any modifications, equivalent replacements, improvements, etc. made within the spirit and principles of the present invention shall be included in the protection scope of the present invention.

Claims

1. A master-slave dual robot path planning method based on key point optimization genetic algorithm, characterized in that: The method comprises the following steps: Step 1: Use the grid method to model the environment to be planned. The model contains the starting position, ending position and obstacle information of the main robot; Step 2: Determine the expansion radius of the grid map obstacle based on the size of the main robot. Step 3: Extract the key points of the master and slave robot paths and initialize the population; specifically, it includes: Obtain the feasible path of the main robot, extract key points from the feasible path point set, and encode them in sequence as the initial solution of the main robot path; Calculate the vertical offset vector between every two adjacent key points in the feasible path of the master robot, and calculate the key points of the slave robot path through the given offset between the master and slave robots, and encode them in sequence as the initial solution of the slave robot path; The master robot path initial solution and the slave robot path initial solution are spliced ​​into the population initial solution, and then the above process is repeated until the required number of initialized populations is obtained; Step 4: According to the fitness function of the individuals, select some individuals from the population as the parents for crossover and mutation operations; Step 5, performing improved crossover and mutation operations on the initialized population to obtain offspring; Step 6, use the deletion operator to prune the feasible solutions of the population; Step 7: merge the feasible solutions of the offspring and the parent generations generated by the crossover, mutation and deletion operations, and select the new generation of feasible solutions in descending order according to the fitness function of the population; Step 8, determine whether the iteration termination condition of the population is met, if so, decode and output the optimal key point of the robot path, if not, jump to step 5 and continue to perform the iteration operation; Step 9, connect the key points of the master and slave robots in sequence, and perform smoothing processing to obtain the global optimal path of the master and slave robots.

2. The master-slave dual robot path planning method based on key point optimization genetic algorithm according to claim 1 is characterized in that: The calculation formula for the expansion radius of the grid map obstacle in step 2 is: Where L and W are the maximum vertical distance and horizontal distance from the centroid to the outer contour of the main robot, respectively, and R safe is the safety margin, R di is the expansion radius of the grid map obstacle.

3. The master-slave dual robot path planning method based on key point optimization genetic algorithm according to claim 1 is characterized in that: In step 3, extracting key points from the feasible path point set specifically includes: The path point that satisfies at least one of the following conditions in the feasible path point set is taken as the key point kp i : x i =x0,y i =y0 x i =x f ,and i =and f In the formula, x i ,y i are the i-th key point kp i The horizontal and vertical coordinates, x i+1 ,y i+1 are the i+1th key points kp i+1 The horizontal and vertical coordinates, x i-1 ,y i-1 are the i-1th key points kp i-1 The horizontal and vertical coordinates of x0 and y0 are the horizontal and vertical coordinates of the initial position kp0, i.e. the starting position of the main robot, respectively. f ,y f are the end positions kp f That is, the horizontal and vertical coordinates of the final position of the main robot, θ0 is the preset angle threshold, d0 is the preset distance threshold, x ob ,y ob are the horizontal and vertical coordinates of the obstacle closest to the path point.

4. The master-slave dual robot path planning method based on key point optimization genetic algorithm according to claim 3 is characterized in that: In step 3, the vertical offset vector between every two adjacent key points in the feasible path of the master robot is calculated, and the key points of the slave robot path are calculated by the given offset between the master and slave robots. The specific calculation formula is: (x′ i ,y′ i )=(x i ,y i )+d off ·v i In the formula, x′ i ,y′ i are the i-th key point kp i The corresponding horizontal and vertical coordinates of the key points of the robot path, d off is the given offset between the master and slave robots, v i is the vertical offset vector between every two adjacent key points in the feasible path of the main robot.

5. The master-slave dual robot path planning method based on key point optimization genetic algorithm according to claim 1 is characterized in that: The fitness function of the individual in step 4 is comprehensively determined by the robot path length function, the robot path smoothness function, the robot path safety function and the master-slave robot coordination function, specifically: fit=μ1fit1+μ2fit2+μ3P co in, fit1=ω1P L +ω2P c +ω3P s fit2=ω′1P′ L +ω′2P′ c +ω′3P′ s Where fit is the fitness function of the population, fit1 is the fitness function of the master robot, fit2 is the fitness function of the slave robot, ω1~ω3, ω′1~ω′3, μ1~μ3 are all weight coefficients, P L , P′ L are the path length functions of the master and slave robots, P c , P′ c are the path smoothness functions of the master and slave robots, P s , P′ s are the path safety functions of the master and slave robots, P co It is the coordination function of master and slave robots.

6. The master-slave dual robot path planning method based on key point optimization genetic algorithm according to claim 5 is characterized in that: The master-slave robot path length function is defined as the sum of the distances between all two consecutive key points in the path: In the formula, (x i ,y i )、(x i+1 ,y i+1 ) represent two consecutive key points, and n represents the total number of key points; The master-slave robot path smoothness function is defined as the sum of the squares of the second-order differences of all key points in the path: In the formula, ||Δ 2 kp i || 2 Indicates the key point kp i The second-order difference of The master-slave robot path safety function is: P s =-λ1P s1 +λ2P s2 Among them, P s1 is the robot path collision penalty term, P s2 is the safety gap term, λ1, +λ2 are P s1 , P s2 The weight coefficient of The master-slave robot coordination function is: Where D i,min is the key point kp in the robot path i ′ is the distance between the key point in the main robot path closest to it, d off is the offset between the given master and slave robots.

7. The master-slave dual robot path planning method based on key point optimization genetic algorithm according to claim 6 is characterized in that: The robot path collision penalty term P s1 for: in, In the formula, φ i is the key point kp i Collision penalty term, d(kp i ,ob mi ) is kp i To the nearest obstacle ob mi The distance, d safe is a predefined safety distance; The safety clearance term P s2 for:

8. The master-slave dual robot path planning method based on key point optimization genetic algorithm according to claim 1 is characterized in that: In step 5, an improved crossover is performed, and the improved crossover operator used specifically includes: The first crossover operator: The parent path base points are combined through nonlinear functions to generate offspring, as shown in the following formula: In the formula, and are the horizontal and vertical coordinates of the key points of the two parents, respectively. of ,y of are the horizontal and vertical coordinates of the offspring, ξ1, ξ2, ξ3, ξ4 are randomly generated weights that satisfy the normalization condition ξ1+ξ2+ξ3+ξ4=1; The second crossover operator: Combine two parents in a random way, as shown below: Where η follows a uniform distribution of [-1,1].

9. The master-slave dual robot path planning method based on key point optimization genetic algorithm according to claim 1, characterized in that: In step 5, an improved mutation operation is performed, and the improved mutation operators used specifically include: The first mutation operator: By randomly selecting a key point in the parent generation and moving the key point a certain distance toward the end point, a child generation is obtained, as shown in the following formula: (x of ,and of )=(x pa ,and pa )+γ·v pa,g In the formula, v pa,g is the direction vector from the parent to the end point, γ is the coefficient, x pa ,y pa are the horizontal and vertical coordinates of the parent key point, respectively, of ,y of are the horizontal and vertical coordinates of the offspring respectively; The second mutation operator: By randomly selecting a finite number of continuous key points with a certain key point as the starting point and another key point as the end point in the parent generation, a feasible path is reconstructed, and the key points of the new feasible path are obtained by taking the key points from the feasible path point set in step 3, replacing the original key points to obtain a child generation.

10. The master-slave dual robot path planning method based on key point optimization genetic algorithm according to claim 1, characterized in that: The deletion operator described in step 6 is specifically: evaluating the key points of individuals in the feasible solution set in order, and if deleting a key point can enhance the fitness of the individual, it will be removed.

Images