Dynamic path planning system based on particle swarm optimization algorithm

Abstract

The invention discloses a dynamic path planning system based on a particle swarm optimization algorithm, and the system comprises a planning center which is connected with a data collection module, a data processing module, a path planning module, and a feedback adjustment module. The data acquisition module is used for acquiring and storing environment information in a moving space of the robot; the data processing module is used for processing the collected environment information to obtain a corresponding processing result, and constructing a grid map according to the processing result; the path planning module is used for obtaining a corresponding objective function according to the obtained grating map and performing path planning according to the obtained objective function to obtain a corresponding initial path; the feedback adjustment module is used for monitoring the driving path of the corresponding robot according to the initial path to obtain a corresponding monitoring result, and performing path adjustment according to the monitoring result; the running efficiency and safety of the robot are improved.

Description

Technical Field

[0001] The present invention relates to the fields of robotics and computer science, and more specifically, to a dynamic path planning system based on the particle swarm optimization algorithm. Background Art

[0002] With the development of technology, robots are increasingly widely used in various fields, such as industrial production, home service, medical health, etc.; however, the problems of autonomous driving and path planning of robots in complex environments have always been the focus and difficulty of research;

[0003] Traditional path planning methods often rely on fixed environmental models and cannot adapt to environmental changes. Moreover, when dealing with large-scale or high-dimensional problems, the computational complexity is high and the efficiency is low; therefore, a new path planning method is needed that can adjust the path in real time according to environmental changes to improve the driving efficiency and safety of robots. For this purpose, we provide a dynamic path planning system based on the particle swarm optimization algorithm. Summary of the Invention

[0004] The purpose of the present invention is to provide a dynamic path planning system based on the particle swarm optimization algorithm.

[0005] The purpose of the present invention can be achieved through the following technical solutions: A dynamic path planning system based on the particle swarm optimization algorithm includes a planning center, which is connected to a data acquisition module, a data processing module, a path planning module, and a feedback adjustment module;

[0006] The data acquisition module is used to collect environmental information within the activity space of the robot and store it;

[0007] The data processing module is used to process the collected environmental information, obtain corresponding processing results, and construct a grid map based on the processing results;

[0008] The path planning module is used to obtain a corresponding objective function based on the obtained grid map and perform path planning based on the obtained objective function to obtain a corresponding initial path;

[0009] The feedback adjustment module is used to supervise the driving path of the corresponding robot based on the initial path, obtain corresponding supervision results, and perform path adjustment based on the supervision results.

[0010] Further, the data acquisition module includes a distance acquisition terminal and an image acquisition terminal. The distance acquisition terminal is used to collect distance data between itself and objects within the activity space; the image acquisition terminal is used to perform image acquisition on the activity space of the robot to obtain corresponding image data;

[0011] Pack the obtained image data and distance data to obtain corresponding environmental information, and send the obtained environmental information to the planning center for storage.

[0012] Further, the process of the data processing module processing the collected environmental information to obtain corresponding processing results includes:

[0013] Read the stored environmental information, perform feature recognition on the image data in the environmental information to obtain feature data of objects in the corresponding activity space; fuse the obtained feature data and distance data to obtain the position information of the corresponding objects in the activity space;

[0014] Geometrize the objects in the activity space according to the obtained feature data to obtain corresponding object geometric figures.

[0015] Further, the process of constructing a grid map based on the obtained processing results includes:

[0016] Create a two-dimensional plane graph, which consists of several blank rectangular cells. Map the robot's activity space into the two-dimensional plane graph according to the obtained position information and object geometric figures. Mark the rectangular cells occupied by the object geometric figures as no-go areas, and mark the other rectangular cells except the no-go areas as feasible areas to obtain the corresponding grid map.

[0017] Further, the process of the path planning module obtaining the corresponding objective function according to the obtained grid map includes:

[0018] Create a two-dimensional coordinate system, take the lower left corner of the obtained grid map as the coordinate origin, and map it into the created two-dimensional coordinate system;

[0019] Obtain the current position of the robot in the activity space, take it as the starting point, and take the position the robot is about to go to as the target point;

[0020] Obtain the corresponding reference path according to the starting point and the target point, and at the same time obtain the number of objects in the activity space, and obtain the corresponding particle swarm according to it. Initialize and assign values to each particle node in the particle swarm obtained based on the particle swarm optimization algorithm;

[0021] Calculate the path safety factor, path length, and path smoothness of the corresponding reference path based on the particle swarm optimization algorithm;

[0022] Obtain the corresponding objective function according to the obtained path safety factor, path length, and path smoothness.

[0023] Further, the process of performing path planning according to the obtained objective function to obtain the corresponding initial path includes:

[0024] Set an iteration threshold, use the obtained objective function as the fitness value function, and based on it, perform iterations on each particle node in the particle swarm, record the fitness value after iteration, compare the obtained fitness value with the initial value, and update the parameters of the corresponding particle node according to the comparison result; and store the updated parameters to obtain the corresponding path array, obtain the corresponding initial path based on the obtained path array, and determine whether the corresponding iteration times meet the iteration threshold;

[0025] If not, use the obtained initial path as the reference path, re-plan the path through the path planning module to obtain the corresponding new initial path, and again determine whether the iteration times corresponding to the obtained new initial path meet the iteration threshold, and so on;

[0026] If satisfied, output the corresponding initial path.

[0027] Further, the process of the feedback adjustment module for supervising the driving path of the corresponding robot based on the initial path to obtain the corresponding supervision result includes:

[0028] Supervise the actual driving data of the robot in real time, compare the obtained driving data with the output initial path according to the supervision, and judge whether there is a deviation in the driving process of the corresponding robot according to the comparison result;

[0029] If there is no deviation, compare the grid map corresponding to the initial path planning with the currently updated grid map in real time, and judge whether they match according to the comparison result. If they match, no other operations are performed; if they do not match, generate a corresponding path adjustment notice and feedback it to the planning center;

[0030] If there is a deviation, generate a corresponding path adjustment notice and feedback it to the planning center.

[0031] Further, the process of path adjustment based on the obtained supervision result includes:

[0032] Use the corresponding initial path as the reference path and feedback it to the path planning module. The path planning module obtains the latest driving path of the corresponding robot again based on the current grid map and the corresponding initial path using the particle swarm optimization algorithm, outputs it and replaces the original initial path;

[0033] Control the robot to drive according to the replaced initial path, continuously supervise the driving situation of the robot, and continuously update the grid map in real time and recalculate the initial path as needed to achieve real-time planning and adjustment of the robot's driving path.

[0034] Compared with the prior art, the beneficial effects of the present invention are as follows: By identifying the characteristic data of an object, more accurate position information and characteristic data are obtained, improving the accuracy of path planning; By introducing the particle swarm optimization algorithm, an optimal path planning scheme can be automatically found, greatly improving the efficiency and accuracy of robot task execution. At the same time, through dynamic adjustment, the path planning strategy can be adjusted in real time according to environmental changes and task requirements, enabling the robot to adapt to complex and changing working environments; By creating a grid map and marking no-go areas, collisions between the robot and obstacles can be avoided, improving the driving safety of the robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0035] Figure 1 It is a schematic diagram of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0036] As Figure 1 shown, a dynamic path planning system based on the particle swarm optimization algorithm includes a planning center, and the planning center is connected to a data acquisition module, a data processing module, a path planning module, and a feedback adjustment module;

[0037] The data acquisition module is used to collect environmental information within the activity space of the robot and store it;

[0038] The data processing module is used to process the collected environmental information, obtain corresponding processing results, and construct a grid map based on the processing results;

[0039] The path planning module is used to obtain a corresponding objective function based on the obtained grid map and perform path planning based on the obtained objective function to obtain a corresponding initial path;

[0040] The feedback adjustment module is used to supervise the driving path of the corresponding robot based on the initial path, obtain corresponding supervision results, and perform path adjustment based on the supervision results;

[0041] It should be further noted that in the specific implementation process, the process of the data acquisition module collecting environmental information within the activity space of the robot and storing it includes:

[0042] The data acquisition module includes a distance acquisition terminal and an image acquisition terminal; the distance acquisition terminal is used for the distance data between itself and the objects within the activity space of the robot; the image acquisition terminal is used to perform image acquisition on the activity space of the robot to obtain corresponding image data; The obtained image information and distance data are packaged to obtain corresponding environmental information, which is uploaded to the planning center for storage.

[0043] It should be further noted that, in the specific implementation process, the process by which the data processing module processes the collected environmental data to obtain the corresponding grid map includes:

[0044] Read the obtained environmental information, use image recognition technology to perform feature recognition on the image data in the environmental information, and obtain the feature data of the corresponding objects in the robot's activity space. The feature data includes the size, contour, and boundary features of the objects;

[0045] Fuse the obtained distance data and feature data to obtain the position information of the corresponding objects;

[0046] Geometrize the corresponding objects based on the obtained feature data to obtain the corresponding object geometric figures;

[0047] Create a two-dimensional plane graph. The two-dimensional plane graph is composed of several blank rectangular cells. Map the activity space to the two-dimensional plane graph in a 1:1 ratio according to the obtained position information and object geometric figures. Mark the rectangular cells occupied by the object geometric figures as no-go areas, and mark the other rectangular cells except the no-go areas as feasible areas to obtain the corresponding grid map;

[0048] It should be further noted that, in the specific implementation process, the data processing module will read the stored environmental information in real time and update the corresponding grid map according to the read environmental information; for example: when there are new additions or removals of objects in the activity space, the data processing module will add or delete the no-go areas in the grid map; if the position and feature data of the objects in the activity space change, the corresponding rectangular cells will also be updated synchronously.

[0049] It should be further noted that, in the specific implementation process, the process by which the path planning module obtains the corresponding objective function according to the obtained grid map includes

[0050] The path planning module includes an objective function unit and a particle swarm optimization unit;

[0051] The process by which the objective function unit is used to obtain the objective function of the particle swarm optimization algorithm includes:

[0052] Create a two-dimensional coordinate system, use the lower left corner of the obtained grid map as the coordinate origin, and map it to the created two-dimensional coordinate system. It should be further noted that a rectangular cell in the grid map corresponds to a coordinate;

[0053] Obtain the current position of the robot in the activity space, use it as the starting point, denoted as Q, then the coordinates of the corresponding starting point are (X Q， Y Q), and mark the corresponding path node as P Q ; Take the position where the robot is about to go as the target point Z, then the corresponding coordinates are (X Z , Y Z ), and mark the corresponding path node as P Z , It should be further noted that the path node refers to the path position searched by the particle node in the grid map;

[0054] Randomly construct a driving path based on the starting point Q and the target point Z, and use it as the reference path; Obtain the number of objects in the robot's activity space, number them from left to right and from top to bottom, denoted as j, where j = 1, 2,..., m, m > 0 and m is an integer, and calculate the corresponding number N of particle nodes in the required particle swarm G according to it;

[0055] Among them, N = 10 + floor(m 1.1 ) × 30;

[0056] In the formula, floor is a mathematical symbol representing the operation of rounding down;

[0057] It should be further noted that the reference path may collide with objects, that is, the corresponding reference path is the path to be optimized;

[0058] Number all the particle nodes in the obtained particle swarm G, denoted as i, i = 1, 2,..., N; Map the obtained particle swarm G to the coordinate system, then the coordinates of the corresponding particle node are (xi, yi), and mark the path node searched by the corresponding particle node i as P i ; Then the corresponding reference path is expressed as (P Q , P1, P2,..., Pn, P Z ), where n ∈ N;

[0059] According to the particle swarm optimization algorithm, randomly assign values to the initial velocity and initial position of each particle node in the particle swarm G, denoted as Vi and Xi respectively; At the same time, initialize other parameters in the particle swarm G, and the parameters include individual optimal fitness value, group optimal fitness value, and dynamic inertia weight, denoted as Pbesti, Gbest, and Ω respectively;

[0060] Obtain the coordinate positions corresponding to the path nodes that make up the reference path, and judge whether the corresponding path node is a dangerous node based on it;

[0061] It should be further noted that in the specific implementation process, the process of judging whether the corresponding path node is a dangerous node includes:

[0062] Obtain the distance between the corresponding path node and the object passed by the reference path, denoted as D i (m)

[0063] Among them,

[0064] In the formula, (xi, yi) are the coordinates corresponding to the path node searched by the i-th particle node; (x d (m), y d (m)) are the central coordinates of the m-th object in the map;

[0065] Set the collision threshold D 碰 ; If D i (m) ≤ D 碰 , it indicates that the corresponding path node collides with the object, and mark it as a dangerous node;

[0066] If D i (m) > D 碰 , it indicates that the corresponding path node is a safe node;

[0067] Obtain the number of path nodes marked as dangerous nodes among all path nodes of the reference path, denoted as u, then obtain the corresponding path safety factor E1 according to the number of dangerous nodes;

[0068] Di(m) ≤ D 碰 ;

[0069] In the formula, Mi is generally set to a fixed value, and the specific value depends on actual requirements; and u ∈ n;

[0070] Calculate the path length of the corresponding reference path according to the Euclidean algorithm, and denote the corresponding path length as L;

[0071] Among them,

[0072] In the formula, (x i (t), y i (t)) is the coordinate position of the particle node i at time t,

[0073] Calculate the path smoothness PH of the corresponding reference path according to the dynamic programming algorithm;

[0074] Among them,

[0075] In the formula, di is the distance between particle node i-1 and particle node i in the expected path; di+1 is the distance between particle node i+1 and particle node i in the expected path; αi is the included angle between di and di+1, where particle node i+1 and particle node i-1 intersect at particle node i;

[0076] Then, a target function fit for the corresponding particle node is constructed by linearly combining the obtained path safety factor, path length, and path smoothness.

[0077] Among them, fit = λ1 × E1 + λ2 × PH + λ3 × L;

[0078] In the formula, λ1 is the path safety weight, λ2 is the path smoothness weight, λ3 is the path length weight, and λ1, λ2, λ3 all belong to (0, 1);

[0079] It should be further noted that in the specific implementation process, the process of the particle swarm optimization unit performing path planning based on the obtained target function includes:

[0080] Set the iteration threshold H, use the obtained target function as the fitness value function, and perform iteration on each particle node in the particle swarm G according to it, and record the fitness value SD after iteration i , and compare the obtained fitness value with the individual optimal fitness value;

[0081] If SD i ≥ pbesti; then no other operations are performed;

[0082] If SD i <pbesti, then update the position and velocity of the corresponding particle node i, and the corresponding update formulas are as follows;

[0083] v i (t + 1) = Ωivi(t) + c1r1(t)[pi(t) - xi(t)] + c2r2(t)[pg(t) - xi(t)];

[0084] X i (t + 1) = X i (t) + Vi(t + 1);

[0085] In the formula, Ωi is the dynamic inertia weight of particle i; pi(t) is the optimal position searched by particle i at time t; pg(t) is the globally optimal position searched at time t; c1 is the population learning factor that sets the length of each movement of the particle in the individual optimal direction; c2 is the population learning factor that sets the length of each movement of the particle in the population optimal direction; r1 and r2 are uniformly distributed random numbers in the range of [0, 1]; Vi ∈ [-vmax, vmax], and vmax is the maximum speed of the particle;

[0086]

[0087] It should be further noted that in the specific implementation process, the larger the value of the dynamic concern weight Ω, the stronger the global contraction ability of the corresponding particle swarm, resulting in a greater search ability in the early stage of iteration to ensure the algorithm efficiency. In the later stage of iteration, the search speed decreases, improving the search accuracy. The specific adaptive adjustment formula is as follows:

[0088]

[0089] In the formula, Ωmax and Ωmin are the maximum and minimum values of Ω respectively, and h is the current iteration number;

[0090] Obtain the updated particle velocity and particle position of each particle node in the corresponding particle swarm. At the same time, store the position of the current optimal individual extreme value pbesti as the optimal position value, and obtain the corresponding path array;

[0091] Obtain the corresponding initial path based on the obtained path data; judge the individual optimal fitness value and the group optimal fitness value of the obtained initial path, and judge whether the corresponding iteration number meets the iteration threshold;

[0092] If not, use the obtained initial path as the reference path, re-plan the path through the path planning module to obtain the corresponding new initial path, and then judge whether the iteration number corresponding to the obtained new initial path meets the iteration threshold, and so on;

[0093] If it meets, output the corresponding initial path;

[0094] It should be further noted that in the specific implementation process, the process in which the feedback adjustment module supervises the driving path of the corresponding robot based on the initial path, obtains the corresponding supervision result, and adjusts the path according to the supervision result includes:

[0095] Supervise the actual driving data of the robot in real time. The driving data includes position, speed, acceleration, and direction; compare the driving data obtained by supervision with the output initial path, and judge whether there is a deviation in the driving process of the corresponding robot according to the comparison result,

[0096] If there is no deviation, the feedback adjustment module compares the grid map corresponding to the planned initial path with the currently updated grid map in real time, and judges whether they match according to the comparison result. If they match, no other operations are performed; if they do not match, generate a corresponding path adjustment notice and feedback it to the planning center;

[0097] If there is a deviation, generate a corresponding path adjustment notice and feedback it to the planning center;

[0098] It should be further noted that in the specific implementation process, the process of recalculating the initial path of the corresponding robot according to the path adjustment notice includes:

[0099] The planning center feeds back the corresponding initial path to the path planning module as a reference path; the path planning module re-uses the particle swarm optimization algorithm based on the current grid map and the corresponding initial path to obtain the latest driving path of the corresponding robot, outputs it and replaces the original initial path; then the feedback adjustment module controls the robot to drive according to the replaced initial path, continuously monitors the driving situation of the robot, and updates the grid map in real time and recalculates the optimal driving path according to the requirements, so as to realize the real-time planning and adjustment of the robot driving path.

[0100] The above embodiments are only used to illustrate the technical method of the present invention and not to limit it. Although the present invention has been described in detail with reference to the preferred embodiments, those of ordinary skill in the art should understand that the technical method of the present invention can be modified or equivalently replaced without departing from the spirit and scope of the technical method of the present invention.

Claims

1. A dynamic path planning system based on the particle swarm optimization algorithm, including a planning center, characterized in that, The planning center is connected to a data acquisition module, a data processing module, a path planning module, and a feedback adjustment module; The data acquisition module is used to collect the environmental information in the robot's activity space and store it; The data processing module is used to process the collected environmental information to obtain corresponding processing results, and construct a grid map based on the processing results; The path planning module is used to obtain a corresponding objective function according to the obtained grid map, and perform path planning according to the obtained objective function to obtain a corresponding initial path; The feedback adjustment module is used to supervise the driving path of the corresponding robot according to the initial path, obtain corresponding supervision results, and perform path adjustment according to the supervision results.

2. The dynamic path planning system based on the particle swarm optimization algorithm according to claim 1, characterized in that, The data acquisition module includes a distance acquisition terminal and an image acquisition terminal. The distance acquisition terminal is used to collect the distance data between itself and the objects in the activity space; the image acquisition terminal is used to collect images of the robot's activity space to obtain corresponding image data; The obtained image data and distance data are packaged to obtain corresponding environmental information, and the obtained environmental information is sent to the planning center and stored.

3. A dynamic path planning system based on the particle swarm optimization algorithm according to claim 2, characterized in that The process of the data processing module processing the collected environmental information to obtain corresponding processing results includes: Reading the stored environmental information, performing feature recognition on the image data in the environmental information to obtain the feature data of the objects in the corresponding activity space; fusing the obtained feature data and distance data to obtain the position information of the corresponding objects in the activity space; Geometrically representing the objects in the activity space according to the obtained feature data to obtain corresponding object geometric figures.

4. A dynamic path planning system based on the particle swarm optimization algorithm according to claim 1, characterized in that, The process of constructing a grid map based on the obtained processing results includes: Creating a two-dimensional plan view, which consists of several blank rectangular cells. The robot's activity space is mapped 1:1 to the two-dimensional plan view according to the obtained position information and object geometric figures. The rectangular cells occupied by the object geometric figures are marked as no-go areas, and the other rectangular cells except the no-go areas are marked as feasible areas to obtain the corresponding grid map.

5. A dynamic path planning system based on the particle swarm optimization algorithm according to claim 4, characterized in that, The process of the path planning module obtaining a corresponding objective function according to the obtained grid map includes: Creating a two-dimensional coordinate system, taking the lower left corner of the obtained grid map as the coordinate origin, and mapping it to the created two-dimensional coordinate system; Obtaining the current position of the robot in the activity space, taking it as the starting point, and taking the position that the robot is about to go to as the target point; Obtaining a corresponding reference path according to the starting point and the target point, and at the same time obtaining the number of objects in the activity space, and obtaining a corresponding particle swarm according to it. Each particle node in the obtained particle swarm is initialized and assigned values based on the particle swarm optimization algorithm; Calculating the path safety factor, path length, and path smoothness of the corresponding reference path based on the particle swarm optimization algorithm; Obtaining a corresponding objective function according to the obtained path safety factor, path length, and path smoothness.

6. The dynamic path planning system based on the particle swarm optimization algorithm according to claim 5, characterized in that The process of performing path planning according to the obtained objective function to obtain a corresponding initial path includes: Set an iteration threshold, use the obtained objective function as the fitness value function, iterate each particle node in the particle swarm based on it, record the fitness value after iteration, compare the obtained fitness value with the initial assignment, and update the parameters of the corresponding particle node according to the comparison result; and store the updated parameters to obtain the corresponding path array, obtain the corresponding initial path based on the obtained path array, and determine whether the corresponding number of iterations meets the iteration threshold; If not, use the obtained initial path as the reference path and re-plan the path through the path planning module; If it is satisfied, output the corresponding initial path.

7. A dynamic path planning system based on the particle swarm optimization algorithm according to claim 6, characterized in that, The feedback adjustment module is used to supervise the driving path of the corresponding robot based on the initial path. The process of obtaining the corresponding supervision result includes: Real-time supervise the actual driving data of the robot, compare the driving data obtained by supervision with the output initial path, and judge whether there is a deviation in the driving process of the corresponding robot according to the comparison result; If there is no deviation, compare the grid map corresponding to the initial path planning with the currently real-time updated grid map, and judge whether they match according to the comparison result. If they match, no other operations are performed; if they do not match, generate a corresponding path adjustment notice and feedback it to the planning center; If there is a deviation, generate a corresponding path adjustment notice and feedback it to the planning center.

8. A dynamic path planning system based on a particle swarm optimization algorithm according to claim 7, characterized in that, The process of adjusting the path according to the generated path adjustment notice includes: When receiving the path adjustment notice, feed the corresponding initial path back to the path planning module as the reference path. The path planning module obtains the latest driving path of the corresponding robot again based on the real-time updated grid map and the corresponding initial path using the particle swarm optimization algorithm, outputs it and replaces the original initial path; Control the robot to drive according to the replaced initial path, continuously supervise the driving situation of the robot, and update the grid map in real time and recalculate the initial path according to the requirements.

Images