A multi-robot collaborative three-dimensional path planning method and system

Abstract

The present invention discloses a method and system for collaborative three-dimensional path planning for multiple robots. This method breaks down the robot path into path segments constructed from path nodes, then uses the amplitude, elevation, and azimuth of the path segments to construct path vectors to represent particle positions. A spherical vector particle swarm algorithm is then introduced and optimized based on a particle spatial position transformation strategy based on game theory. A fitness function representing the cost is set, and the spherical vector particle swarm algorithm is used to iteratively update the particle position corresponding to each robot until an iterative termination condition is met. The optimal particle position for each robot is obtained, and a collision-free path for collaborative multi-robot planning is output. This method can better manage the relationships between robots, enhancing the security and feasibility of path planning in a shared environment while maintaining the flexibility and adaptability of the multi-robot system.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot path planning, and specifically relates to a multi-robot collaborative three-dimensional path planning method and system, in particular to a path planning method that integrates game theory and particle swarm hybrid optimization method. Background Art

[0002] In recent years, game theory has been introduced to the problem of multi-robot cooperation. Wu designed a new distributed multi-agent task allocation framework based on potential game methods to solve the dynamic multi-agent task allocation problem, ensuring that any Nash equilibrium has at least 50% suboptimality, and the optimal Nash equilibrium represents the optimal solution. Zheng Yanbin used game theory to construct a dynamic obstacle avoidance model among multi-agents in a two-stage path planning algorithm. He applied a virtual action method to resolve the game problems at conflict points and in the selection of multiple Nash equilibria, thereby reducing the total path cost and improving the algorithm's convergence speed. However, the difficulty of considering all possible scenarios limits the success rate of reaching a Nash equilibrium. This limitation, in turn, affects the stability and reliability of path planning. Guo Yaning used game theory and multi-agent reinforcement learning algorithms to implement collaborative path planning for multi-agent systems. This cooperative game framework extends traditional single-agent Q-learning algorithms to multi-agent systems. While this framework may perform well in specific scenarios, its generalization ability is limited. Furthermore, the frequent reliance on communication between robots can lead to communication delays, which can affect the overall performance and responsiveness of the system.

[0003] Furthermore, collision avoidance, risk reduction, and optimal path identification are crucial for robot path planning in complex environments. Traditional grid-based and sampling-based methods often struggle with scalability and efficiency in dynamic, high-dimensional, or constrained environments. In contrast, intelligent optimization algorithms such as gray wolf optimization, differential evolution, genetic algorithms, particle swarm optimization, and ant colony optimization have garnered widespread attention. These algorithms use nature-inspired strategies to enhance global search capabilities and robustness for complex pathfinding tasks. Compared to other nature-inspired algorithms, particle swarm optimization (PSO) excels at finding stable global solutions in a shorter computational time. However, the PSO algorithm is insensitive to variations in initial conditions and objective functions. It primarily relies on a small set of parameters, including an acceleration coefficient and two weighting factors, to adapt to various environmental configurations. Subsequently, researchers have proposed several variants, such as the classic PSO, phase-encoded PSO, quantum-behaved PSO (QPSO), and discrete PSO (DPSO). These variants share the same population-based structure but differ in how the search space is represented and the solutions are encoded in the particles. Phung et al. proposed a spherical vector-based particle swarm optimization (SPSO) algorithm for trajectory planning of unmanned aerial vehicles in complex environments. SPSO expands the algorithm's solution space, enabling it to adapt to diverse complex environments and dynamic constraints, thereby improving the algorithm's performance. Although SPSO broadens the solution space, it is not suitable for multi-robot systems due to its lack of ability to handle complex interactions between multiple robots.

[0004] Therefore, when exploring multi-robot path planning in a complex three-dimensional environment, how to more efficiently plan safe and high-quality robot paths has always been a problem to be studied / solved. Summary of the Invention

[0005] The purpose of the present invention is to plan safe and high-quality robot paths, and further provide a multi-robot collaborative three-dimensional path planning method and system, wherein the technical solution of the present invention is to achieve path planning by integrating game theory and particle swarm hybrid optimization method. By integrating game theory methods to improve the particle swarm optimization algorithm, that is, introducing game theory strategies to adjust the spatial position of particles, the aim is to resolve conflicts between robots and enhance coordination between multiple robots. Among them, game theory can optimize the collaborative behavior of the multi-robot system at the global level and optimize the path planning of a single robot at the local level, thereby better handling the relationship between robots, enabling it to enhance the safety and feasibility of path planning in a shared environment, and maintain the flexibility and adaptability of the multi-robot system.

[0006] To this end, the present invention provides the following technical solutions:

[0007] In one aspect, the present invention provides a multi-robot collaborative three-dimensional path planning method, comprising the following steps:

[0008] S1: Obtain map information of the area to be planned;

[0009] S2: Introduce the spherical vector particle swarm algorithm and optimize it based on the particle space position transformation strategy of game theory; then set a corresponding particle population for each robot, and the particle position of each particle in the particle population represents a path of the corresponding robot. The path planning problem is converted into a multi-objective optimization problem, and cost reduction is taken as the optimization goal. A fitness function representing the cost is constructed, and the spherical vector particle swarm algorithm is used to iteratively update the particle position corresponding to each robot until the iteration termination condition is met. The optimal particle position corresponding to each robot is obtained, and then a collision-free path for multi-robot collaborative planning is output;

[0010] The robot path is split into path segments constructed by path nodes, and then the path vector is constructed using the amplitude, elevation angle, and azimuth angle of the path segment to represent the particle position;

[0011] The process of each iterative operation is as follows: first, the particle position is updated based on the initially set fitness function and particle position equation as well as the current particle position; then, based on the updated particle position, it is determined whether there is a collision conflict between the multiple robots; if there is a collision conflict, the particle space position transformation strategy of game theory is used to optimize the fitness function and particle position equation, and then the particle position is updated again; then the next iterative operation is entered; if there is no collision conflict, the next round of iterative operation is entered based on the currently updated particle position.

[0012] In path planning, a safety distance factor is typically defined. When the distance between two robots falls below this preset safety distance, the system determines a collision threat. This method determines whether there is a collision risk by monitoring the relative distance between the robots.

[0013] Optionally, define the particle position Ω of particle i representing the robot i and the incremental vector ΔΩ representing the spatial velocity i Expressed as:

[0014] Ω i =(ρ i1 ,ψ i1 ,φ i1 ,ρ i2 ,ψ i2 ,φ i2 ,...,ρ iL ,ψ iL ,φ iL ),L=l-2

[0015] ΔΩ i =(Δρ i1 ,Δψi1 ,Δφ i1 ,Δρ i2 ,Δψ i2 ,Δφ i2 ,...,Δρ iL ,Δψ iL ,Δφ iL )

[0016] Where L represents the number of path nodes excluding the starting point and the end point, l is the total number of path nodes; amplitude ρ∈(0, path length), elevation angle and azimuth angle φ∈[-π,π]; Δρ, Δψ, Δφ correspond to the amplitude increment, elevation increment, and azimuth increment, respectively; subscripts i1, i2, iL correspond to the 1st, 2nd, and Lth path segments of particle i, respectively;

[0017] Define the position corresponding to the jth path segment of particle i (ρ ij ,ψ ij ,φ ij ) is expressed as e ij , speed (Δρ ij ,Δψ ij ,Δφ ij ) is expressed as Δe ij For a group of M particles, the particle position equation is optimized using the particle space position transformation strategy of game theory, and the updated particle position is expressed as:

[0018]

[0019] Where, They represent the strategies Δs of the j-th path segment of particle i at the k+1-th iteration and the k-th iteration respectively. ij , strategy Δs ij By speed Δe ij Converted from; w k is the exploration weight w of the kth iteration, c1 and c2 are both learning factors, are the position parameters of the jth path segment in the global optimal position and local optimal position of particle i in the kth iteration; r 1j , r 2j are two random samples in [0,1] from a uniform probability distribution; is the particle position e of the j-th path segment of particle i at the k-th and k+1-th iterations ij .

[0020] Optionally, when optimizing the fitness function using a particle spatial position transformation strategy based on game theory, the fitness function is a total game function or is converted from the total game function, and the total game function introduces the obstacle avoidance benefit generated by the strategy when the robot plays the game on the basis of the cost function;

[0021] Among them, the amplitude ρ∈(0, path length), the elevation angle and azimuth angle φ∈[-π,π]; Δρ′, Δψ′, Δφ′ are the amplitude increment, elevation increment, and azimuth increment, respectively; the particle position (ρ ij ,ψ ij ,φ ij ) is expressed as e ij , speed (Δρ ij ,Δψ ij ,Δφ ij ) is expressed as Δe ij , the strategy Δs of particle i on the jth path segment ij By speed Δe ij converted;

[0022] When the p robot plays against the q robot, the strategy Δs of the p robot ij The obstacle avoidance benefit v caused by the position change pq as follows:

[0023]

[0024] The total collision payoff of particle i of robot p Expressed as:

[0025]

[0026] Where Δρ′ ij , Δψ′ ij , Δφ′ ij is the strategy Δs ij Elements, corresponding to amplitude, elevation, azimuth, strategy Δs ij By speed Δe ij Converted from; N represents the number of robots involved in path planning, rand() is a random number generator that generates a random floating point number between 0 and 1, It represents the absolute profit when robot p plays against robot q, and k represents the robot number.

[0027] The technical solution of this invention preferentially integrates collision costs and multi-objective heuristic functions into the payoff structure of game theory, thereby optimizing the path planning quality of individual robots within a collaborative framework. Combining the advantages of both coupling and decoupling algorithms, the technical solution of this invention effectively coordinates the local and global objectives of the multi-robot system, thereby achieving a Nash equilibrium, ensuring global optimality in multi-robot path selection, and improving the overall performance of the system.

[0028] Optionally, the strategy Δs ij With the speed Δeij The relationship is as follows:

[0029]

[0030] Where rand() is a random number generator that generates a random floating-point number between 0 and 1. The superscripts + and - represent two different adjustment directions or changes. That is, in the event of a collision, + and - are used to indicate two possible ways of adjusting the variable. + may indicate an increase, while - indicates a decrease. The value of Δρ′ is preset, ij , Δψ′ ij , Δφ′ ij is the strategy Δs ij The elements correspond to the amplitude increment, elevation increment, and azimuth increment, and R is the diameter of the defined collision area.

[0031] Optionally, the cost type of the cost function is a complete combination or a partial combination of a safety constraint cost, an altitude cost, a path cost, and a smoothing cost.

[0032] Optionally, during each iteration, the iteration termination condition further includes: determining whether a Nash equilibrium state has been reached among all robots; if not, proceeding to the next round of iteration; if so, determining whether other iteration termination conditions have been met; if not, proceeding to the next round of iteration;

[0033] in, is the optimal strategy set of all competitors of the nth robot, is the optimal strategy set of the 1st, n-1th, n+1th, and Nth competitors, and the Nash equilibrium state is expressed as:

[0034]

[0035] Where, is the optimal strategy of the nth robot, U n Represents the total cost function of the game for the nth robot.

[0036] Optionally, the global optimal position of each robot in the spherical vector particle swarm algorithm is and the local optimal position Defined as:

[0037]

[0038] Where ξ represents the mapping of the vector set from the Ω space to the path points on the X plane; U is the fitness function, representing the cost; represents the position of the robot particle p Total cost Smaller than the previous position Total cost Update the local optimal position of particle p to position Otherwise, the local optimal position is still Indicates the location with the lowest total cost is the global best position.

[0039] In another aspect, the present invention further provides a multi-robot system based on the above-mentioned path planning method, characterized in that it includes a plurality of communication-connected robots and a collaborative control subsystem, wherein the robots are all connected to the collaborative control subsystem, and the collaborative control subsystem includes:

[0040] A map information acquisition module, configured to acquire map information of the area to be planned, wherein the map information includes at least obstacle information;

[0041] The particle optimization planning module is used to introduce a spherical vector particle swarm algorithm and optimize the spherical vector particle swarm algorithm based on the particle space position transformation strategy of game theory. A corresponding particle population is then set for each robot. The particle position of each particle in the particle population represents a path for the corresponding robot. The path planning problem is converted into a multi-objective optimization problem. With cost reduction as the optimization goal, a fitness function representing the cost is constructed. The spherical vector particle swarm algorithm is used to iteratively update the particle position corresponding to each robot until the iteration termination condition is met. The optimal particle position corresponding to each robot is obtained, and then a collision-free path for multi-robot collaborative planning is output.

[0042] The robot path is split into path segments constructed by path nodes, and then the path vector is constructed using the amplitude, elevation angle, and azimuth angle of the path segment to represent the particle position;

[0043] The process of each iterative operation is as follows: first, the particle position is updated based on the initially set fitness function and particle position equation as well as the current particle position; then, based on the updated particle position, it is determined whether there is a collision conflict between the multiple robots; if there is a collision conflict, the particle space position transformation strategy of game theory is used to optimize the fitness function and particle position equation, and then the particle position is updated again; then the next iterative operation is entered; if there is no collision conflict, the next round of iterative operation is entered based on the currently updated particle position.

[0044] In a third aspect, the present invention further provides a computer device comprising at least:

[0045] one or more processors;

[0046] a memory storing one or more computer programs;

[0047] Among them, the processor calls the computer program to implement: the steps of a multi-robot collaborative three-dimensional path planning method.

[0048] In a fourth aspect, the present invention further provides a computer-readable storage medium storing a computer program, wherein the computer program is called by a processor to implement: steps of a multi-robot collaborative three-dimensional path planning method.

[0049] Beneficial effects

[0050] Compared with the existing method, the advantages of the present invention are:

[0051] 1. The technical solution of this invention incorporates game theory, combining it with an intelligent path planning method to plan more specific and executable paths without the need for complex communication overhead. This method allows for multi-level optimization, where game theory can optimize the collaborative behavior of the multi-robot system at the global level and the path planning of individual robots at the local level. This method can better manage the relationships between robots and solve inter-robot collaboration issues, making it possible to enhance the safety and feasibility of path planning in shared environments while maintaining the flexibility and adaptability of the multi-robot system.

[0052] 2. The technical solution of this invention is an improved particle swarm optimization algorithm based on game theory. That is, by introducing game theory strategies to adjust the spatial position of particles, it aims to resolve conflicts between robots and enhance coordination among multiple robots. BRIEF DESCRIPTION OF THE DRAWINGS

[0053] Figure 1 is a schematic diagram of a multi-robot system provided by an embodiment of the present invention;

[0054] Figure 2 is a schematic diagram of a collision area and a danger area provided by an embodiment of the present invention;

[0055] Figure 3 is a schematic diagram of the flight altitude of a UAV relative to the ground provided by an embodiment of the present invention;

[0056] Figure 4 Schematic diagram of steering angle provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0057] This invention provides a method for collaborative three-dimensional path planning among multiple robots, designed to generate safe, high-quality, collision-free paths. This method integrates game theory and a particle swarm optimization algorithm to achieve collaborative three-dimensional path planning. A game-theory-based multi-robot path planning framework is constructed, using collision costs and multi-objective heuristic functions as game-theoretical gains to maintain game-theoretical interactions between robots. In an improved particle swarm optimization (PSO) algorithm, a particle spatial position transformation method is designed based on three-dimensional spatial vectors, serving as a game-theoretical strategy update mechanism. Furthermore, to avoid collisions between robots, each robot adjusts its cooperative strategy based on the behavior of the other robots. Each robot selects the optimal cooperative strategy, gradually approaching a Nash equilibrium.

[0058] To this end, the technical solution of the present invention provides a multi-robot collaborative three-dimensional path planning method with the following technical ideas:

[0059] S1: Obtain map information of the area to be planned;

[0060] S2: Introducing the spherical vector particle swarm algorithm and optimizing it based on the particle spatial position transformation strategy of game theory; then characterizing the robot path with the particle position, setting a fitness function that characterizes the cost, and iteratively updating the particle position corresponding to each robot using the spherical vector particle swarm algorithm until the iteration termination condition is met, obtaining the optimal particle position corresponding to each robot, and then outputting a collision-free path for multi-robot collaborative planning;

[0061] The process of each iterative operation is as follows: first, the particle position is updated based on the initially set fitness function and the current particle position; then, based on the updated particle position, it is determined whether there is a collision conflict between the multiple robots; if there is a collision conflict, the particle position equation and the fitness function are optimized using the particle space position transformation strategy of game theory, and then the particle position is updated again; then the next iterative operation is entered; if there is no collision conflict, the next round of iterative operation is entered based on the currently updated particle position.

[0062] The present invention will be further described below with reference to the embodiments.

[0063] Example 1:

[0064] Based on the above technical ideas, the embodiment of the present invention will be described in detail to provide a multi-robot collaborative three-dimensional path planning method, including the following steps:

[0065] Step 1: Obtain map information of the area to be planned and initialize parameters.

[0066] Multi-robot path planning involves multiple robots collaboratively planning individual motion paths to achieve mission objectives in complex environments. Optimizing path selection ensures safe obstacle avoidance, prevents collisions, and improves overall system efficiency.

[0067] like Figure 1 As shown, the multi-robot system of this embodiment includes multiple ground unmanned vehicles (UGVs), multiple unmanned aerial vehicles (UAVs) and a UGV / UAV collaborative control subsystem for solving complex tasks. The robots in the same system communicate with each other, that is, communicate with each other through wireless communication technology to obtain each other's positions. In order to better describe the working environment, a three-dimensional map is used to simulate the known spatial information and the position of obstacles in a complex environment. The multi-unmanned aerial vehicles (UAVs) and multi-ground unmanned vehicles (UGVs) of this embodiment can use on-board sensors to detect the surrounding environment, identify and locate surrounding obstacles and other robots during the path planning process.

[0068] It should be understood that the map information obtained in step 1 contains at least obstacle information, which is used for subsequent collision avoidance. This is explained in detail in the cost function section below. Parameter initialization includes: parameter learning factors c1 and c2, exploration weight w, constraint weights b1, b2, b3, b4, and b5; as well as setting the starting and ending points, population size, and number of iterations. Since these are all conventional settings, they will not be elaborated on in this document.

[0069] Step 2: For each robot, the particle position is used to represent the robot's path, and then a random path is created and the spatial velocity ΔΩ of the initial particle is initialized. i , spatial position Ω i , the global optimal position Q g , local optimal position Q i .

[0070] In the particle swarm algorithm, due to the insufficient ability to handle the complexity of the environment and the failure to consider the interaction between multiple robots, the local path planning performance of the multi-robot system is poor. In order to optimize the path of a single robot, the collaboration between robots is considered to ensure overall efficiency and safety. Therefore, the technical solution of the present invention introduces game theory. In order to combine with game theory and adapt to complex spatial multi-robot pathfinding, the present invention proposes a spherical vector particle swarm algorithm, that is, each feasible path of the robot is encoded into a set of vectors, each vector represents the movement of the drone from the current waypoint to another waypoint. These vectors include three components in the three-dimensional coordinate system, namely, amplitude ρ∈(0, path length), elevation angle and azimuth angle φ∈[-π,π]. The motion constraints of the UAV are realized through spatial angle constraints, and the particle search space is optimized.

[0071] For example:

[0072] For the robot, the path x i Represents a list of l waypoints that the robot needs to pass through. Each waypoint corresponds to a path node in the map, with coordinates P ij =(x ij ,y ij ,z ij ) or P ij =(x ij ,y ij ), P ij ∈O, where O represents the operating space of the drone and the path between two path nodes is a path segment. The amplitude, elevation, and azimuth of all path segments of a path are used to construct a path vector to represent the particle position. Therefore, the path Ω of particle i is i The vector set is as follows:

[0073] Ω i =(ρ i1 ,ψ i1 ,φ i1 ,ρ i2 ,ψ i2 ,φ i2 ,...,ρ iL ,ψ iL ,φ iL ),L=l-2

[0074] Where L represents the number of path nodes excluding the starting point and the end point, and l is the total number of path nodes; the subscripts i1, i2, and iL correspond to the first, second, and Lth path segments of particle i, respectively; the velocity of particle i in space is given by the following incremental vector ΔΩ i express:

[0075] ΔΩ i =(Δρ i1 ,Δψ i1 ,Δφ i1 ,Δρ i2 ,Δψ i2 ,Δφ i2 ,…,Δρ iL ,Δψ iL ,Δφ iL )

[0076] Define the spherical vector (ρ ij ,ψ ij ,φ ij ) is expressed as e ij , speed (Δρ ij ,Δψ ij ,Δφ ij ) is expressed as Δe ij .

[0077] For the multi-robot proposed in the present invention, each robot is corresponding to at least one particle population containing M particles, and the local optimal position Q of the corresponding particle is i It refers to the optimal solution found by a single particle i of each robot in its search history; the global optimal position Q g It is the optimal solution found by each robot in the corresponding particle swarm.

[0078] Step 3: Update the particle position based on the initially set fitness function and the current particle position. The update equation in this step does not involve game theory optimization and can be updated using the existing update formula. The specific update formula is not limited.

[0079] This embodiment uses the following update formula to update the location:

[0080]

[0081] Where, is the particle position e of the jth path segment of particle i at the kth and k+1th iterations ij ; is the velocity Δe of the jth path segment of particle i at the kth and k+1th iterations ij ;w k is the exploration weight w,r of the kth iteration 1j , r 2j are two random samples in the range [0,1] drawn from a uniform probability distribution. i =(q i1 ,q i2 ,...,q iL ) represents the local optimal position of particle i; Q g =(q g1 ,q g2 ,...,q gL ) represents the global optimal position of particle i; They represent the position parameters of the jth path segment in the global optimal position and local optimal position of particle i respectively; M is the total number of particles in a particle population.

[0082] It should be understood that in order to calculate the speed It is necessary to clarify the local optimal position and global optimal position of the current particle. Based on the particle swarm algorithm, it is known that the local optimal position and global optimal position of the particle are determined based on the fitness function. The present invention preferably converts the path planning problem into a multi-objective optimization problem and constructs a cost function with cost reduction as the optimization goal. This example uses the cost function constructed by safety constraint cost, altitude cost, path cost and smoothness cost as the optimal implementation method, which is specifically expressed as:

[0083]

[0084] In the formula, F(X i ) is the cost function value, b κ are weight factors, b1, b2, b3, b4 correspond to F1(X i ),F2(X i ),F3(X i ) and F4(X i ) weight factor; F1(X i ),F2(X i ),F3(X i ) and F4(X i ) correspond to the safety constraint cost, altitude cost, path cost, and smoothness cost respectively. And it should be understood that in order to calculate the cost function, it is necessary to map the three-dimensional space vector to the two-dimensional plane coordinate system. The path x of particle i is i The corresponding coordinate system in the two-dimensional plane is represented as X i .

[0085] Space vector e ij =(ρ ij ,ψ ij ,φ ij ) and plane waypoint P ij =(x ij ,y ij ,z ij ) are as follows:

[0086] x ij =x i,j-1 +ρ ij sinψ ij cosφ ij

[0087] y ij =y i,j-1 +ρ ij sinψ ij sinφ ij

[0088] z ij =z i,j-1 +ρ ij cosψ ij

[0089] Regarding the safety constraint cost F1(X i ):

[0090] For the path planning of the robot, the principle is to avoid collision to ensure safety. Let A be the set of all threats, each threat is assumed to be defined in a cylinder, and its projection has the center coordinate C a and radius D a ,like Figure 2 As shown in Figure 2, the diameter R of the collision zone is determined by the maximum size of the UAV and UGV, and the distance S of the danger zone is defined as the danger distance from the collision zone. For a given path segment The associated threat cost is the distance from the drone or unmanned vehicle to the center coordinate C k The distance d a Considering the diameter R of the collision area and the dangerous distance S, for the ath fault in the fault set O, the path segment Security constraint cost It is expressed as follows:

[0091]

[0092] Thus, the decision variable X i The safety constraint cost F1(X i ) is expressed as:

[0093]

[0094] In the formula, the decision variable X i Contains n waypoints P ij .

[0095] Regarding the altitude cost F2(X i ):

[0096] The flight altitude of a drone is usually limited between two given extremes, namely the minimum altitude and the maximum altitude. The purpose of introducing altitude constraints is to avoid obstacles, meet specific mission requirements, and improve the effectiveness of path planning in different environments. For example, in measurement and search applications, the camera is required to collect visual data with a specific resolution and field of view, which limits the flight altitude. Let the minimum altitude and maximum altitude be h respectively. min and h max Waypoint p ij The altitude cost H ij To maintain the average height and penalize out-of-range values, there is:

[0097]

[0098] Among them, h ij Indicates the flight altitude of the drone relative to the ground, such as Figure 3 As shown. The altitude cost H of all waypoints ij Add up to get X i The altitude cost F2(X i :

[0099]

[0100] Regarding the path cost F3(X i ):

[0101] The Euclidean distance between two nodes is expressed as The path cost F3 related to the path length can be calculated as:

[0102]

[0103] About the smoothing cost F4(X i ):

[0104] The smoothness cost evaluates the turning rate and climbing rate, which are the key to generating a feasible path. Figure 4 As shown, the steering angle φ ij are two consecutive path segments and The angle between them is projected onto the horizontal plane Oxy. is the unit vector in the z-axis direction, the path segment The projection vector It can be calculated as:

[0105]

[0106] Therefore, the steering angle φ ij The calculation is:

[0107]

[0108] Elevation angle ψ ij Is a path segment Its projection on the horizontal plane The angle between them exists:

[0109]

[0110] The smoothing cost is calculated as follows:

[0111]

[0112] Where a1 and a2 are the turning and climbing angles respectively.

[0113] Step 4: Determine whether there is a collision between multiple robots. If so, go to step 5, otherwise go to step 6.

[0114] Step 5: Adopt a spatial position transformation strategy based on game theory, update the particle position, and then execute step 6.

[0115] The technical solution of this invention utilizes game theory to solve the multi-robot collaborative path planning problem, treating each robot as a participant in the game. The game strategy defines the possible paths or behavioral states for each participant, while the game payoff is determined by factors such as multi-objective optimization and behavioral costs. Participants select paths by establishing a payoff function, thereby obtaining corresponding payoffs.

[0116] Definition: Game theory transforms and solves the path planning problem as follows:

[0117] G={S1,S2,...,S N ; U1, U2, ..., U N}

[0118] Among them, S1, S2, ..., S N represents the strategies of the 1st, 2nd, ..., Nth participants (robots), U1, U2, ..., U N represents the total cost obtained by the 1st, 2nd, ..., Nth participants (robots) adopting this strategy, U n represents the total cost obtained by the nth participant adopting this strategy. N is the total number of robots, and n ranges from 1 to N.

[0119] The strategy space S of the nth participant n It consists of the path variables of each robot, the path variable e nη Denote the η decision variables in the strategy space of the nth participant as follows:

[0120] S1={e 11 ,e 12 ,...,e 1η},...,S n ={e n1 ,e n2 ,...,e nη}

[0121] Where m represents a path point or a waypoint. The avoidance cost v of p participants in the game is p The influence of its own strategy choice and the strategy choice of q participants is as follows:

[0122]

[0123] Where, represents the absolute collision avoidance benefit obtained by participant p after adopting the collision avoidance strategy, represents the benefit that participant p obtains due to the influence of participant q’s strategic behavior. In addition, ω is the weight, ω pp ≠0,ω pq ≠0.

[0124] Based on the above theoretical statements, this embodiment uses the particle spatial position transformation strategy of game theory to optimize the particle position equation and fitness function, and then updates the particle position again.

[0125] Define velocity Δe ij With strategy Δs ij The corresponding relationship is as follows:

[0126] Δe ij ←Δs ij =(Δρ′ ij ,Δψ′ ij ,Δφ′ ij )

[0127] Where Δρ′ ij represents the distance increment, Δψ′ ij Indicates the climb angle increment, Δφ′ ij Represents the rotation angle increment. Its definition is as follows:

[0128]

[0129] Here, rand() represents a function that generates random floating-point numbers in the interval [0,1]. It is a preset value. The superscript plus and minus indicate the positive and negative of the value, and also represent the direction such as up, down, left and right.

[0130] The robots participating in the game have different random increments in distance, climbing angle, and turning angle, which means that the choice of positive and negative signs is different. The current position change of the robot is not only related to its own actions, but also to the game strategy of the opponent. Therefore, when the p robot plays against the q robot, the strategy Δs of the p robot is ij The total profit v caused by the position change pq The calculation is as follows:

[0131]

[0132] in, It represents the absolute profit when robot p plays against robot q.

[0133] Since the robot maintains the original planned path, the obstacle avoidance benefit of each robot is v pq It helps to reduce the total planning cost, thus helping to achieve Nash equilibrium. Add to the total cost function of the game middle:

[0134]

[0135] According to each robot, the corresponding strategy Δs ijAdjust particle position e ij Combining game theory, the position update equation of the improved particle swarm optimization algorithm is as follows:

[0136]

[0137] After the game ends, the global best position of each robot and the local optimal position is updated as follows:

[0138]

[0139] Among them, ξ represents the mapping of the vector set from the Ω space to the path points on the X plane, represents the position of the robot particle p Total cost Smaller than the previous position Total cost Update the local optimal position of particle p to position Otherwise, the local optimal position is still Indicates the location with the lowest total cost is the global best position.

[0140] Step 6: Determine whether all particles have completed the search. If so, go to step 7, otherwise return to step 3.

[0141] Step 7: Check whether the Nash equilibrium is reached. If so, go to step 8; otherwise, return to step 3.

[0142] In the context of multi-robot global path planning, a Nash equilibrium means that if all other robots maintain the same strategy, no single robot can improve its outcome by unilaterally changing its strategy. When multiple robots perform global path planning, each robot reaches a consensus on the global goal. Collision conflicts are resolved through cooperative strategies, ensuring path safety and achieving a Nash equilibrium. Therefore, when the robots reach a Nash equilibrium through game play, a stable solution has been found. However, this solution is not necessarily globally optimal. Therefore, the present invention identifies and calculates all possible Nash equilibria. Within the identified equilibrium, strategy combinations are analyzed to identify scenarios where at least one participant can achieve a higher payoff without reducing the payoffs of other participants. If such a combination does not exist, the current Nash equilibrium is considered optimal. That is, if there exists a strategy combination where at least one participant can achieve a higher payoff without reducing the payoffs of others, the current Nash equilibrium is suboptimal because there is room for improvement. If such a combination does not exist, it means that all participants have achieved the best response given the strategies of the other participants. Therefore, the Nash equilibrium is considered optimal because no participant can improve their outcome by changing their strategy. Simply put, an optimal solution means that, given the current strategy combination, no participant can achieve a better outcome individually.

[0143] Due to the stability of the Nash equilibrium solution, once it is reached, each participant will not change their strategy in order to gain more. Therefore, Nash equilibrium provides a way for all participants to obtain the best results. After reaching Nash equilibrium, the overall game strategy of the multi-robot is optimal and the cost function is minimized. Nash equilibrium is defined as It meets the following conditions:

[0144] Nash equilibrium is defined as a set of strategy combinations Among them S n Represents the strategy of the nth robot. This strategy combination satisfies the following conditions: For each robot n, its strategy Given the strategies of all other robots When the total cost is U n Minimize. Here the utility function U n represents the total cost function of the game for the nth robot, which measures the benefits or costs of the robot under a given strategy.

[0145]

[0146] For the robot's strategy S n Any strategy space e in nσ , given other robots Strategy space When (here σ is an index that traverses all possible strategies of robot n), the total game cost function U of the robot n The value in Nash equilibrium will not be higher than its value in Nash equilibrium strategy This means that in the Nash equilibrium state, no robot can obtain higher utility by unilaterally changing its strategy.

[0147] in is the optimal strategy set of the nth robot competitor. Therefore, the Nash equilibrium is as follows:

[0148]

[0149] Describes how to find the Nash equilibrium strategy for each robot That is, by taking all possible strategies e n and other competitors Find a n Here, argmin means to select a strategy that minimizes the function value among all possible strategies.

[0150] It should be understood that after the current iteration is completed, each robot population corresponds to a global optimal position and a local optimal position. The present invention preferably determines whether each robot's global optimal position satisfies a Nash equilibrium. In other feasible embodiments, depending on actual accuracy requirements, the determination range can be relaxed. Specifically, particles that meet corresponding criteria can be set to determine the robot's optional strategies, from which strategies can be screened to determine whether a strategy satisfies a Nash equilibrium. The present invention does not specifically limit this.

[0151] Step 8: Determine whether the number of iterations has been reached; if so, go to step 9, otherwise return to step 3.

[0152] Step 9: Output the collision-free path planned by multi-robot collaboration

[0153] The present invention evaluates the performance of the algorithm and verifies its effectiveness through three different simulation experiments. In order to ensure that the simulation environment accurately reflects the real-world scenario, the characteristics and constraints of the actual environment are taken into account in the design of the simulation model. The simulation environment replicates the key elements of the real-world setting, including terrain, obstacles and dynamic constraints faced by UGVs and UAVs. The experiments cover complex urban and mountainous environments. Through experiments, the multi-robot collaborative path planning algorithm (GTPHM) proposed in this paper is compared with the improved particle swarm optimization algorithm (IPSO), the multi-robot path planning based on the dragonfly algorithm (I-DA) and the particle swarm multi-robot path planning algorithm improved by the sine-cosine algorithm (IPSO-SCA). To ensure the accuracy and reliability of the results, each group of experiments is repeated 30 times, and the average value of each evaluation index is calculated. The evaluation indicators include the number of iterations of the algorithm, the running time, the path length of each robot, and the number of conflicts.

[0154] This set of experiments evaluated collaborative path planning for a heterogeneous multi-robot system consisting of two drones and two UGVs using four different algorithms. The IPSO algorithm suffered from collision issues due to the lack of a collision prevention mechanism. However, the GTPHM algorithm maintained a safe distance between the UGVs and drones, adhered to motion constraints, and enhanced safety. While the I-DA and IPSO-SCA algorithms also produced safe paths, the GTPHM algorithm produced a shorter path compared to IPSO-SCA. Furthermore, GTPHM and I-DA performed well in terms of path smoothness, while the IPSO-SCA algorithm showed fluctuations between UAV1 and UAV2 that affected stability. Therefore, the GTPHM strategy was superior in terms of path safety and stability.

[0155] Example 2:

[0156] This embodiment provides a multi-robot system based on the above-mentioned path planning method, including several communication-connected robots and a collaborative control subsystem, wherein the robots are all connected to the collaborative control subsystem, and the collaborative control subsystem includes: a map information acquisition module and a particle optimization planning module.

[0157] The map information acquisition module is used to acquire map information of the area to be planned, wherein the map information at least includes obstacle information;

[0158] The particle optimization planning module is used to introduce a spherical vector particle swarm algorithm and optimize it based on the particle spatial position transformation strategy of game theory. The particle position is then used to represent the robot path. A fitness function representing the cost is set. The spherical vector particle swarm algorithm is used to iteratively update the particle position corresponding to each robot until the iteration termination condition is met. The optimal particle position corresponding to each robot is obtained, and then a collision-free path for multi-robot collaborative planning is output.

[0159] The process of each iterative operation is as follows: first, the particle position is updated based on the initially set fitness function and the current particle position, and then based on the updated particle position, it is determined whether there is a collision conflict between the multiple robots. If there is a collision conflict, the particle position equation and the fitness function are optimized using the particle space position transformation strategy of game theory, and then the particle position is updated again; then the next iterative operation is entered; if there is no collision conflict, the next round of iterative operation is entered based on the currently updated particle position.

[0160] It should be understood that in some embodiments, the robots are multiple unmanned ground vehicles (UGVs) and multiple unmanned aerial vehicles (UAVs); in some embodiments, they can also be set as other types of robots.

[0161] It should also be understood that the specific implementation process of each module please refer to the above method content, the present invention will not go into details here, and the division of the above functional modules is only for example illustration. In some embodiments, some functional modules can be merged, and some functional modules can be split. Each functional module can be implemented in software or hardware or a combination of software and hardware. Among them, software and hardware equipment include but are not limited to general-purpose computer equipment, programmable gate arrays, digital signal processors, microprocessors and their corresponding programming or burning software.

[0162] Example 3:

[0163] An embodiment of the present invention further provides a computer device, comprising at least: one or more processors; and a memory storing one or more computer programs;

[0164] Among them, the processor calls the computer program to implement: the steps of a multi-robot collaborative three-dimensional path planning method.

[0165] For the specific implementation process of each step, please refer to the description of the above method.

[0166] It should be understood that in the embodiments of the present invention, the processor referred to may be a central processing unit (CPU), and the processor may also be other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field-programmable gate arrays (FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. The general-purpose processor may be a microprocessor or the processor may also be any conventional processor, etc. The memory may include a read-only memory and a random access memory, and provides instructions and data to the processor. A portion of the memory may also include a non-volatile random access memory. For example, the memory may also store information about the device type.

[0167] Example 4:

[0168] An embodiment of the present invention also provides a computer-readable storage medium storing a computer program, wherein the computer program is called by a processor to implement: steps of a multi-robot collaborative three-dimensional path planning method.

[0169] For the specific implementation process of each step, please refer to the description of the above method.

[0170] The readable storage medium is a computer-readable storage medium, which can be an internal storage unit of the software and hardware device described in any of the aforementioned embodiments, such as a hard disk or memory of a controller. The readable storage medium can also be an external storage device of the controller, such as a plug-in hard disk equipped on the controller, a smart memory card (Smart Media Card, SMC), a secure digital (Secure Digital, SD) card, a flash card (Flash Card), etc. Furthermore, the readable storage medium can also include both an internal storage unit of the controller and an external storage device. The readable storage medium is used to store the computer program and other programs and data required by the controller. The readable storage medium can also be used to temporarily store data that has been output or is to be output.

[0171] Based on this understanding, the technical solution of the present invention, or the portion that contributes to the prior art, or all or part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes instructions for causing a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the method described in each embodiment of the present invention. The aforementioned readable storage medium includes various media that can store program code, such as a USB flash drive, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk, or an optical disk.

[0172] Those skilled in the art will appreciate that the embodiments of the present application can be provided as methods, systems, or computer program products. Therefore, the present application can adopt the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Moreover, the present application can adopt the form of a computer program product implemented on one or more computer-readable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code. The present application is a flow chart according to the method, device (system), and computer program product of the embodiment of the present application and / or the instructions executed by the processor to generate a device for realizing the function specified in one flow chart or multiple flows and / or one box or multiple boxes of the block diagram. These computer program instructions can also be stored in a computer-readable memory that can guide a computer or other programmable data processing device to work in a specific manner, so that the instructions stored in the computer-readable memory generate a product comprising an instruction device, which realizes the function specified in one flow chart or multiple flows and / or one box or multiple boxes of the block diagram. These computer program instructions may also be loaded onto a computer or other programmable data processing device so that a series of operational steps are executed on the computer or other programmable device to produce a computer-implemented process, whereby the instructions executed on the computer or other programmable device provide steps for implementing the functions specified in one or more processes in the flowchart and / or one or more boxes in the block diagram.

[0173] It should be emphasized that the examples described in the present invention are illustrative rather than restrictive. Therefore, the present invention is not limited to the examples described in the specific embodiments. Any other embodiments derived by those skilled in the art based on the technical solution of the present invention that do not depart from the purpose and scope of the present invention, whether modified or replaced, also fall within the scope of protection of the present invention.

Claims

1. A multi-robot collaborative three-dimensional path planning method, characterized by: The following steps are involved: S1: Obtain map information of the area to be planned; S2: Introduce the spherical vector particle swarm algorithm and optimize it based on the particle space position transformation strategy of game theory; then set a corresponding particle population for each robot, and the particle position of each particle in the particle population represents a path of the corresponding robot. The path planning problem is converted into a multi-objective optimization problem, and cost reduction is taken as the optimization goal. A fitness function representing the cost is constructed, and the spherical vector particle swarm algorithm is used to iteratively update the particle position corresponding to each robot until the iteration termination condition is met. The optimal particle position corresponding to each robot is obtained, and then a collision-free path for multi-robot collaborative planning is output; The robot path is split into path segments constructed by path nodes, and then the path vector is constructed using the amplitude, elevation angle, and azimuth angle of the path segment to represent the particle position; The process of each iterative operation is as follows: first, the particle position is updated based on the initially set fitness function and particle position equation as well as the current particle position; then, based on the updated particle position, it is determined whether there is a collision conflict between the multiple robots; if there is a collision conflict, the particle space position transformation strategy of game theory is used to optimize the fitness function and particle position equation, and then the particle position is updated again; then the next iterative operation is entered; if there is no collision conflict, the next round of iterative operation is entered based on the currently updated particle position.

2. The method according to claim 1, wherein: Define the particle position Ω of particle i representing the robot i and the incremental vector ΔΩ representing the spatial velocity i Expressed as: Oh i =(ρ i1 ,ψ i1 ,f i1 ,r i2 ,ψ i2 ,f i2 ,...,r iL ,ψ iL ,f iL ),L=l-2 DO i =(Dr. i1 ,Dp i1 ,Df i1 ,Dr. i2 ,Dp i2 ,Df i2 ,...,Dr. iL ,Dp iL ,Df iL ) Where L represents the number of path nodes excluding the starting point and the end point, l is the total number of path nodes; amplitude ρ∈(0, path length), elevation angle and azimuth angle φ∈[-π,π]; Δρ, Δψ, Δφ correspond to the amplitude increment, elevation increment, and azimuth increment, respectively; subscripts i1, i2, iL correspond to the 1st, 2nd, and Lth path segments of particle i, respectively; Define the position corresponding to the jth path segment of particle i (ρ ij ,ψ ij ,φ ij ) is expressed as e ij , speed (Δρ ij ,Δψ ij ,Δφ ij ) is expressed as Δe ij For a group of M particles, the particle position equation is optimized using the particle space position transformation strategy of game theory, and the updated particle position is expressed as: Where, They represent the strategies Δs of the j-th path segment of particle i at the k+1-th iteration and the k-th iteration respectively. ij , strategy Δs ij By speed Δe ij Converted from; w k is the exploration weight w of the kth iteration, c1 and c2 are both learning factors, are the position parameters of the jth path segment in the global optimal position and local optimal position of particle i in the kth iteration; r 1j , r 2j are two random samples in [0,1] from a uniform probability distribution; is the particle position e of the j-th path segment of particle i at the k-th and k+1-th iterations ij .

3. The method according to claim 1, wherein: When the particle spatial position transformation strategy of game theory is used to optimize the fitness function, the fitness function is the total game function or is converted from the total game function. The total game function introduces the obstacle avoidance benefits generated by the strategy when the robot plays the game on the basis of the cost function; Define amplitude ρ∈(0, path length), elevation angle and azimuth angle φ∈[-π,π]; Δρ, Δψ, Δφ correspond to the amplitude increment, elevation increment, and azimuth increment, respectively; the particle position (ρ ij ,ψ ij ,φ ij ) is expressed as e ij , speed (Δρ ij ,Δψ ij ,Δφ ij ) is expressed as Δe ij , the strategy Δs of particle i on the jth path segment ij By speed Δe ij converted; When the p robot plays against the q robot, the strategy Δs of the p robot ij The obstacle avoidance benefit v caused by the position change pq as follows: The total collision payoff V of particle i of robot p pi Expressed as: Where Δρ′ ij , Δψ′ ij , Δφ′ ij is the strategy Δs ij Elements, corresponding to amplitude, elevation, azimuth, strategy Δs ij By speed Δe ij Converted from; N represents the number of robots involved in path planning, rand() is a random number generator that generates a random floating point number between 0 and 1, It represents the benefit obtained by participant p due to the influence of participant q’s strategic behavior, and k represents the robot number.

4. The method according to claim 2 or 3, characterized in that: The strategy Δs ij With the speed Δe ij The relationship is as follows: Where rand() is a random number generator that generates a random floating-point number between 0 and 1. The superscripts + and - represent two different adjustment directions or changes. That is, in the event of a collision, + and - are used to indicate two possible ways of adjusting the variable. + may indicate an increase, while - indicates a decrease. The value of Δρ′ is preset, ij , Δψ′ ij , Δφ′ ij is the strategy Δs ij The elements correspond to the amplitude increment, elevation increment, and azimuth increment, and R is the diameter of the defined collision area.

5. The method according to claim 3, wherein: The cost type of the cost function is a complete combination or a partial combination of a safety constraint cost, an altitude cost, a path cost, and a smoothing cost.

6. The method according to claim 1, wherein: During each iteration, the termination conditions also include: determining whether all robots have reached a Nash equilibrium state. If not, proceed to the next round of iteration. If so, determine whether other termination conditions are met. If not, proceed to the next round of iteration. in, is the optimal strategy set of all competitors of the nth robot, is the optimal strategy set of the 1st, n-1th, n+1th, and Nth competitors, and the Nash equilibrium state is expressed as: Where, is the optimal strategy of the nth robot, U n Represents the total cost function of the game for the nth robot.

7. The method according to claim 1, wherein: The global optimal position of each robot in the spherical vector particle swarm algorithm and the local optimal position Defined as: Where ξ represents the mapping of the vector set from the Ω space to the path points on the X plane; U is the fitness function, representing the cost; represents the position of the robot particle p Total cost Smaller than the previous position Total cost Update the local optimal position of particle p to position Otherwise, the local optimal position is still Indicates the location with the lowest total cost is the global best position.

8. A multi-robot system based on the method according to any one of claims 1 to 7, characterized in that: It includes several communication-connected robots and a collaborative control subsystem, wherein each of the robots is connected to the collaborative control subsystem, and the collaborative control subsystem includes: A map information acquisition module, configured to acquire map information of the area to be planned, wherein the map information includes at least obstacle information; The particle optimization planning module is used to introduce a spherical vector particle swarm algorithm and optimize the spherical vector particle swarm algorithm based on the particle space position transformation strategy of game theory. A corresponding particle population is then set for each robot. The particle position of each particle in the particle population represents a path for the corresponding robot. The path planning problem is converted into a multi-objective optimization problem. With cost reduction as the optimization goal, a fitness function representing the cost is constructed. The spherical vector particle swarm algorithm is used to iteratively update the particle position corresponding to each robot until the iteration termination condition is met. The optimal particle position corresponding to each robot is obtained, and then a collision-free path for multi-robot collaborative planning is output. The robot path is split into path segments constructed by path nodes, and then the path vector is constructed using the amplitude, elevation angle, and azimuth angle of the path segment to represent the particle position; The process of each iterative operation is as follows: first, the particle position is updated based on the initially set fitness function and particle position equation as well as the current particle position; then, based on the updated particle position, it is determined whether there is a collision conflict between the multiple robots; if there is a collision conflict, the particle space position transformation strategy of game theory is used to optimize the fitness function and particle position equation, and then the particle position is updated again; then the next iterative operation is entered; if there is no collision conflict, the next round of iterative operation is entered based on the currently updated particle position.

9. A computer device, characterized in that: At least: one or more processors; a memory storing one or more computer programs; The processor calls the computer program to implement: The steps of the method according to any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that: A computer program is stored, which is called by a processor to implement: The steps of the method according to any one of claims 1 to 7.

Images