Ground mobile robot autonomous path planning method based on dynamic multi-mode sheep flock migration algorithm

Abstract

The invention provides a robot dynamic path planning method based on a sheep flock collective movement mechanism, and the method comprises the following steps: 1, obtaining task region information, and initializing algorithm parameters and a population; 2, updating the initial population according to the historical optimal path set; 3, dividing the sheep flock into a preset number of subgroups according to the position in the solution space; 4, performing search and migration optimization on each subgroup by adopting a sheep flock algorithm mechanism; 4.1, executing a grazing operator by each subgroup, and performing random search; 4.2, judging whether a condition for ending the grazing operator is met or not; 4.3, sorting the subgroups from small to large according to fitness function values, and selecting head sheep; 4.4, executing a collective motion operator by each subgroup, and performing global search; 4.5, executing a contraction strategy by each subgroup; 4.6, executing a compensation strategy by each subgroup; 5, judging whether the sheep flock algorithm is ended or not; 6, updating a historical optimal path set according to the output optimal path planning scheme; 7, judging whether the robot completes the task or not, and if yes, ending; otherwise, returning to execute the step 1.

Description

(I) Technical field

[0001] The invention belongs to the field of robot path planning, and in particular relates to an autonomous dynamic path planning method for a ground mobile robot. (II) Background technology

[0002] The rapid development of artificial intelligence technology has greatly promoted the advancement and application of robotics technology, making it an important part of modern society and gradually taking on many tasks in the fields of logistics, industrial production, military operations, etc. In actual application, robots usually need to move within a certain area to complete tasks, such as transporting goods, military reconnaissance, etc., and the actual mission scene is usually full of obstacles, such as shelves, natural obstacles, etc. Whether the robot can autonomously avoid obstacles in the scene and complete the predetermined task at the fastest speed or the shortest path has become a key issue affecting the success rate and work efficiency of mobile robot tasks. Therefore, autonomous path planning technology has always been a key technology in the field of ground mobile robots.

[0003] Traditional robot path planning problems mostly consider static environments and targets, that is, the optimal path is calculated based on fixed task area information and target positions. However, in most real-world task scenarios, the positions of obstacles and targets change in real time, which poses a challenge to traditional path planning methods. At this time, the static path planning results are no longer applicable, which can easily lead to problems such as non-optimal path selection and collisions with obstacles.

[0004] This patent solves this problem by proposing a new dynamic multimodal swarm intelligence optimization algorithm. In order to cope with the real-time changes in the task environment, the method proposed in this patent uses optical sensors to obtain the spatial information of the task area in real time, and calculates and updates the optimal path in real time based on this, dynamically guides the robot to move, and avoids the problem of outdated information. Dynamic path planning is a type of optimal control problem, which is constrained by continuous state inequalities. Due to its high dimensionality, multimodality, and dynamicity, it is very difficult to optimize and solve it using traditional analytical methods. At present, the heuristic optimization algorithm based on swarm intelligence is the main method to solve the dynamic autonomous path planning of robots. Due to its simple principle and easy operation, it has been applied in engineering. Among them, the sheep flock migration optimization algorithm is a new type of swarm intelligence optimization algorithm proposed by domestic scholars based on the behavior mechanism of sheep flocks recently. It has the advantages of good high-dimensional performance and not easy to fall into local optimality. However, most swarm intelligence algorithms, including the sheep flock migration algorithm, are designed for static problems. When dealing with the dynamic path planning problem of robots, the method of re-optimization after environmental changes is adopted, resulting in low optimization efficiency and poor real-time performance. This patent draws on the main computational models in traditional flock optimization algorithms, and combines natural flock mechanisms such as memory mechanisms and multi-subgroup mechanisms to propose a new dynamic multimodal swarm intelligent optimization algorithm, which is expected to provide an efficient solution for autonomous dynamic path planning of ground mobile robots. (III) Summary of the invention

[0005] This paper draws on the main calculation model of the flock migration optimization algorithm, integrates the flock memory mechanism and the multi-subgroup mechanism, proposes a new dynamic multimodal flock migration algorithm, and applies it to the robot dynamic path planning problem, and proposes a ground mobile robot autonomous dynamic path planning method based on the dynamic multimodal flock migration algorithm. This method can overcome the current shortcomings of robot autonomous dynamic path planning, such as slow strategy generation speed, low calculation efficiency, and poor real-time performance, and provide a new solution for the robot's autonomous dynamic path planning in complex task scenarios.

[0006] The specific steps are as follows:

[0007] Step 1: Establish an environmental feature model and initialize algorithm parameters, including:

[0008] Step 1.1 Use optical sensors to collect environmental information and establish a spatial point cloud map, including the distribution locations of obstacles and target points, and thereby define the task area and establish a two-dimensional environmental feature model;

[0009] Step 1.2 Initialize the parameters of the dynamic multimodal flock migration algorithm, including the maximum number of iterations, the number of populations, the maximum number of searches, the benchmark search step, the number of subgroups, the number of navigation points, the historical optimal path set, etc.;

[0010] Step 1.3: Initialize the robot path planning solution set (sheep flock) in the task area, where each sheep represents a path planning solution;

[0011] Step 2: Update the initial population based on the historical optimal path set;

[0012] Step 3: Use K-means clustering to divide the flock in the solution space into multiple sub-groups;

[0013] Step 4: The search and migration mechanism of the herd algorithm is executed in parallel in each sub-group, including:

[0014] Step 4.1 Each subgroup executes the grazing operator separately, searches for a better path planning solution in the local range of its corresponding solution space, and updates the individual optimal solution and migration index;

[0015] Step 4.2 Each subgroup determines whether it meets the conditions for ending the grazing operator. If so, it executes step 4.3, otherwise it continues to execute step 4.1;

[0016] Step 4.3: Sort the subgroups in ascending order according to their fitness function values, and select the one with the smallest fitness function value as the leader;

[0017] Step 4.4: Each subgroup executes the collective motion operator respectively, migrates in the order determined in step 4.3, and searches for a better path planning solution globally;

[0018] Step 4.5 Each subgroup executes the contraction strategy respectively and converges around the current optimal solution of the subgroup;

[0019] Step 4.6 Each subgroup executes a compensation strategy to improve the algorithm's exploration ability;

[0020] Step 5 determines whether to end this round of calculation based on whether the maximum number of iterations has been reached. If so, output the optimal planning path of the entire flock at the current moment and execute step 6; otherwise, return to step 4;

[0021] Step 6: Update the historical optimal path set according to the optimal planning path output in step 5;

[0022] In step 7, an optical sensor is used to obtain current environmental information to determine whether the robot has completed the task. If so, the method ends; otherwise, the method returns to step 1. (IV) Description of the drawings

[0023] Figure 1 This is a schematic flow chart of a method for rapid reconstruction of a UAV formation based on flock migration behavior proposed by the present invention. (V) Specific implementation methods

[0024] The exemplary implementation of the present invention is described in detail below in conjunction with the accompanying drawings. The following description includes specific details to assist understanding, but these specific details should be shown as exemplary only. Therefore, it will be appreciated by those of ordinary skill in the art that various changes and modifications may be made to the various examples described herein without departing from the scope and spirit of the present disclosure. In addition, for the sake of clarity and conciseness, descriptions of well-known functions and structures are omitted.

[0025] The terms and words used in the following description and claims are not limited to their literal meanings, but are only used by the inventor to implement a clear and consistent understanding of the present invention. Therefore, it should be clear to those skilled in the art that the following description of various example implementations of the present invention is provided only for illustrative purposes and is not intended to limit the present invention defined by the attached claims and their equivalents.

[0026] Step 1: Establish an environmental feature model and initialize algorithm parameters, including:

[0027] Step 1.1: Use optical sensors to collect environmental information and build a spatial point cloud map, including the distribution of obstacles and target points, and then define the task area and build a two-dimensional environmental feature model: First, model the task area as a two-dimensional plane space with a limited range, and model the obstacles as a circular area in the plane space with a radius of Ri and a center coordinate of (a i ,b i ),i=1,2,...,m, where m is the number of obstacles and the area covered by the obstacles is Get the coordinates of the own position as (x0, y0), and the coordinates of the target point as (x e ,y e ).

[0028] Step 1.2: Initialize the dynamic multimodal flock migration algorithm parameters, including: maximum number of iterations T, population size N, maximum number of searches T GOmax , migration index NDVI lim , Benchmark search step a / a leader , the number of subgroups K, the number of navigation points n, the historical optimal path set Ω, the capacity of the historical optimal path set O, etc.;

[0029] Step 1.3: Initialize the robot path planning solution set (flock) within the task area, and plan the robot's path using navigation points: select n navigation points between the robot's own position and the target position, and their coordinates are (x1, y1), (x2, y2), ..., (xn ,y n ), after reaching a navigation point, the robot moves forward with the next navigation point as the target until it reaches the target point;

[0030] Therefore, the coordinates of all navigation points can determine and form a solution for the robot path planning X = (x1, y1, x2, y2, ..., x n ,y n ), randomly generate N path planning solutions X i , i=1,2,....,N as the initial population and initial solution set, where X i Each component of is randomly generated within the task area.

[0031] Example 1: Take the case of a cargo robot delivering goods as an example. According to step 1, the current information of the task area is first detected by the sensor: there are 3 obstacles in the task area, and their coverage range is (x-20) 2 +(y-40) 2 =25, (x-40) 2 +(y-60) 2 =100 and (x-70) 2 +(y-30) 2 =100; the robot's own position coordinates are (5,5), and the target point coordinates are (100,100). Next, initialize the improved flock algorithm parameters: According to step 1, first initialize the parameters: T = 100, N = 150, T GOmax =10, NDVI lim =0.3, a=10, a leader =15, K=5, n=6, O = 10. Next, 150 path planning solutions X are randomly generated within the coordinate range of the mission area. i =[x1,y1,x2,y2,...,x6,y6], as the coordinates of the initial population, each path planning solution X i That is, it is regarded as a sheep in the herd.

[0032] Step 2: Update the flock according to the historical optimal path set, specifically including: if the historical optimal path set is empty, skip this step directly; otherwise, assuming that the number of solutions contained in the current historical optimal path set Ω is o, randomly select o solutions from the initial flock and replace them with the solutions in Ω;

[0033] Example 2, continued from Example 1.

[0034] Take the case of a cargo robot delivering goods as an example. During the first optimization, the historical optimal path set Ω is an empty set, so step 3 is directly executed; during subsequent optimization, all solutions in Ω are used to replace the solutions in the initial flock in equal amounts.

[0035] Step 3: Use K-means clustering to divide the flock in the solution space into K sub-groups, including: randomly selecting K solutions as centers in the initial flock, denoted as μ1, μ2, ..., μ K ; For each sheep in the flock, calculate the Euclidean distance D to the K centers respectively i,k =||X i -μ k || 2 , and assign it to the subgroup corresponding to the center with the smallest distance; for each subgroup, recalculate its center position as Where N k is the number of solutions contained in subgroup k; repeat the process of classifying individuals into subgroups and calculating the positions of subgroup centers until the positions of the subgroup centers no longer change. At this time, the division process is completed and the obtained K subgroups are expressed as

[0036] Example 3, continued from Example 2.

[0037] Take the case of a cargo robot delivering goods as an example. After updating the flock in step 2, randomly select 5 individuals as the subgroup centers; then classify the other individuals into the subgroups corresponding to the centers with the closest Euclidean distance; after the classification is completed, recalculate the center position of each subgroup, that is, the mean of the corresponding solutions of all individuals in each subgroup; after calculating the center position of each subgroup, the flock is cycled again in "classification-calculation center" until the center positions of the 5 subgroups no longer change.

[0038] Step 4: The search and migration mechanism of the standard herding algorithm is executed in parallel in each sub-group, including:

[0039] Step 4.1: Each subgroup executes the grazing operator to calculate the total path length and fitness function value of each sheep’s path planning scheme; the goal of robot path planning is to reach the target point with the shortest moving distance and avoid obstacles. The total length of the corresponding path can be calculated as Based on this, the designed fitness function is:

[0040]

[0041] Among them, ω is the penalty coefficient, which is usually set to a larger value; is a sign function that indicates whether the path trajectory overlaps with the obstacle. otherwise

[0042] Next, the one with the smallest fitness function in the subgroup is selected as the current optimal solution of the subgroup This sheep is the leader of the search; the sub-group performs random search to search for a better path planning solution in the local range of the solution space corresponding to itself. The search formula for each sheep is:

[0043]

[0044] Among them, t is the current iteration number of the calculation process, t GO This is the number of searches the flock has done in this iteration. is a vector whose components are randomly selected between [-1,1] and whose dimension is the same as Consistent, A is the search step length, and its value is determined as follows:

[0045] A=a-(a-0.1)·t / T (3)

[0046] For the leader, the search step length is:

[0047] A=a leader -(a leader -0.1)·t / T (4)

[0048] When a sheep completes the search, it calculates its fitness value according to step 4.1. If the fitness value of the newly searched solution is smaller, it updates its own individual optimal solution. At the same time, the sheep that searches for a better solution will also update its own migration index value. The update formula is:

[0049]

[0050] in, Represents the optimal solution searched by the sheep in one iteration, which is recorded as its own individual optimal solution.

[0051] Step 4.2 Each subgroup independently evaluates to determine whether it meets the conditions for ending the grazing operator. The specific conditions are as follows:

[0052] (1) Calculate the average migration index value of the subgroup according to the following formula

[0053]

[0054] (2) Update the current optimal solution of the group

[0055] (3) Whether to end the grazing operator is determined by the following two conditions:

[0056] Condition one: and Get updates.

[0057] Condition 2: t GO >T GOmax .

[0058] When condition 1 or condition 2 is met, execute step 4.3; otherwise, return to step 2.4.

[0059] Step 4.3 Each subgroup sorts all individuals in ascending order according to the fitness function value of each sheep's corresponding solution: 1, 2, ..., R is the order of arrangement. Select the sheep with the smallest fitness function value For the head sheep;

[0060] Step 4.4 Each subgroup executes the collective movement operator and migrates in the order determined in step 4.3, searching for a better path planning solution globally. The leader moves first:

[0061]

[0062] Among them, t CMO is the number of times the flock moves in the current collective motion operator process; C is the migration step length of the flock, and its value is:

[0063]

[0064] Where c is the pre-set benchmark migration step length. After the head sheep completes the movement, the other sheep move in sequence, and their movement directions are determined by three unit direction vectors, as follows:

[0065]

[0066] The moving direction of a sheep is determined by the linear combination of these three vectors. The specific calculation method and moving strategy are as follows:

[0067]

[0068] Among them, ω α ,ω β ,ω γ ∈R is the preset proportional coefficient in three directions, and its value satisfies ω α +ω β +ω γ = 1. After each subgroup moves in turn according to formulas (12) and (13), it is determined whether the fitness function value of the solution represented by the leader decreases. If it decreases, repeat step 4.4; if it does not decrease, execute step 4.5.

[0069] Step 4.5 Each subgroup executes the contraction strategy and converges around the current optimal solution of the subgroup:

[0070] The position remains unchanged, and the positions of the remaining sheep are updated based on the position of the head sheep and their own individual optimal solutions. The specific calculation method is as follows:

[0071]

[0072] Among them, δ1,δ2∈R are contraction coefficients, satisfying δ1+δ2=1, and the calculation method is:

[0073]

[0074] Step 4.6 Each subgroup executes a compensation strategy to increase the search range of the flock in the grazing operator phase in the next iteration and improve the algorithm's exploration ability:

[0075] A=e·(a-(a-0.1)·t / T) (15)

[0076] A leader =e·(a leader -(a leader -0.1)·t / T max ) (16)

[0077] Among them, e∈R is the compensation factor, which needs to be given in advance.

[0078] Example 4, continued from Example 3.

[0079] Take the case of a cargo robot delivering goods as an example. First, the five subgroups execute the herding operator respectively: according to step 4.1, the fitness function value of each sheep's corresponding path planning scheme is calculated and the head sheep is selected. All sheep perform random search according to formula (2) and update the individual optimal solution and migration index; every time all subgroups complete a search, they judge whether to continue searching according to the standard of step 4.2 until they decide to enter the next stage. Then, all subgroups are sorted according to the rules of step 4.3, the migration head sheep is selected, and the constants c = 10, ω α =0.5 / ω β =0.25 / ω γ =0.25. Then, the five subgroups execute the collective movement operator respectively: if the current optimal solution of the group is updated in the grazing operator stage, each subgroup migrates according to the method described in step 4.4 until the fitness function value of the solution represented by the head sheep no longer decreases; if the current optimal solution of the group is not updated in the grazing operator stage, all subgroups shrink according to step 4.5, and then execute step 4.6 to determine the search parameters for the next iteration, and the collective movement operator ends.

[0080] Step 5 determines whether to end this round of calculation according to the maximum number of iterations T. If t<T, return to step 4 to continue execution; if t=T, output the current optimal planning path;

[0081] Example 5, continued from Example 4.

[0082] Take the case of a cargo robot delivering goods as an example. After 100 iterations, the end condition is reached and the path planning solution corresponding to the optimal solution of the flock at the current moment is output: the coordinates of the 6 navigation points (x1, y1), (x2, y2), ..., (x6, y6) are given to the robot. The robot can use the controller to control its own trajectory based on the coordinates of the navigation points.

[0083] Step 6 updates the historical optimal path set according to the optimal path planning solution output in step 5. In the dynamic path planning problem, since the environment will change, the optimal path planning solution calculated once cannot meet the task requirements. Therefore, after calculating the current optimal path planning solution each time, this method will store it in the historical optimal path set as a reference for subsequent optimization calculations: the optimal solution obtained this time is recorded as X best , if the number of solutions contained in the historical optimal path set Ω is less than the capacity O, then X best Add to Ω; otherwise, randomly delete a solution from Ω and then add X best Add it in.

[0084] Example 6, continued from Example 5.

[0085] Take the case of a cargo robot delivering goods as an example. After the optimal solution is output in step 5, if the number of solutions contained in Ω is less than 10 at this time, the optimal solution is directly added; if the number of solutions contained in Ω is equal to 10, a solution is randomly deleted from it and the optimal solution at this time is added.

[0086] In step 7, an optical sensor is used to obtain current environmental information and determine whether the robot has completed the task. If the robot has reached the target point, the method ends; otherwise, the method returns to step 1.

[0087] Example 7, continued from Example 6.

[0088] Take the case of a delivery robot delivering goods as an example. After the algorithm completes one run, the sensor determines whether the robot has reached the delivery destination. If it has, the task is completed and the method ends; if it has not, it starts again from step 1 to start path planning for the new environment.

[0089] It can be seen that the dynamic path planning of the robot can be realized according to the steps described in the present invention.

[0090] This paper draws on the main calculation model of the flock migration optimization algorithm, integrates the flock memory mechanism and the multi-subgroup mechanism, proposes a new dynamic multimodal flock migration algorithm, and applies it to the robot dynamic path planning problem, and proposes a ground mobile robot autonomous dynamic path planning method based on the dynamic multimodal flock migration algorithm. This method can overcome the current shortcomings of robot autonomous dynamic path planning, such as slow strategy generation speed, low calculation efficiency, and poor real-time performance.

[0091] The examples described above have provided detailed explanations of the implementation methods of various parts of the present invention. The specific implementation forms of the present invention are not limited thereto. For those skilled in the art, various obvious changes to the method of the present invention without departing from the spirit of the method of the present invention and the scope of the claims are within the protection scope of the present invention.

Claims

1. A method for autonomous path planning of a ground mobile robot based on a dynamic multimodal flock migration algorithm, characterized in that: The following steps are involved: Step 1: Establish an environmental feature model and initialize algorithm parameters. Specifically, use optical sensors to collect environmental information, establish a spatial point cloud map, including the distribution of obstacles and the location of target points, and then define the task area and establish a two-dimensional environmental feature model; initialize algorithm parameters, including the original parameters of the flock migration algorithm, as well as the number of subgroups K, the number of navigation points n, the historical optimal path set Ω, and the capacity of the historical optimal path set O; in the task area, randomly generate N path planning solution vectors X according to the structural relationship between the navigation point coordinates and the solution. i ,i=1,2,...,N as the initial population and initial solution set; Step 2: Update the flock according to the historical optimal path set. Specifically, if the number of solutions in the current historical optimal path set Ω is o, randomly select o solutions from the initial flock and replace them with the solutions in Ω; if Ω is an empty set, skip this step; Step 3: Use K-means clustering to divide the flock in the solution space into K sub-groups. Specifically, randomly select K solutions as centers in the initial flock, denoted as μ1, μ2, ..., μ K ; For each sheep in the flock, calculate its Euclidean distance to the K centers respectively, and classify it into the subgroup corresponding to the center with the shortest distance; for each subgroup, recalculate its center position; repeat the process of classifying individual subgroups and calculating the subgroup center position until the position of each subgroup center no longer changes, at which point the division process is completed and K subgroups are obtained; Step 4: The search and migration mechanism of the standard herding algorithm is executed in parallel in each subgroup, specifically: each subgroup executes the grazing operator to search for a better path planning solution in the local range; the subgroup determines whether the conditions for ending the grazing operator are met; each subgroup is sorted from small to large according to the fitness function value, and the leader is selected; each subgroup executes the collective motion operator to search for a better solution in the global range; each subgroup executes the contraction strategy to converge with the current optimal solution as the center; Each subgroup implements a compensation strategy to improve the algorithm’s exploration capabilities; Step 5: Determine whether to end this round of calculation based on the maximum number of iterations T. If t<T, return to step 4 to continue execution; if t=T, output the current optimal planning path; Step 6: Update the historical optimal path set Ω according to the optimal planning path output in step 5. Specifically, let the optimal planning path obtained this time be X best , if the number of solutions contained in the historical optimal path set Ω is less than the capacity O, then directly best Add to Ω; otherwise, randomly delete a solution from Ω and then add X best Add it in. Step 7: Use an optical sensor to obtain current environmental information and determine whether the robot has completed the task. If the robot has reached the target point, the method ends. Otherwise, return to the first step.

Images