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

Abstract

The application discloses a master-slave dual-robot path planning method based on a key point optimized genetic algorithm, and aims at the problem of excessively high path redundancy generated by a traditional path planning method, selects key points representing path characteristics according to angle conditions, distance conditions and the like, so as to reduce the redundancy in genetic optimization; the master-slave framework is adopted to solve the problem of poor path coordination caused by independent generation of robot paths in dual-robot system path planning, the slave robot trajectory is generated according to the master robot trajectory and is combined as an initial feasible solution of the genetic algorithm, a comprehensive fitness function considering the coordination of the master robot and the slave robot is designed according to the framework, nonlinear crossover and mutation operators are introduced, and the exploration ability of the optimization algorithm is further improved, so that the safe and coordinated dual-robot cooperative path is iteratively optimized by the algorithm.

Description

TECHNICAL FIELD

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

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

[0003] In the research of traditional path planning, traditional planning algorithms such as A* algorithm, Dijkstra algorithm and RRT algorithm can generate feasible paths for single robots to avoid obstacles. However, when these methods are applied to dual-robot systems, the generated paths often contain most of the redundant base points, resulting in excessive 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 can lead to motion conflicts or disconnection between robots, thereby affecting the smooth completion of the task. SUMMARY

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

[0005] The technical solution for achieving the purpose of the present application is as follows: a master-slave dual-robot path planning method based on a key point optimized genetic algorithm, the method comprising the following steps:

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

[0007] Step 2: determining the obstacle inflation radius of the grid map according to the size of the master robot;

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

[0009] obtaining the feasible path of the master robot, taking out the key points from the feasible path point set, 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 obtaining 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] The master robot path initial solution and the slave robot path initial solution are spliced into a population initial solution, and then the above process is repeated until the required number of initialized populations is obtained;

[0012] Step 4, according to the fitness function of the individual, some individuals are selected from the population as the parents for cross and mutation operations;

[0013] Step 5, the improved cross and mutation operations are performed on the initialized population to obtain offspring;

[0014] Step 6, the feasible solutions of the population are pruned using the deletion operator;

[0015] Step 7, the offspring and the parent feasible solutions generated by the cross, mutation and deletion operations are combined, and the new generation of feasible solutions are selected in descending order according to the fitness function of the population;

[0016] Step 8, it is judged whether the iteration termination condition of the population is reached, if yes, the optimal key points of the robot path are decoded and output, if not, the iteration operation is continued by jumping to step 5;

[0017] Step 9, the master and slave robot key points are connected in order respectively, and smoothing processing is performed, and then the global optimal path of the master and slave robots is obtained.

[0018] Further, the calculation formula of the obstacle inflation radius in step 2 of the grid map 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 inflation radius of the obstacle of the grid map.

[0021] Further, in step 3, the key points are taken out from the feasible path point set, which specifically includes:

[0022] The path point in the feasible path point set at least satisfies the following condition to be a key point kp i :

[0023] x i =x0,y i =y0

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

[0025]

[0026] where x i , y i are the horizontal and vertical coordinates of the i-th key point kp i , x i+1 , y i+1 are the horizontal and vertical coordinates of the i+1-th key point kp i+1 , x i-1 , y i-1 are the horizontal and vertical coordinates of the i-1-th key point kp i-1 , x0, y0 are the horizontal and vertical coordinates of the initial position kp0, i.e., the starting position of the master robot, x f , y f are the horizontal and vertical coordinates of the terminal position kp f , i.e., the ending position of the master robot, θ0 is a preset angle threshold, d0 is a preset distance threshold, x ob , y ob are the horizontal and vertical coordinates of the obstacle closest to the path point.

[0027] Further, 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 path key point of the slave robot is calculated through the given offset between the master and slave robots, and the specific calculation formula is:

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

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

[0030] Further, in step 4, the fitness function of the individual is 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, and specifically:

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

[0032] where,

[0033] fit1= ω1PL + ω2P c + ω3P s

[0034] fit2 = ω'1P' L + ω'2P' c + ω'3P' s

[0035] 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 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, and P co is the coordination function of the master and slave robots.

[0036] Further, the path length function of the master and slave robots is defined as the sum of the distances between all consecutive key points in the path:

[0037]

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

[0039] 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:

[0040]

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

[0042] The path safety function of the master and slave robots is:

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

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

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

[0046]

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

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

[0049]

[0050] where,

[0051]

[0052] where φ i is the key point kp i collision penalty term, d(kp i , ob mi ) is the distance from kp i to the closest obstacle ob mi , d safe is a predefined safety distance.

[0053] The safety gap term P s2 is:

[0054]

[0055] Further, the improved crossover in step 5 is performed, and the improved crossover operator specifically includes:

[0056] The first crossover operator:

[0057] The parent path base points are combined by a nonlinear function to generate the offspring, as shown in the following formula:

[0058]

[0059] where, and are the horizontal and vertical coordinates of the key points of the two parents, x of , y ofrespectively, ξ1, ξ2, ξ3, ξ4 are random weights, satisfying the normalization condition ξ1+ξ2+ξ3+ξ4=1;

[0060] The second crossover operator:

[0061] Combining two parents in a random way, as shown in the following formula:

[0062]

[0063] In the formula, η is uniformly distributed in [-1, 1].

[0064] Further, the improved mutation operation in step 5 is carried out, and the improved mutation operator specifically includes:

[0065] The first mutation operator:

[0066] By randomly selecting a key point in the parent, moving the key point to the terminal point direction by a certain distance to obtain a child, as shown in the following formula:

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

[0068] In the formula, v pa,g is the direction vector of the parent to the terminal point, γ is a coefficient, x pa , y pa are the horizontal and vertical coordinates of the parent key point respectively, x of , y of are the horizontal and vertical coordinates of the child respectively.

[0069] The second mutation operator:

[0070] By randomly selecting a certain key point in the parent as the starting point and another key point as the terminal point between a limited number of continuous key points, a feasible path is reconstructed, and the key points of the new feasible path are obtained by the method of taking out the key points from the feasible path point set in step 3, to replace the original key points to obtain a child.

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

[0072] Compared with the prior art, the present application has the following advantages:

[0073] (1) In order to solve the problem of high path redundancy generated by traditional path planning method, the key points representing path characteristics are selected according to angle condition, distance condition, etc., so as to reduce the redundancy in genetic optimization.

[0074] (2) In order to solve the problem of poor path coordination caused by independent path generation of robots in double mobile robot system path planning, master-slave framework is adopted, the slave robot trajectory is generated according to the master robot trajectory and combined as the initial feasible solution of genetic algorithm, and the comprehensive fitness function considering the coordination of master-slave robots is designed according to the framework, and nonlinear crossover and mutation operators are introduced, so as to further improve the exploration ability of optimization algorithm, thereby guiding the algorithm to iteratively optimize the safe and coordinated double robot cooperative path.

[0075] (3) The starting point and the ending point of the optimized path are ensured to be the given targets, secondly, the path smoothness and safety are ensured to the maximum, finally, a large number of redundant path points generated by random sampling are eliminated, the initial path population is simplified, and the calculation efficiency is improved.

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

[0077] The application will be described in further detail below with reference to the drawings. BRIEF DESCRIPTION OF DRAWINGS

[0078] Figure 1 The improved genetic algorithm flowchart proposed by the application in an embodiment.

[0079] Figure 2 The robot size parameter diagram.

[0080] Figure 3 The path planning flowchart of the application in an embodiment. DETAILED DESCRIPTION

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

[0082] It should be noted that if the embodiments of the present application involve directionality indication (such as up, down, left, right, front, back, …), the directionality indication is only used to explain the relative position relationship, motion condition and the like between components in a certain specific posture (as shown in the drawings), and if the specific posture changes, the directionality indication also changes accordingly.

[0083] In addition, if the embodiments of the present application involve descriptions such as "first", "second", etc., the descriptions of "first", "second", etc. are only for description purposes, and cannot be understood as indicating or implying the relative importance or implicitly indicating the number of indicated technical features. Therefore, the features defined as "first", "second" can explicitly or implicitly include at least one of the features. In addition, the technical solutions of each embodiment can be combined with each other, but it must be based on the fact that a person skilled in the art can realize it, and when the combination of technical solutions contradicts each other or cannot be realized, it should be considered that the combination of technical solutions does not exist and is not within the protection scope required by the present application.

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

[0085] Step 1: A grid method is used to model the environment to be planned, and the model contains the starting position, the ending position and the obstacle information of the master robot;

[0086] Step 2: According to the size of the master robot itself, the obstacle inflation radius of the grid map is determined;

[0087] Step 3: Extract the master-slave robot path key points and initialize the population; specifically including:

[0088] Obtain the feasible path of the master robot, take out the key points from the feasible path point set, 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 slave robot path key points 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] Splice 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 individual, select some individuals from the population as the parents for cross 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 feasible solutions of the offspring and parents 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 has been met. If yes, decode and output the optimal key point of the robot path (the path with the highest fitness, decode it and output the optimal KP of the robot path, i.e., the key point). If no, jump to step 5 and continue to execute the iteration operation.

[0096] Step 9: Connect the key points of the master and slave robots sequentially and perform smoothing processing to obtain the globally optimal path of the master and slave robots. Preferably, the smoothing processing is based on the cubic spline method.

[0097] Figure 3 This is a schematic diagram of the global path of the master-slave robot generated according to this method.

[0098] Furthermore, in one embodiment, the formula for calculating the radius of expansion of the grid map obstacle in step 2 is:

[0099]

[0100] In the formula, L and W are the maximum vertical and horizontal distances from the centroid of the main robot to its outer contour, respectively (e.g., ...). Figure 2 As shown), R safe For safety margin, R di This represents the expansion radius of obstacles in the raster map.

[0101] Furthermore, in one embodiment, step 3 specifically includes:

[0102] Step 3-1: Use the RRT method to obtain the feasible path of the main robot, and extract the key point sequence KP in order from the set of feasible path points to form the initial solution of the main robot path.

[0103] for A path point kp must satisfy at least one of the following conditions to be considered a key point. i :

[0104] x i =x0,y i =y0

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

[0106]

[0107] In the formula, x i y i Let kp be the i-th key point. i x and y coordinates i+1 y i+1 The i+1th key point kp i+1 x and y coordinates i-1 y i-1 The i-1th key point kp i-1 The x and y coordinates are given by x0 and y0, respectively, representing the x and y coordinates of the initial position kp0, i.e., the starting position of the main robot. f y f To terminate at position kp respectively f That is, the x and y coordinates of the termination position of the main robot, where θ0 is a preset angle threshold, d0 is a preset distance threshold, and x... ob y ob The x and y coordinates are the nearest obstacles to the path point.

[0108] Step 3-2, given the offset d between the master and slave robots off Calculate the vertical offset vector v between every two adjacent key points in the main robot's path. i The corresponding key point kp′ of the robot path is obtained according to the following formula. i , kp′ i Encoded sequentially from 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 These are the key points kp′ on the robot path. i The horizontal and vertical coordinates;

[0111] Step 3-3: Concatenate KP and KP′ to form a feasible coded solution K. f Place the initial population into the population POP, and repeat steps 3.1 and 3.2 until a sufficient number of initial populations are obtained.

[0112] Further, in step 4, according to the fitness function of the individual, the selection method of roulette is used 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, so better individuals have more opportunities to be selected. In addition, the population of the i-th iteration is constructed from the pool consisting of the new offspring obtained from the population of the (i-1)-th iteration and the genetic operators. The fitness function of the population needs to satisfy the requirements of the master-slave robot path length, smoothness, safety, and also ensure the motion coordination between the master-slave robot trajectories.

[0113] The fitness function of the individual is 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:

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

[0115] wherein,

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

[0117] fit2 = ω'1P' L + ω'2P' c + ω'3P' s

[0118] In the formula, 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 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, and P co is the master-slave robot coordination function.

[0119] Here, preferably, in some embodiments, the master-slave robot path length function is defined as the sum of the distances between all consecutive two key points in the path:

[0120]

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

[0122] The master-slave robot path smoothness function is defined as the square sum of the second-order difference of all key points in the path:

[0123]

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

[0125] Here, preferably, in some embodiments, the master-slave robot path safety function is:

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

[0127] Where 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 , P s2 respectively.

[0128] Wherein the robot path collision penalty term P s1 is:

[0129]

[0130] Wherein,

[0131]

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

[0133] The safety gap term P s2 is:

[0134]

[0135] Here preferably, in some embodiments, to ensure as much as possible that the slave robot always follows the master robot and keeps a reasonable, safe spatial distance from it, both avoiding being too close to cause collision and too far to lose cooperativeness, based on the spatial distance constraint, the master-slave robot coordination function is designed as:

[0136]

[0137] where D i,min is the distance between the slave robot path at the key point kp′ i and the closest key point in the master robot path, d off is the given offset between the master and slave robots.

[0138] Further, in one of the embodiments, step 5 specifically comprises:

[0139] Step 5-1, crossover operation is performed with a proper probability, the crossover operator combines two parent K f with the same or different number of KPs to generate a new offspring. The crossover operators used include two kinds:

[0140] The first crossover operator:

[0141] Combines the parent path base points to generate the offspring through a nonlinear function, as shown in the following formula:

[0142]

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

[0144] The second crossover operator:

[0145] Combines the two parents in a random way, as shown in the following formula:

[0146]

[0147] where η is uniformly distributed in [-1, 1].

[0148] The first crossover operator can generate paths with better smoothness to some extent, and 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 key points of the parent with a smaller number of key points are combined with the nearest key point in the parent with a larger number of key points to generate a descendant.

[0149] Step 5-2, mutation operation is performed with a proper probability, and a mutation operator changes a single or part of the key points in the parent to generate a new descendant. The improved mutation operator used specifically includes: f

[0150] The first mutation operator:

[0151] A child is obtained by randomly selecting a key point in the parent and moving the key point a certain distance in the direction of the terminal point, as shown in the following formula:

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

[0153] In the formula, v pa,g is the direction vector of the parent to the terminal point, γ is a coefficient, x pa , y pa are the horizontal and vertical coordinates of the parent key point, and x of , y of are the horizontal and vertical coordinates of the child, respectively.

[0154] The second mutation operator:

[0155] A child is obtained by randomly selecting a certain key point in the parent as a starting point and a certain key point as a terminal point, reconstructing a feasible path using the RRT method, and obtaining the key points of the new feasible path by the method of taking out the key points from the feasible path point set in step 3, and replacing the original key points to obtain a child.

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

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

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

[0159] The first module is configured to model the environment to be planned by using a grid method, and the model includes a start position, an end position and obstacle information of the master robot;

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

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

[0162] The third module is configured to extract key points of the master-slave robot path and perform population initialization, and specifically includes:

[0163] The third module is configured to extract key points of the master-slave robot path and perform population initialization, and specifically includes:

[0164] The third module is configured to extract key points of the master-slave robot path and perform population initialization, and specifically includes:

[0165] The fourth module is configured to select some individuals from the population as parents for performing cross and mutation operations according to an individual fitness function;

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

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

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

[0169] The eighth module is configured to judge whether an iteration termination condition of the population is reached, if yes, decode and output optimal key points of the robot path, and if not, jump to the fifth module to continue the iteration operation;

[0170] The ninth module is configured to connect the key points of the master and slave robots in sequence respectively, and perform smoothing processing, and then obtain the global optimal path of the master and slave robots.

[0171] The specific definitions of the master-slave dual-robot path planning system based on the key point optimization genetic algorithm can refer to the definitions of the master-slave dual-robot path planning method based on the key point optimization genetic algorithm in the foregoing, and will not be repeated here. Each module in the master-slave dual-robot path planning system based on the key point optimization genetic algorithm can be realized by software, hardware, or a combination thereof, in whole or in part. Each module described above can be embedded in or independent of the processor in the computer device in hardware form, or can be stored in the memory in the computer device in software form, so as to be called and executed by the processor to perform the operations corresponding to each module.

[0172] In one embodiment, a computer device is provided, comprising a memory, a processor, and a computer program stored on the memory and executable on the processor, wherein the processor implements the following when executing the computer program:

[0173] Step 1: The grid method is used to model the environment to be planned, and the model contains the starting position, the ending position, and the obstacle information of the master robot;

[0174] Step 2: The obstacle inflation radius of the grid map is determined according to the size of the master robot itself;

[0175] Step 3: Extract the key points of the master-slave robot path and initialize the population; specifically including:

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

[0177] Calculate the vertical offset vector between each two adjacent key points in the feasible path of the master robot, and obtain 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] Splice 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 as the parents for cross and mutation operations according to the fitness function of the individual;

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

[0181] Step 6: Use the delete operator to prune the feasible solutions of the population;

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

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

[0184] Step 9, connect the master and slave robot key points in sequence respectively, and perform smoothing processing, and then obtain the global optimal path of the master and slave robots.

[0185] For specific definitions of each step, please refer to the definitions of the master-slave dual robot path planning method based on key point optimization genetic algorithm in the above, which will not be repeated here.

[0186] In one embodiment, a computer readable storage medium is provided, which stores a computer program, the computer program is executed by a processor to implement:

[0187] Step 1, model the environment to be planned using the grid method, the model contains the starting position, the termination position and the obstacle information of the master robot;

[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 master-slave robot path key points and initialize the population; specifically including:

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

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

[0192] Splice 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 parents for cross and mutation operations according to the fitness function of the individual;

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

[0195] Step 6, use the delete operator to prune the feasible solutions of the population;

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

[0197] Step 8, judge whether the iteration termination condition of the population is reached, if yes, output the optimal key point 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 sequence respectively, and perform smoothing processing, and then obtain the global optimal path of the master and slave robots.

[0199] The specific definition of each step can refer to the definition of the master-slave dual robot path planning method based on the key point optimization genetic algorithm in the above, which will not be repeated here.

[0200] The basic principles, main features and advantages of the present application are shown and described above. It should be understood by those skilled in the art that the present application is not limited by the above examples, and the above examples and descriptions in the specification are only to illustrate the principles of the present application. Any modification, equivalent replacement, improvement, etc. within the spirit and principles of the present application should be included in the protection scope of the present application.

Claims

1. A master-slave dual-robot path planning method based on a key point optimized genetic algorithm, characterized in that, The method comprises the following steps: Step 1, a grid method is used to model the environment to be planned, and the model comprises a starting position, a terminal position and obstacle information of a host robot; Step 2, an obstacle inflation radius of a grid map is determined according to the size of the host robot; Step 3, key points of a master-slave robot path are extracted and population initialization is performed; specifically comprising: acquiring a feasible path of the host robot, taking out key points from a feasible path point set, and encoding the key points in sequence as an initial solution of the host robot path; calculating a vertical offset vector between every two adjacent key points in the feasible path of the host robot, and calculating key points of a slave robot path through a given offset between the host robot and the slave robot, and encoding the key points in sequence as an initial solution of the slave robot path; splicing the initial solution of the host robot path and the initial solution of the slave robot path into an initial solution of the population, and then repeating the above process until a required number of initialized populations are obtained; Step 4, selecting some individuals from the population as parents for performing cross and mutation operations according to an individual fitness function; Step 5, performing improved cross and mutation operations on the initialized population to obtain offspring; Step 6, performing pruning operations on feasible solutions of the population using a deletion operator; Step 7, merging the offspring and the parent feasible solutions generated through the cross, mutation and deletion operations, and selecting new generation feasible solutions in descending order according to the population fitness function; Step 8, judging whether an iteration termination condition of the population is reached, if yes, decoding and outputting optimal key points of the robot path, if not, jumping to Step 5 and continuing to perform iteration operations; Step 9, sequentially connecting the key points of the host robot and the slave robot respectively, and performing smoothing processing, and then obtaining a global optimal path of the host robot and the slave robot.

2. The master-slave dual-robot path planning method based on the key point optimized genetic algorithm according to claim 1, characterized in that, The calculation formula of the obstacle inflation radius of the grid map in Step 2 is: In the formula, L and W are the maximum vertical and horizontal distances from the center of the master robot to the outer contour, respectively, and R safe is the safety margin, and R di is the inflation radius of the grid map obstacle.

3. The master-slave dual-robot path planning method based on the key point optimized genetic algorithm according to claim 1, characterized in that, In Step 3, the key points are taken out from the feasible path point set, specifically comprising: The path point in the feasible path point set that at least satisfies certain condition is taken as a key point kp i : x i = x0, y i = y0 x i = x f , y i = y f In the formula, x i , y i are the horizontal and vertical coordinates of the i-th key point kp i , x i+1 , y i+1 are the horizontal and vertical coordinates of the i+1-th key point kp i+1 , x i-1 , y i-1 are the horizontal and vertical coordinates of the i-1-th key point kp i-1 , x0, y0 are the horizontal and vertical coordinates of the initial position kp0, i.e., the starting position of the host robot, x f , y f are the horizontal and vertical coordinates of the terminal position kp f , i.e., the terminal position of the host robot, θ0 is a preset angle threshold, d0 is a 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 the key point optimized genetic algorithm according to claim 3, characterized in that, In Step 3, the vertical offset vector between every two adjacent key points in the feasible path of the host robot is calculated, and the key points of the slave robot path are calculated through the given offset between the host robot and the slave robot, and the specific calculation formula is: (x i ,y i ) i (x i ,y off ) i where x′ i ,y′ i are the horizontal and vertical coordinates of the i-th key point kp i corresponding to the master robot path, d off is the given offset between the master and slave robots, and v i is the vertical offset vector between each pair of adjacent key points in the master robot feasible path.

5. The master-slave dual-robot path planning method based on the key point optimized genetic algorithm according to claim 1, characterized in that, The individual fitness function in Step 4 is determined by a robot path length function, a robot path smoothness function, a robot path safety function and a master-slave robot coordination function, and specifically is: fit = μ1fit1+ μ2fit2+ μ3P co Wherein, fit1 = ω1P L + ω2P c + ω3P s fit2 = ω'1P' L + ω'2P' c + ω'3P' s In the formula, fit is a fitness function of the population, fit1 is a fitness function of the master robot, fit2 is a fitness function of the slave robot, ω1-ω3, ω'1-ω'3, μ1-μ3 are weight coefficients, P L , P' L are path length functions of the master and slave robots respectively, P c , P' c are path smoothness functions of the master and slave robots respectively, P s , P' s are path safety functions of the master and slave robots respectively, and P co is a coordination function of the master and slave robots.

6. The master-slave dual-robot path planning method based on the key point optimized genetic algorithm according to claim 5, characterized in that, The master-slave robot path length function is defined as the sum of distances between all consecutive two key points in the path: In the formula, (x i ,y i ) and (x i+1 ,y i+1 ) respectively 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 square sum of second-order differences of all key points in the path: wherein ||Δ 2 kp i || 2 denotes the second order difference of the key point kp i . The master-slave robot path safety function is: P s = -λ1P s1 + λ2P s2 where P s1 is a robot path collision penalty term, P s2 is a safety gap term, and λ1, λ2 are weight coefficients for P s1 and P s2 , respectively. The master-slave robot coordination function is: where D i,min is the distance between the key point kp i ′ in the master robot path and the distance between the key point in the slave robot path, d off is the given offset between the master and slave robots.

7. The master-slave dual-robot path planning method based on the key point optimized genetic algorithm according to claim 6, characterized in that, The robot path collision penalty term P s1 is: Wherein, where φ i is the key point kp i collision penalty term, d(kp i , ob mi ) is the distance from the key point kp i to the nearest obstacle ob mi , and d safe is a predefined safety distance; The safety margin term P s2 is:

8. The master-slave dual-robot path planning method based on the key point optimized genetic algorithm according to claim 1, characterized in that, In Step 5, the improved cross is performed, and the improved cross operator specifically comprises: The first cross operator: The parent path base points are combined through a nonlinear function 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, x of , y of are the horizontal and vertical coordinates of the child, respectively, and ξ1, ξ2, ξ3, and ξ4 are randomly generated weights that satisfy the normalization condition ξ1+ξ2+ξ3+ξ4=1. The second cross operator: The two parents are combined in a random manner, as shown in the following formula: In the formula, η is uniformly distributed in [-1, 1]. 9.The master-slave dual-robot path planning method based on the key point optimized genetic algorithm of claim 1, wherein, In Step 5, the improved mutation operation is performed, and the improved mutation operator specifically comprises: The first mutation operator: A child is generated by moving a key point in the parent a certain distance towards the end point, as shown in the following equation: (x of ,y of ) = (x pa ,y pa ) + γ · v pa,g where v pa,g is the direction vector of the parent to the end point, γ is a coefficient, x pa , y pa are the horizontal and vertical coordinates of the parent key point, respectively, x of , y of are the horizontal and vertical coordinates of the child, respectively; The second mutation operator: A child is generated by re-constructing a feasible path from a finite number of consecutive key points between a randomly selected starting key point and a randomly selected ending key point in the parent, and replacing the original key points with the new key points obtained by the method described in step 3.

10. The master-slave dual-robot path planning method based on the key point optimized genetic algorithm according to claim 1, characterized in that, The deletion operator described in step 6 is as follows: the key points of the individuals in the feasible solution set are evaluated in order, and if the removal of a key point can enhance the fitness of the individual, the key point is removed.

Images