Multi-vehicle distributed continuous coverage trajectory planning method based on particle swarm optimization

Abstract

The invention discloses a multi-vehicle distributed continuous coverage trajectory planning method based on a particle swarm algorithm, and the method enables a multi-unmanned vehicle system to fully sense and collect information resources in a target map through the distributed search characteristics of multiple unmanned vehicles, and achieves the multi-vehicle path planning through the cooperation mechanism of the particle swarm algorithm. The map rasterization method supports dynamic adjustment of the size and number of grids, so that the method can better adapt to maps of different sizes, and motion parameters of the unmanned vehicle or a knowable function of the grids do not need to be changed; secondly, the positions of the particles correspond to unmanned vehicle control input, the advantages and disadvantages of the particles can be rapidly determined through fitness function evaluation, the search direction of the particles is guided, the convergence process is accelerated, and the particle swarm optimization does not need training and has certain stability; and finally, recording the optimal positions experienced by the particles in the target area range of the map, and learning the optimal particles in the group at the same time, so that environmental interference can be effectively coped with through group cooperation and dynamic adjustment.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot intelligent decision-making and control, and particularly relates to a multi-vehicle distributed continuous coverage trajectory planning method based on a particle swarm algorithm. Background Art

[0002] With the continuous progress of robot perception technology, the persistent coverage problem has received extensive attention. The definition of the persistent coverage problem is to continuously cover and monitor a specified area or target within a certain time range. Persistent coverage aims to keep track of the status and change trends of the target area or object in real time, and enhance the situation awareness ability in complex environments. Such special properties make it widely applicable in both military and civilian scenarios. For example, in civilian aspects, environmental monitoring, urban planning, and emergency rescue; on the military battlefield, border patrol, position reconnaissance and defense to ensure the safety of the camp. Continuous monitoring is also applicable to target search tasks, especially for those dynamically changing targets. The characteristics of such targets, such as their location and moving speed, are often uncertain, and it is difficult for searchers to directly obtain relevant information. Therefore, traditional search strategies usually adopt the method of comprehensively covering the entire task area instead of directly locating the target. Given that the target may reappear in the area that has already been searched, continuous monitoring becomes a more effective strategy, which can achieve uninterrupted and continuous monitoring of the target area.

[0003] Since this type of coverage task may be somewhat dangerous and is often difficult to complete manually, the inspection unmanned ground vehicle (UGV) has emerged. It has the ability to operate autonomously and is suitable for various harsh and complex environments. However, as the environmental area expands, the task performance of a single unmanned vehicle will be significantly reduced. To address this challenge, many researchers have proposed solutions for multi-unmanned vehicle collaborative operations to improve the efficiency of task execution. Compared with a single unmanned vehicle, the multi-unmanned vehicle system consists of multiple unmanned vehicles with relatively low costs and simpler structures, and can generally achieve better results in most cases. It is worth mentioning that the multi-unmanned vehicle system has stronger fault tolerance performance and parallel processing ability, and shows more excellent performance in executing large-scale tasks, such as scenarios of extensive area monitoring or exploration, so it has become an ideal choice for carrying out continuous monitoring of the target area.

[0004] The persistent coverage problem has been proven to be an NP-hard problem mathematically, indicating that there is no direct analytical solution to this problem. For a long time, the methods for solving the persistent coverage problem have mainly been divided into two categories. One is the method based on optimization theory, such as the ant colony algorithm (ACO), genetic algorithm (GA), and branch-and-price algorithm, etc. The other is the method based on learning, such as the deep reinforcement learning (DRL) algorithm, multi-agent reinforcement learning (MARL) algorithm, etc. However, in a dynamic and uncertain task environment, it is difficult for the learning-based method to ensure its stability and convergence. Therefore, the optimization-based evolutionary algorithm is still the mainstream solution to such problems.

[0005] In the evolutionary algorithm, the particle swarm optimization algorithm has gradually become the research direction of path planning. The particle swarm optimization algorithm simulates the behavior of "birds foraging", and iteratively searches for the optimal solution in the solution space through individual exploration and group collaboration, so as to determine the current optimal or sub-optimal single-step action. As time or the number of steps increases, a complete path and trajectory can be obtained. The particle swarm optimization algorithm is applicable to global or local dynamic and high-dimensional complex problems because of its simple principle, few set parameters, and fast convergence speed.

[0006] The trajectory planning problem of persistent coverage exactly belongs to the typical dynamic scenario task with a relatively high dimension. Not only are the dynamic targets unknown, but the information of the target area to be monitored is also not fully known and is dynamically changing. Therefore, in the case where the state information is partially observable, how to achieve the cooperative persistent monitoring trajectory planning of multiple vehicles for the target area is still an urgent problem to be solved. Summary of the Invention

[0007] In view of this, the present invention provides a multi-vehicle distributed persistent coverage trajectory planning method based on the particle swarm optimization algorithm. Based on the control signal generated by a single unmanned vehicle for estimating its own domain area in the absence of prior information about the target area, and then using the distributed cooperation exploration among multiple vehicles, the persistent coverage of the target area is realized.

[0008] A multi-vehicle distributed persistent coverage trajectory planning method based on the particle swarm optimization algorithm provided by the present invention includes the following steps:

[0009] Step 1: Construct a visibility grid map G(t) of the target area, where the visibility function is g(a, b, t), (a, b) is the grid, and t is the current time;

[0010] Step 2: Establish a first motion model for the trajectory planning of multiple unmanned vehicles. The first motion model consists of a state variable s i (t), a control variable u i (t), a dynamic model, and a state transition model;

[0011] Step 3: Based on the visibility grid map established in Step 1 and the first motion model established in Step 2, establish a trajectory optimization problem for continuous coverage of multiple unmanned vehicles, including the objective function V and the constraint conditions;

[0012] Step 4: Design a particle swarm optimization algorithm according to the trajectory optimization problem established in Step 3, including specifying the coding criteria for the particle velocity and position, and constructing the fitness function;

[0013] Step 5: In actual use, set the initial state variables of multiple unmanned vehicles, and use the particle swarm optimization algorithm for calculation to obtain the actual trajectories of continuous coverage of multiple unmanned vehicles based on the particle swarm planning strategy.

[0014] Further, the construction method of the visibility grid map of the target area in Step 1 is as follows:

[0015] Step 1.1: Construct a grid map of m c ×m r , and the state of the grid map is represented by the visibility function g. For the grid (a, b) with the abscissa a ∈ (1, m c ), and the ordinate b ∈ (1, m r ), its visibility function g(a, b, t) is expressed as:

[0016]

[0017] where t v (a, b) represents the moment when the grid (a, b) is visited by any vehicle within the set time, and its value is set to +∞ before being visited; g v ≥0 represents the initial visibility of the grid (a, b) at the visit moment t v (a, b), η ∈ (0, 1) is the visibility decay factor, and the visibility of the grid satisfies the condition g(a, b, t) ∈ (0, g v );

[0018] Step 1.2: Construct the visibility of each grid into a two-dimensional matrix of m c ×m r , defined as the visibility map matrix G(t) as the visibility grid map.

[0019] Further, the state variables of the first motion model are expressed as:

[0020] s i (t) = [p i (t), θ i (t)]

[0021] where i = 1, 2,..., N, N is the number of unmanned vehicles; p i(t) is the position of the autonomous vehicle i at the current moment t, and θ i (k) is the azimuth angle of the autonomous vehicle i in the inertial coordinate system;

[0022] The control variables of the first motion model are expressed as:

[0023] u i (t) = [v i (t), w i (t)]

[0024] where, v i (t) is the forward speed of the autonomous vehicle i at the current moment t, and w i (t) is the angular velocity of the autonomous vehicle i at the current moment t;

[0025] The dynamic model of the first motion model is expressed as:

[0026]

[0027] where, is the position increment, is the azimuth angle increment;

[0028] The state transition model of the first motion model is expressed as:

[0029]

[0030] where, dt is the time interval, is the state variable increment at the next moment.

[0031] Furthermore, the calculation method of the state variable increment is: f() is the dynamic differential operator of the autonomous vehicle i, which is used to calculate the state variable increment at the next moment according to the state variable s i (t) of the autonomous vehicle i at the current moment and the control variable u i (t).

[0032] Furthermore, the objective function V established in step 3 is expressed as:

[0033]

[0034] where, maximizing V(t f ) represents maximizing the historical cumulative full-map visibility, u(t) is the joint control variable of the autonomous vehicle formation, and g(a, b, τ) represents the visibility of the grid (a, b) within dτ time;

[0035] The constraint conditions are expressed as:

[0036]

[0037] 0 ≤ v i (t) ≤ v max , -w max ≤ w i (t) ≤ w max

[0038] where u i (t) is the input sequence of the control variable of the driverless vehicle i, n is the end time, v max and w max are the maximum speed and the maximum angular velocity constraints of the driverless vehicle i, respectively.

[0039] Furthermore, the encoding criteria for the particle velocity and position in step 4 are expressed as:

[0040]

[0041] where m ∈ [1, M] is the particle number, k ∈ (1, K) is the iteration generation, the joint control variable u(t) and the control variation Δu(t) of the driverless vehicle formation at time t are encoded as the particle position and the particle velocity

[0042] The update formulas for the particle position and velocity are:

[0043]

[0044] where w is the self-inertia factor, c1 and c2 are the learning factors of the individual and the group, respectively, and r1, r2 ∈ [0, 1] both represent random decay coefficients;

[0045] The fitness function is constructed as the global map visibility at time t + dt, expressed as:

[0046] Fitness = V(t + dt)

[0047] It is obtained by summing the visibilities g(a, b, t + dt) of each grid at time t + dt.

[0048] Furthermore, step 5 includes:

[0049] Step 5.1: Let the maximum value of the planning time be T, the time interval be d, the maximum number of iterations of the particle swarm optimization algorithm be K, and the number of particles in each generation be M;

[0050] Step 5.2: Initialize the time t = 0, and initialize the initial positions p₁(0), p₂(0), …, p N(0), obtain all the initial state variables s(0) = [s1(0), s2(0), ···, s N (0)] and initialize the grid map;

[0051] Step 5.3, record the state s(k) of the vehicle, calculate the visibility matrix G(t) and the objective function value V(t) of the grid map at the current time t; initialize the position and velocity of each particle according to the coding criterion;

[0052] Step 5.4, adopt the fitness value function Fitness to initialize the individual historical optimal position of the population Individual historical optimal fitness value Group historical optimal position Group historical optimal fitness value Initialize the iteration number k = 0;

[0053] Step 5.5, for each particle m, calculate the position and velocity of the particle m in the (k + 1)-th generation using the update formula, and calculate the fitness value of the particle m using the fitness value function Update the individual historical optimal position and fitness value

[0054] Step 5.6, update the group historical optimal position and fitness value

[0055] Step 5.7, if k > K, then execute Step 5.8 to end the iteration of the particle swarm algorithm; otherwise, let k = k + 1 and execute Step 5.5;

[0056] Step 5.8, perform reverse decoding using the coding criterion, and regard the position x gbest of the group historical optimal after iteration as the true joint control variable u(t) = [u1(t), u(t), ···, u N (t)] of the unmanned vehicle at the current time t;

[0057] Step 5.9, combine the control variable u i (t) with the state s i (t), and use the state transition model to calculate the state variable s i (t + 1) at the next moment;

[0058] Step 5.10, if t ≤ T, the unmanned vehicle continues to run, update the current time t, and execute Step 5.3; otherwise, the unmanned vehicle terminates running, and output the historical state variables [s(0), s(1), ···, s(T)] of each unmanned vehicle as the final continuous coverage trajectory of the unmanned vehicle formation.

[0059] Furthermore, the way to initialize the position and velocity of each particle according to the coding criterion is random initialization.

[0060] Beneficial effects:

[0061] The present invention utilizes the distributed search characteristics of multiple unmanned vehicles to enable the multi-unmanned vehicle system to fully perceive and collect information resources in the target map, and uses the cooperation mechanism of the particle swarm algorithm for multi-vehicle path planning. First, since the map rasterization method supports dynamic adjustment of the raster size and quantity, the present invention can better adapt to maps of different sizes without changing the motion parameters of the unmanned vehicle or the form of the visibility function of the raster. Second, since the position of the particle corresponds to the control input of the unmanned vehicle, the quality of the particle can be quickly determined through the evaluation of the fitness function, thereby guiding the search direction of the particle, accelerating the convergence process, and the particle swarm algorithm does not require training and has a certain stability. Finally, the present invention has strong anti-interference ability. Within the target area of the map, the particles record the optimal positions they have experienced, and at the same time learn from the optimal particles in the group. Therefore, through group cooperation and dynamic adjustment, it can effectively cope with environmental interference.

[0062] Based on the distributed motion exploration of the unmanned vehicle system, the present invention cooperatively updates the visibility map, enabling the system to make full use of information through the information sharing mechanism of the particle swarm algorithm and jointly achieve the problem of continuous coverage trajectory planning for the map area, which has important practical reference significance. Description of the drawings

[0063] Figure 1 It is a schematic diagram of the processing flow of a multi-vehicle distributed continuous coverage trajectory planning method based on the particle swarm algorithm provided by the present invention.

[0064] Figure 2 It is a schematic diagram of the raster map generated by using the multi-vehicle distributed continuous coverage trajectory planning method based on the particle swarm algorithm provided by the present invention in the embodiment.

[0065] Figure 3 It is a schematic diagram of the continuous coverage trajectory of multiple unmanned vehicles for the map area generated by using the multi-vehicle distributed continuous coverage trajectory planning method based on the particle swarm algorithm provided by the present invention in the embodiment.

[0066] Figure 4 It is a schematic diagram of the curve of the cumulative visibility changing with time obtained by using the multi-vehicle distributed continuous coverage trajectory planning method based on the particle swarm algorithm provided by the present invention in the embodiment.

[0067] Figure 5Schematic diagram of the historical coverage rate varying with time obtained by using a multi-vehicle distributed continuous coverage trajectory planning method provided by the present invention in an embodiment.

[0068] Figure 6 Schematic diagram of the grid visibility at the end of the continuous coverage task of the map area obtained by using a multi-vehicle distributed continuous coverage trajectory planning method provided by the present invention in an embodiment. Detailed implementation manners

[0069] The following takes embodiments in conjunction with the drawings and describes the present invention in detail.

[0070] A multi-vehicle distributed continuous coverage trajectory planning method provided by the present invention, its core idea is: first, rasterize the target area into a grid map, then introduce the visibility function of the grid map to quantify the coverage degree of the map. In terms of the motion models of multiple unmanned vehicles, the state variables, control variables, dynamic equations and state transition functions of the unmanned vehicles are designed; on this basis, a multi-vehicle trajectory optimization problem for continuous coverage of the target area is constructed, and the objective function and constraint conditions are clarified; then, based on the particle swarm algorithm, the joint estimated state variables and change amounts of multiple unmanned vehicles are regarded as optimization parameters, encoded into the position and velocity of particles, and searched in the solution space until the maximum number of iterations converges; the true optimal joint control variables at the current moment are obtained; then input into the state transition model to find the state variables at the next moment, that is, the position and direction information of the multi-vehicle system. Repeat this process. When the maximum time step is reached, the optimal trajectory of continuous coverage of the multi-unmanned vehicle system is finally obtained. The present invention is based on the distributed visibility map of the unmanned vehicle system, enabling the system to solve the trajectory planning problem relatively quickly and achieve as many full coverages of the target area as possible.

[0071] A multi-vehicle distributed continuous coverage trajectory planning method provided by the present invention, the processing flow is as Figure 1 shown, and specifically includes the following steps:

[0072] Step 1: Construct a visibility grid map of the target area.

[0073] Step 1.1: Construct an m c ×m r grid map for the target area, and the state of the grid map is represented by the visibility function g. For the grid (a, b) with the abscissa a ∈ (1, m c ), and the ordinate b ∈ (1, m r ), at the current moment t, its visibility function g(a, b, t) is expressed as the following formula:

[0074]

[0075] Among them, t v (a, b) represents the time when the grid (a, b) is accessed by any vehicle within the adjacent set time. Before being accessed, its value is set to +∞; g v ≥0 represents the initial knowability of the grid (a, b) at the access time t v (a, b), η ∈ (0, 1) is the knowability decay factor, and the knowability of the grid satisfies the condition g(a, b, t) ∈ (0, g v ).

[0076] Step 1.2: Construct the knowability of each grid into an m c ×m r two-dimensional matrix, defined as the knowability map matrix G(t).

[0077] Step 2: Construct the first motion model for the trajectory planning of multiple unmanned vehicles. The first motion model consists of state variables, control variables, a dynamic model, and a state transition model.

[0078] Among them, the state variable is the state variable s i (t) of unmanned vehicle i, and the expression is:[[]]

[0079] s i (t) = [p i (t), θ i (t)][[]]

[0080] Among them, i = 1, 2,..., N, and N is the number of unmanned vehicles; p i (t) represents the position of unmanned vehicle i at the current time t, and θ i (k) represents the azimuth angle of unmanned vehicle i in the inertial coordinate system.

[0081] The control variable is u i (t), and the expression is:[[]]

[0082] u i (t) = [v i (t), w i (t)][[]]

[0083] Among them, v i (t) is the forward speed of unmanned vehicle i at the current time t, and w i (t) is the angular velocity of unmanned vehicle i at the current time t.

[0084] Establish a dynamic model for each unmanned vehicle i, which is used to calculate the increment of the state variable at the next moment by using the state variable and control variable at the current time t. The expression is:[[]]

[0085]

[0086] Among them, is the state variable increment at the next moment; f() represents the differential operator of the unmanned vehicle dynamics, which is used to derive the state variable increment at the next moment according to the state variable s i (t) of the unmanned vehicle at the current moment and the control variable u i (t).

[0087] A state transition model is established for each unmanned vehicle i, which is used to calculate the state variable at the next moment given the state variable and the state variable increment at the current moment t. The expression is:

[0088]

[0089] where dt represents the time interval.

[0090] Step 3: Establish a trajectory optimization problem for continuous coverage of multiple unmanned vehicles based on the first motion model constructed in Step 2, including the objective function and constraints.

[0091] Specifically, for the objective function V of the continuous coverage trajectory optimization problem established for the unmanned vehicle i, the expression is:

[0092]

[0093] where maximizing V(t f ) represents maximizing the historical cumulative map visibility, u(t) is the joint control variable of the unmanned vehicle formation, and g(a, b, τ) represents the visibility of the grid (a, b) within the time dτ.

[0094] For the constraints of the continuous coverage trajectory optimization problem established for the unmanned vehicle i, the expression is:

[0095]

[0096] 0 ≤ v i (t) ≤ v max , -w max ≤ w i (t) ≤ w max

[0097] where u i (t) represents the input sequence of the control variable, and n is the end time. v max and w max are respectively the maximum speed and maximum angular velocity constraints of the unmanned vehicle i in the physical sense.

[0098] Step 4: Design a particle swarm optimization algorithm, including specifying the encoding criteria for particle velocity and position and constructing the fitness function. The encoding criteria for particle position and velocity are as follows:

[0099]

[0100] Among them, \(m\in[1, <]\) represents the particle number, and \(k\in(1, K)\) represents the iteration generation. The joint control variable \(u(t)\) and the control change amount \(\Delta u(t)\) of the unmanned vehicle formation at time \(t\) are respectively encoded as the particle positions. Particle velocity

[0101] The update formulas for the particle position and velocity are as follows:

[0102]

[0103] In the velocity update formula of the particle, \(w\) represents the self-inertia factor, \(c1\) and \(c2\) respectively represent the individual and group learning factors, and \(r1, r2\in[0, 1]\) both represent the random decay coefficients.

[0104] Construct the fitness function as follows:

[0105] Fitness = V(t + dt)

[0106] The fitness value function is defined as the global map visibility at time \(t + dt\), which can be obtained by summing the visibilities \(g(a, b, t + dt)\) of each grid at time \(t + dt\).

[0107] Step 5. The process of obtaining the continuous coverage trajectory planning of multiple unmanned vehicles in actual use is as follows:

[0108] Step 5.1. Set parameters.

[0109] Let the maximum value of the planning time be \(T\), the time interval be \(dt\), the maximum number of iterations of the PSO algorithm within each time interval be \(K\), the number of particles in each generation of the particle population be \(M\), the self-inertia factor in the velocity update formula be \(w\), the individual and group learning factors be \(c1\) and \(c2\) respectively, and the random decay coefficients be \(r1\) and \(r2\).

[0110] Step 5.2. Initialize \(t = 0\), initialize the initial positions \(p1(0), p2(0), \cdots, p\) N (0) of all unmanned vehicles, and obtain the joint initial state variables \(s(0)=[s1(0), s2(0), \cdots, s\) N (0)] of all unmanned vehicles, and initialize the grid map.

[0111] Step 5.3. Record the state \(s(k)\) of the vehicle, and calculate the visibility matrix \(G(t)\) and the objective function value \(V(t)\) of the grid map at the current time \(t\).

[0112] Step 5.4. According to the coding rule, randomly initialize the position and velocity of each particle. The position of each particle Denote an estimated combined control variable at time t Speed Denote an estimated combined control variation at time t

[0113] Step 5.5: Initialize the individual historical optimal positions of the population using the fitness value function Fitness Individual historical optimal fitness value Group historical optimal position Group historical optimal fitness value Initialize the iteration count as k = 0

[0114] Step 5.6: For each particle m, calculate the position of particle m in the (k + 1)-th generation using the update formula And speed Calculate the fitness value of particle m using the fitness value function Update the individual historical optimal position of particle m And fitness value

[0115] Step 5.7: Update the group historical optimal position And fitness value

[0116] Step 5.8: If k > K, then execute Step 5.9 to end the particle swarm algorithm iteration; otherwise, set k = k + 1 and execute Step 5.6

[0117] Step 5.9: Perform reverse decoding using the coding rule, and regard the position x of the group historical optimal after iteration gbest As the true combined control variable u(t) = [u1(t), u(t), ···, u N (t)] of the driverless vehicle at the current time t

[0118] Step 5.10: Combining the control variable u i (t) with the state s i (t), the driverless vehicle i returns the state variable s of the next time step through the state transition model i (t + 1)

[0119] Step 5.11: If t ≤ T, then continue the operation of the driverless vehicle, update the current time t, and execute Step 5.3; otherwise, the task reaches the predetermined maximum duration, the driverless vehicle terminates the operation, and outputs the historical state variables [s(0), s(1), ···, s(T)] of each driverless vehicle, which is the final continuous coverage trajectory of the driverless vehicle formation

[0120] Example:

[0121] In this embodiment, the Python language is used to write and build a simulation environment to verify a multi-vehicle distributed continuous coverage trajectory planning method based on the particle swarm optimization algorithm provided by the present invention. The specific process is as follows:

[0122] Simulation settings: The method proposed by the present invention is implemented by writing in the Python language. In this embodiment, there are a total of N = 5 unmanned vehicles, and the target area is divided into a 10×10 grid map. As Figure 2 shown, the initial grid visibility g v = 1, and the attenuation coefficient τ = 0.02; let the maximum experimental duration T = 250, the time interval dt = 1, the maximum number of iterations of the PSO algorithm at each step be K = 100, and the number of particle populations in each generation M = 20 remains unchanged. The self-inertia factor w in the velocity update formula is 0.5, and the individual and group learning factors are respectively: c1 = 1.6, c2 = 1.8, and the random attenuation coefficients r1 and r2 are random numbers in the interval [0, 1].

[0123] Simulation results: Figure 3 represents the final result, that is, the trajectory planning under multi-vehicle distributed continuous coverage generated by the PSO algorithm. Figure 3 The starting points of the five unmanned vehicles in are all random positions within the area, and they cooperate to complete the trajectory of continuous coverage of the target area. From Figure 3 the results, it can be seen that the unmanned vehicles continue to move separately in the grid map, achieving continuous coverage of the entire area.

[0124] Figure 4 and Figure 5 respectively represent the changes in the cumulative visibility and historical coverage rate of the multi-unmanned vehicle system with N = 5 within the time step T = 250 during training. The historical coverage rate refers to the grids that have been searched historically at the end of the continuous coverage task, that is; the proportion of grids with historical visibility g(a, b, t)>0 in the total number of grids on the map can be used to measure the continuous coverage effect. At the initial moment, the cumulative visibility and historical coverage rate obtained by the unmanned vehicle are very small. As the number of time steps increases, the cumulative visibility generally shows a fluctuating but rising trend, while the historical coverage rate gradually increases and almost approaches 100%, and finally tends to be stable.

[0125] Figure 6 shows the visibility state of the grid map at the end of the task. The visibility of most grids is greater than 0, showing a good effect of continuous coverage.

[0126] In summary, the above is only a preferred embodiment of the present invention and is not intended to limit the protection scope of the present invention. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A multi-vehicle distributed continuous coverage trajectory planning method based on the particle swarm algorithm, characterized in that Including the following steps: Step 1: Construct the visibility grid map G(t) of the target area, where the visibility function is g(a, b, t), (a, b) is the grid, and t is the current time; Step 2: Establish a first motion model for the trajectory planning of multiple unmanned vehicles. The first motion model consists of a state variable s i (t), a control variable u i (t), a dynamics model, and a state transition model; Step 3: Establish a trajectory optimization problem for continuous coverage of multiple unmanned vehicles based on the visibility grid map established in Step 1 and the first motion model established in Step 2, including the objective function V and the constraint conditions; Step 4: Design a particle swarm optimization algorithm according to the trajectory optimization problem established in Step 3, including specifying the encoding criteria for particle velocity and position and constructing a fitness function; Step 5: In actual use, set the initial state variables of multiple unmanned vehicles and use the particle swarm optimization algorithm for calculation to obtain the actual trajectories of continuous coverage of multiple unmanned vehicles based on the particle swarm planning strategy.

2. The multi-vehicle distributed continuous coverage trajectory planning method according to claim 1, wherein The construction method of the visibility grid map of the target area in Step 1 is as follows: Step 1.1: Construct an m c ×m r grid map. The state of the grid map is represented by the visibility function g. For a grid (a, b) with abscissa a ∈ (1, m c ), and ordinate b ∈ (1, m r ), its visibility function g(a, b, t) is expressed as: where t v (a, b) represents the moment when the grid (a, b) is accessed by any vehicle within the set time, and its value is set to +∞ before being accessed; g v ≥0 represents the initial knowability of the grid (a, b) at the access moment t v (a, b), η ∈ (0, 1) is the knowability decay factor, and the knowability of the grid satisfies the condition g(a, b, t) ∈ (0, g v ). Step 1.

2. Construct the knowability of each grid as m c ×m r two-dimensional matrix, defined as the knowability map matrix G(t) as the knowability grid map.

3. The multi-vehicle distributed continuous coverage trajectory planning method according to claim 1, wherein The state variables of the first motion model are represented as: s i (t) = [p i (t), θ i (t)] where \(i = 1, 2, \ldots, N\), and \(N\) is the number of autonomous vehicles; \(p\) i (t) is the position of autonomous vehicle \(i\) at the current time \(t\), and \(\theta\) i (k) is the azimuth angle of autonomous vehicle \(i\) in the inertial coordinate system; The control variables of the first motion model are represented as: u i (t) = [v i (t), w i (t)] where, v i (t) is the forward speed of the driverless vehicle i at the current moment t, and w i (t) is the angular velocity of the driverless vehicle i at the current moment t; The dynamic model of the first motion model is represented as: wherein, is the position increment, is the azimuth increment; The state transition model of the first motion model is represented as: where dt is the time interval, is the increment of the state variable at the next moment.

4. The multi-vehicle distributed continuous coverage trajectory planning method according to claim 3, wherein The increment of the state variable is calculated as follows: f() is the dynamic differential operator of the autonomous vehicle i, which is used to calculate the increment of the state variable at the next moment according to the state variable s i (t) and the control variable u i (t) of the autonomous vehicle i at the current moment.

5. The multi-vehicle distributed continuous coverage trajectory planning method according to claim 1, wherein The objective function V established in Step 3 is represented as: Among them, maximizing V(t f ) represents maximizing the historical cumulative global visibility. u(t) is the joint control variable of the unmanned vehicle formation, and g(a, b, τ) represents the visibility of grid (a, b) within dτ time; The constraint conditions are represented as: 0 ≤ v i (t) ≤ v max , -w max ≤ w i (t) ≤ w max Among them, u i (t) is the input sequence of the control variable of the driverless vehicle i, n is the end time, v max and w max are respectively the maximum speed and maximum angular velocity constraints of the driverless vehicle i.

6. The multi-vehicle distributed continuous coverage trajectory planning method according to claim 1, wherein The encoding criteria for particle velocity and position in Step 4 are represented as: where \(m\in[1,M]\) is the particle number, and \(k\in(1,K)\) is the iteration generation. The joint control variable \(u(t)\) and the control variation \(\Delta u(t)\) of the unmanned vehicle formation at time \(t\) are respectively encoded as the particle position and the particle velocity The update formulas for the particle position and velocity are as follows: where w is the self-inertia factor, c1 and c2 are the learning factors of the individual and the group respectively, and r1, r2 ∈ [0, 1] both represent random decay coefficients; The constructed fitness function is the overall map visibility at time t + dt, which is represented as: Fitness = V(t + dt) It is obtained by summing the visibilities g(a, b, t + dt) of each grid at time t + dt.

7. The multi-vehicle distributed continuous coverage trajectory planning method according to claim 6, wherein Step 5 includes: Step 5.1: Let the maximum value of the planning time be T, the time interval be dt, the maximum number of iterations of the particle swarm optimization algorithm be K, and the number of particles in each generation of the particle swarm be M; Step 5.2: Initialize time \(t = 0\), initialize the initial positions \(p_1(0), p_2(0), \cdots, p\) N (0), and obtain the combined initial state variables \(s(0)=[s_1(0), s_2(0), \cdots, s\) N (0)] of all the driverless vehicles, and initialize the grid map; Step 5.3: Record the state s(k) of the vehicle, calculate the visibility matrix G(t) of the grid map at the current time t and the objective function value V(t); initialize the position and velocity of each particle according to the encoding criteria; Step 5.4: Use the fitness value function Fitness to initialize the individual historical optimal positions of the population Individual historical optimal fitness value Group historical optimal position Group historical optimal fitness value Initialize the iteration number to k = 0; Step 5.5: For each particle m, calculate the position of particle m in the (k + 1)-th generation using the update formula and velocity Calculate the fitness value of particle m using the fitness value function Update the individual historical optimal position of particle m and fitness value Step 5.6, update the group's historical optimal position and fitness value Step 5.7: If k > K, then execute Step 5.8 to end the iteration of the particle swarm algorithm; otherwise, let k = k + 1 and execute Step 5.5; Step 5.

8. Perform reverse decoding using the coding criterion, and regard the position x of the population historical optimum after iteration gbest as the true joint control variable u(t)=[u1(t), u(t), ···, u N (t)] of the driverless vehicle at the current moment t; Step 5.9: Combine the control variable u i (t) and the state s i (t), and use the state transition model to calculate the state variable s i (t + 1); Step 5.10: If t ≤ T, the unmanned vehicle continues to run, update the current time t, and execute Step 5.3; otherwise, the unmanned vehicle terminates running, and output the historical state variables [s(0), s(1), ···, s(T)] of each unmanned vehicle as the final continuous coverage trajectory of the unmanned vehicle formation.

8. The multi-vehicle distributed continuous coverage trajectory planning method according to claim 7, wherein The method of initializing the position and velocity of each particle according to the encoding criteria is random initialization.

Images