Multi-robot collaborative three-dimensional path planning method and system

Abstract

The invention discloses a multi-robot collaborative three-dimensional path planning method and system, and the method comprises the steps: splitting a robot path into path segments constructed by path nodes, and constructing a path vector through the amplitude, elevation angle and azimuth angle of the path segments, and enabling the path vector to represent the position of a particle; introducing a spherical vector particle swarm algorithm, and optimizing the spherical vector particle swarm algorithm based on a particle space position transformation strategy of a game theory; wherein a fitness function representing cost is set, the spherical vector particle swarm algorithm is used for carrying out iterative operation to update the particle position corresponding to each robot until an iteration termination condition is met, the optimal particle position corresponding to each robot is obtained, and then a collision-free path of multi-robot collaborative planning is output. By means of the method, the relation between the robots can be better processed, the safety and feasibility of path planning in the shared environment can be enhanced, and the flexibility and adaptability of a multi-robot system are kept.

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 implemented by integrating game theory and particle swarm hybrid optimization method. Background Art

[0002] In recent years, game theory has been introduced into the problem of multi-robot cooperation. Wu designed a new distributed multi-agent task allocation framework based on the potential game method 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 between multi-agents in a two-stage path planning algorithm, and applied the virtual action method to solve the game problems in conflict points and multiple Nash equilibrium selections, thereby reducing the total path cost and improving the convergence speed of the algorithm. However, the difficulty of considering all possible scenarios limits the success rate of reaching 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 realize collaborative path planning for multi-agent systems. The cooperative game framework extends the traditional single-agent Q-learning algorithm to multi-agent systems. Although the framework may perform well in specific scenarios, its generalization ability is limited. In addition, the frequent reliance on communication between robots may cause communication delays, which will affect the overall performance and response speed of the system.

[0003] In addition, collision avoidance, risk reduction, and optimal path identification are crucial when planning robot paths in complex environments. Traditional grid-based and sampling-based methods often have difficulties in scalability and efficiency in dynamic, high-dimensional, or constrained spaces. On the contrary, intelligent optimization algorithms such as gray wolf optimization, differential evolution, genetic algorithms, particle swarm optimization, and ant colony optimization have received widespread attention. These algorithms use nature-inspired strategies to enhance global search capabilities and robustness for complex pathfinding tasks. Compared with other nature-inspired algorithms, PSO is good at finding stable global solutions in a shorter computing time. However, the PSO algorithm is insensitive to changes in initial conditions and objective functions. It mainly relies on a set of small parameters, including an acceleration coefficient and two weight factors, to adapt to various environmental configurations. Subsequently, researchers have proposed several variants, such as classical PSO, phase-encoded PSO, quantum-behaved PSO (QPSO), and discrete PSO (DPSO). These variants have the same population-based structure, but differ in the way they represent the search space and the solutions encoded in the particles. Phung proposed a spherical vector-based particle swarm optimization (SPSO) algorithm for trajectory planning of drones in complex environments. SPSO expands the solution space of the algorithm, enabling it to adapt to diverse complex environments and dynamic constraints, thereby improving the performance of the algorithm. 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 a safe and high-quality robot path, and further provide a multi-robot collaborative three-dimensional path planning method and system, wherein the technical solution of the present invention is a path planning implemented by integrating game theory and particle swarm hybrid optimization method. The particle swarm optimization algorithm is improved by integrating the game theory method, that is, the game theory strategy is introduced to adjust the spatial position of particles, aiming 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, so as to better handle the relationship between robots, so that it can 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] On the one hand, 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 the spherical vector particle swarm algorithm based on the particle space position transformation strategy of game theory; then set a corresponding particle population for each robot, the particle position of each particle in the particle population represents a path of the corresponding robot, convert the path planning problem into a multi-objective optimization problem, take cost reduction as the optimization goal, construct a fitness function representing the cost, use the spherical vector particle swarm algorithm to iteratively update the particle position corresponding to each robot until the iteration termination condition is met, and obtain the optimal particle position corresponding to each robot, and then output the collision-free path of multi-robot collaborative planning;

[0010] Among them, 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 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 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 generally defined. When the distance between two robots is less than this preset safety distance, the system will determine that there is a collision threat. This method determines whether there is a collision risk by monitoring the relative distance between robots.

[0013] Optionally, define the particle position Ω of particle i representing the robot i and the incremental vector ΔΩ representing the spatial velocity i It is 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 φ∈[-π,π]; Δρ, Δψ, Δφ 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 represented by 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] In the formula, They represent the strategies Δs of the j-th path segment of particle i at the k+1th iteration and the kth iteration respectively. ij , strategy Δs ij From the 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 the particle space position transformation strategy of game theory is used to optimize the fitness function, 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 φ∈[-π,π]; Δρ′, Δψ′, Δφ′ are the amplitude increment, elevation increment, and azimuth increment, respectively; the particle position (ρ ij ,ψ ij ,φ ij ) is represented by e ij , speed (Δρ ij ,Δψ ij ,Δφ ij ) is expressed as Δe ij , the strategy Δs of particle i on the jth path segment ij From the speed Δe ij Converted from

[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 It is expressed as:

[0025]

[0026] In the formula, Δρ′ ij , Δψ′ ij , Δφ′ ij is the strategy Δs ij The elements correspond to amplitude, elevation, azimuth, strategy Δs ij From the speed Δe ij 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 the present invention preferably integrates the collision cost and multi-objective heuristic function into the payoff structure of game theory, thereby optimizing the path planning quality of a single robot under a collaborative framework. The technical solution of the present invention combines the advantages of the coupling and decoupling algorithms, effectively coordinates the local and global goals of the multi-robot system, thereby achieving Nash equilibrium, ensuring the global optimality of 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] In the formula, 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 represent two possible ways of adjusting the variable. + may represent an increase, while - represents 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 smoothness cost.

[0032] Optionally, in each iteration process, the iteration termination condition also includes: determining whether a Nash equilibrium state has been reached among all robots, if not, entering the next round of iteration operation; if reached, determining whether other iteration termination conditions have been met, if not, entering the next round of iteration operation;

[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] In the formula, 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 optimum Defined as:

[0037]

[0038] Among them, ξ 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 Less than the previous position Total cost Update the local best position of particle p to position Otherwise, the local optimal position is still 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 comprises 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 comprises:

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

[0041] The particle optimization planning module is used to introduce the spherical vector particle swarm algorithm and optimize the spherical vector particle swarm algorithm based on the particle space position transformation strategy of game theory; then set a corresponding particle population for each robot, the particle position of each particle in the particle population represents a path of the corresponding robot, convert the path planning problem into a multi-objective optimization problem, take cost reduction as the optimization goal, construct a fitness function representing the cost, use the spherical vector particle swarm algorithm to perform iterative calculations to update the particle position corresponding to each robot until the iteration termination condition is met, and obtain the optimal particle position corresponding to each robot, and then output the collision-free path of multi-robot collaborative planning;

[0042] Among them, 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 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 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 the present invention introduces game theory, and combines game theory with intelligent path planning methods to plan more specific and executable paths without complex communication overhead. The method allows multi-level optimization, where game theory can optimize the collaborative behavior of multi-robot systems at the global level and optimize the path planning of individual robots at the local level. Through this method, multi-robot path planning can better handle the relationship between robots and solve the problem of collaboration between robots, so that it can enhance the safety and feasibility of path planning in a shared environment, and maintain the flexibility and adaptability of the multi-robot system.

[0052] 2. The technical solution of the present 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 between 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] The present invention provides a multi-robot collaborative three-dimensional path planning method for planning a safe and high-quality multi-robot collision-free path. The collaborative three-dimensional path planning is realized by integrating game theory and particle swarm algorithm, a multi-robot path planning framework based on game theory is built, and collision cost and multi-objective heuristic function are used as game theory gain to maintain game theory interaction between robots; in an improved particle swarm optimization algorithm (PSO), a particle space position transformation method is designed according to a three-dimensional space vector as a strategy update mechanism based on game theory; and in order to avoid collisions between robots, each robot adjusts its cooperation strategy according to the behavior of other robots. Each robot selects the optimal cooperation strategy and then gradually approaches the 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: Introduce the spherical vector particle swarm algorithm, and optimize the spherical vector particle swarm algorithm based on the particle space position transformation strategy of game theory; then characterize the robot path with the particle position, set the fitness function that characterizes the cost, and use the spherical vector particle swarm algorithm to iteratively update the particle position corresponding to each robot until the iteration termination condition is met, and obtain the optimal particle position corresponding to each robot, and then output the collision-free path of multi-robot collaborative planning;

[0061] Among them, the process of each iterative operation is: first update the particle position based on the initially set fitness function and the current particle position, then judge whether there is a collision conflict between the multiple robots based on the updated particle position, if there is a collision conflict, use the particle space position transformation strategy of game theory to optimize the particle position equation and the fitness function, and then update the particle position again; then enter the next iterative operation; if there is no collision conflict, enter the next round of iterative operation based on the currently updated particle position.

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

[0063] Embodiment 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 refers to the collaborative planning of individual motion paths by multiple robots in a complex environment to achieve mission objectives. Among them, the path selection is optimized to ensure safe obstacle avoidance, prevent mutual collisions, and improve the overall smooth operation efficiency of the system.

[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 UGV / UAV collaborative control subsystems, which are used to solve 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. Among them, in order to better describe the working environment, a three-dimensional map is used to simulate the known spatial information and the location 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 at least contains obstacle information, which is used to avoid collision with obstacles in the future. The cost function will be described in detail below. Parameter initialization includes: parameter learning factors c1, c2, exploration weight w, constraint weights b1, b2, b3, b4, b5; and setting the starting point, end point, population size, number of iterations, etc. Since they are all conventional settings, the present invention will not elaborate on them in detail.

[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 best position Q g , local optimal position Q i .

[0070] In the particle swarm algorithm, due to the insufficient ability to handle environmental complexity 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 φ∈[-π,π]. The motion constraints of the UAV are realized through spatial angle constraints, and the particle search space is optimized.

[0071] Example:

[0072] For the robot, the path x i Represents a list of l waypoints that the robot needs to pass. 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, 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, l is the total number of path nodes; the subscripts i1, i2, iL correspond to the 1st, 2nd, and Lth path segments of particle i, respectively; the velocity of particle i in space is given by the following increment vector ΔΩ i express:

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

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

[0077] For the multi-robot proposed in the present invention, each robot corresponds 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 its corresponding entire 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 the original update formula can be used for update, and the specific update formula is not limited.

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

[0080]

[0081] In the formula, is the particle position e of the jth path segment of particle i at the kth, k+1th iteration 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 [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 certain 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 can be 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 κ is the weight factor, b1, b2, b3, b4 corresponds 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. 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 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 ) is 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 robot's path planning, 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 The 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] About 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. Set the altitude cost H of all waypoints ij Add together 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, 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 the spatial position transformation strategy based on game theory, update the particle position, and then execute step 6.

[0115] When the technical solution of the present invention uses game theory to solve the multi-robot collaborative path planning problem, each robot is regarded as a participant in the game. The game strategy defines the optional path or behavior state of each participant, and the game benefit is determined by factors such as multi-objective optimization and behavior cost. The participants select the path by establishing a benefit function, thereby obtaining corresponding benefits.

[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 It represents the total cost obtained by the nth participant adopting this strategy. N is the total number of robots, and the value of 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η represents the n 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 p The influence of its own strategy selection and the strategy selection of q participants is as follows:

[0122]

[0123] In the formula, represents the absolute collision avoidance benefit obtained by participant p after adopting the collision avoidance strategy, represents the benefit obtained by participant p 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 space position transformation strategy of game theory to optimize the particle position equation and the fitness function, and then updates the particle position again.

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

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

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

[0128]

[0129] Among them, rand() represents a function that generates random floating-point numbers in the interval [0,1]. It is the preset value. The superscript plus and minus indicate the positive or negative 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, Represents the absolute profit of robot p against robot q.

[0133] Since the robot maintains the original planned path, the obstacle avoidance benefit v of each robot pq It helps to reduce the total planning cost, thus facilitating the realization of 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 ij . Combining game theory, the position update equation of the improved particle swarm optimization algorithm is as follows:

[0136]

[0137] After the game is over, the global best position of each robot and the local optimum 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 Less than the previous position Total cost Update the local best position of particle p to position Otherwise, the local optimal position is still The location with the lowest total cost is the global best position.

[0140] Step 6: Determine whether all particles have completed the search. If yes, 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 case of multi-robot global path planning, Nash equilibrium means that if all other robots keep the same strategy, no robot can improve its results by unilaterally changing its own strategy. When multiple robots perform global path planning, each robot reaches a consensus on the global goal. Collision conflicts are resolved through cooperative strategies to ensure path safety and achieve Nash equilibrium. Therefore, when the robot reaches Nash equilibrium through game, it means that a stable solution has been found. However, the solution is not necessarily the global optimal. Therefore, the present invention identifies and calculates all possible Nash equilibria. In the determined equilibrium, the strategy combination is analyzed to determine the situation where at least one participant can obtain higher benefits without reducing the benefits of other participants. When such a combination does not exist, the current Nash equilibrium is considered to be the optimal solution. That is, if there is a strategy combination in which at least one participant can obtain higher benefits without reducing the benefits of others, then the current Nash equilibrium is not optimal because there is room for improvement. When such a combination does not exist, it means that all participants have reached the best response given the strategies of other participants, so this Nash equilibrium is considered to be the optimal solution because no participant can improve their results by changing their strategies. In short, the optimal solution means that under the current strategy combination, no participant can do better alone.

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

[0144] Nash equilibrium is defined as a set of strategy combinations Where 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 benefit or cost 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 The 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 the Nash equilibrium will not be higher than its value in the 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 choosing among 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 the global optimal position of each robot satisfies Nash equilibrium. In other feasible embodiments, according to the actual accuracy requirements, the judgment range can be relaxed, that is, setting particles that meet the corresponding indicators to determine the optional strategies of the robot, and screening whether there are strategies that meet Nash equilibrium. The present invention does not make specific limitations on 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 scene, the characteristics and constraints of the actual environment are considered 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 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 evaluates collaborative path planning for a heterogeneous multi-robot system consisting of two UAVs and two UGVs using four different algorithms. The IPSO algorithm suffers from collision issues due to the lack of a collision prevention mechanism. However, the GTPHM algorithm maintains a safe distance between the UGVs and the UAVs, adheres to motion constraints, and enhances safety. While the I-DA and IPSO-SCA algorithms also produce safe paths, the GTPHM algorithm produces a shorter path compared to IPSO-SCA. In addition, GTPHM and I-DA excel in path smoothness, while the IPSO-SCA algorithm shows fluctuations between UAV1 and UAV2 that affect stability. Therefore, the GTPHM strategy is superior in terms of path safety and stability.

[0155] Embodiment 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 the spherical vector particle swarm algorithm based on the particle space position transformation strategy of game theory; then the particle position represents the robot path, 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 the iteration termination condition is met, thereby obtaining the optimal particle position corresponding to each robot, and then outputting a collision-free path for multi-robot collaborative planning.

[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 be repeated 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, the hardware and software equipment includes but is not limited to general-purpose computer equipment, programmable gate arrays, digital signal processors, microprocessors and their corresponding programming or burning software.

[0162] Embodiment 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 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 gates or transistor logic devices, discrete hardware components, etc. A 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 provide 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 type of device.

[0167] Embodiment 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 may 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 may also be an external storage device of the controller, such as a plug-in hard disk, a smart memory card (Smart Media Card, SMC), a secure digital (Secure Digital, SD) card, a flash card (Flash Card), etc. equipped on the controller. Furthermore, the readable storage medium may 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 may 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, in essence or in other words, the part that contributes to the prior art, or all or part of the technical solution can be embodied in the form of a software product, which is stored in a storage medium and includes several instructions for enabling a computer device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the method described in each embodiment of the present invention. The aforementioned readable storage medium includes: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), disk or optical disk, etc., various media that can store program codes.

[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 codes. The present application is a device for realizing the function specified in one flow or multiple flows of the flow chart and / or one or multiple blocks of the block diagram with reference to the instructions executed by the processor according to the method, device (system) and computer program product of the embodiment of the present application. 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 produce a product including an instruction device, which realizes the function specified in one flow or multiple flows of the flow chart and / or one or multiple blocks 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, and 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 in that: 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 the spherical vector particle swarm algorithm based on the particle space position transformation strategy of game theory; then set a corresponding particle population for each robot, the particle position of each particle in the particle population represents a path of the corresponding robot, convert the path planning problem into a multi-objective optimization problem, take cost reduction as the optimization goal, construct a fitness function representing the cost, use the spherical vector particle swarm algorithm to iteratively update the particle position corresponding to each robot until the iteration termination condition is met, and obtain the optimal particle position corresponding to each robot, and then output the collision-free path of multi-robot collaborative planning; Among them, 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 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 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, characterized in that: Define the particle position Ω of particle i representing the robot i and the incremental vector ΔΩ representing the spatial velocity i It is 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 φ∈[-π,π]; Δρ, Δψ, Δφ 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 represented by 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: In the formula, They represent the strategies Δs of the j-th path segment of particle i at the k+1th iteration and the kth iteration respectively. ij , strategy Δs ij From the 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, characterized in that: When the particle space 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, 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; Define amplitude ρ∈(0, path length), elevation angle and azimuth φ∈[-π,π]; Δρ, Δψ, Δφ correspond to the amplitude increment, elevation increment, and azimuth increment, respectively; the particle position (ρ ij ,ψ ij ,φ ij ) is represented by e ij , speed (Δρ ij ,Δψ ij ,Δφ ij ) is expressed as Δe ij , the strategy Δs of particle i on the jth path segment ij From the speed Δe ij Converted from 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 It is expressed as: In the formula, Δρ′ ij , Δψ′ ij , Δφ′ ij is the strategy Δs ij The elements of the corresponding amplitude, elevation, azimuth, strategy Δs ij From the speed Δe ij 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 profit 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: In the formula, 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 represent two possible ways of adjusting the variable. + may represent an increase, while - represents 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, characterized in that: 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 smoothness cost.

6. The method according to claim 1, characterized in that: In each iteration, the iteration termination conditions also include: judging whether all robots have reached the Nash equilibrium state. If not, proceed to the next round of iteration. If so, judge whether other iteration 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: In the formula, 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, characterized in that: The global optimal position of each robot in the spherical vector particle swarm algorithm and the local optimum Defined as: Among them, ξ 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 Less than the previous position Total cost Update the local best position of particle p to position Otherwise, the local optimal position is still 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 the robots are all connected to the collaborative control subsystem, and the collaborative control subsystem includes: A map information acquisition module, used to acquire map information of the area to be planned, wherein the map information at least includes obstacle information; The particle optimization planning module is used to introduce the spherical vector particle swarm algorithm and optimize the spherical vector particle swarm algorithm based on the particle space position transformation strategy of game theory; then set a corresponding particle population for each robot, the particle position of each particle in the particle population represents a path of the corresponding robot, convert the path planning problem into a multi-objective optimization problem, take cost reduction as the optimization goal, construct a fitness function representing the cost, use the spherical vector particle swarm algorithm to perform iterative calculations to update the particle position corresponding to each robot until the iteration termination condition is met, and obtain the optimal particle position corresponding to each robot, and then output the collision-free path of multi-robot collaborative planning; Among them, 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 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 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