Multi-robot formation control method under GPS denial and complex electromagnetic conditions

Abstract

The present invention proposes a multi-robot formation control method under GPS denial and complex electromagnetic conditions, including the following steps: Step 1, establish a general multi-robot system model; Step 2, adjust the state vector model of the multi-robot formation system in a real environment to generate a multi-robot system with multiple time delays; Step 3, determine the maximum time delay of communication between robots and the maximum amplitude of the noise environment, and then design the position proportional coefficient and the speed proportional coefficient based on this. The present invention solves the consistency problem of the second-order multi-robot system under multiple time delay and noise conditions, converts the system characteristic equation into a quadratic polynomial of pure imaginary characteristic roots through frequency domain analysis, and then solves it to finally obtain its critical stable state.

Description

Technical Field

[0001] The present invention relates to the technical field of robot formation, and in particular to a multi-robot formation control method under GPS denial and complex electromagnetic conditions. Background Art

[0002] Equation Chapter (Next) Section 1 Aiming at the shortcomings of traditional formation control algorithms, considering the increase in the number of robots in multi-robot systems and the continuous improvement of the data processing capabilities of individual robots, distributed multi-robot control algorithms have received widespread attention from researchers. Distributed multi-robot systems can make better use of the data processing resources of robots and share the pressure of the central processing unit. They have great advantages in flexibility and fault tolerance, and the consistency problem is the most basic problem in distributed multi-robot control algorithms. The consistency problem in an ideal environment has been studied in depth and many research results have been achieved. However, in actual application environments, due to the existence of uncertain factors such as communication noise, communication delay, and measurement error, the analysis of the consistency problem has new difficulties.

[0003] In the prior art, the document "Distributed Consensus Control for Second-Order Agents with Fixed Topology and Time-Delay" studies the conditions for the system to reach consistency under uniform time delay conditions when the communication structure of the second-order multi-robot system is a directed graph containing a spanning tree or a strongly connected graph, but the document does not consider noise conditions, nor does it consider the consistency problem under multiple different time delay conditions. The document "Sampled-data based average consensus of second-order integral multi-agent systems: switching topologies and communication noises" aims at the consistency problem of second-order multi-robot systems under noise conditions and proposes a control protocol based on distributed sampling data to achieve system consistency, but the algorithm does not take time delay conditions into account. The document "Distributed control for multi-agent consensus motions with nonuniform time delays" uses frequency domain analysis to study the consistency problem of second-order multi-robot systems under non-uniform multiple time delay conditions. Compared with the Lyapunov method, it has lower conservatism and generalizes the results to high-order multi-robot systems, but does not consider noise conditions. Summary of the invention

[0004] In order to overcome the consistency problem of robot system delay and noise environment, the present invention proposes a multi-robot formation control method under GPS denial and complex electromagnetic conditions.

[0005] The present invention aims to provide a multi-robot formation control method under GPS denial and complex electromagnetic conditions, characterized in that the multi-robot formation control method under GPS denial and complex electromagnetic conditions comprises the following steps:

[0006] Step 1, establish a general multi-robot system model;

[0007] Step 2, adjusting the state vector model of the multi-robot formation system in a real environment to generate a multi-robot system with various time delays;

[0008] Step 3: Determine the maximum delay of communication between robots and the maximum amplitude of the noise environment, and then design the position proportional coefficient and speed proportional coefficient based on this.

[0009] Furthermore, in step 1, the general multi-robot system model is:

[0010]

[0011] in, represents the general multi-robot system model, Ψ is the coefficient matrix, is the Kronecker product, k1 is the position proportional coefficient, k2 is the velocity proportional coefficient, I represents the unit matrix, L represents the Laplace matrix; S(t) is the state vector of the multi-robot system:

[0012] S(t)=[δ1(t), δ2(t), δ3(t),..., δ i (t)]

[0013] Among them, δ i (t) is the state vector of the ith robot at time t:

[0014] δ i (t) = [x i (t), v i (t)] T

[0015] Among them, x i (t) is the position of the ith robot at time t, v i (t) is the velocity of the ith robot at time t;

[0016]

[0017] Among them, u i(t) is the input control variable of the ith robot at time t.

[0018] Furthermore, in step 2, the multi-delay multi-robot system is:

[0019]

[0020] Where ζ(t) is the communication noise or measurement noise between robots, τ d is the transmission delay, L d When the delay is τ d When the robot nodes form the sub-topology graph corresponding to the Laplace matrix,

[0021] Furthermore, in step 3, when the multi-robot system with multiple time delays is an undirected graph, the relationship between the system delay and noise of the multi-robot system with multiple time delays and the position proportional coefficient and the speed proportional coefficient is:

[0022]

[0023] Among them, τ max represents the maximum value of the transmission delay τ, w max represents the maximum value of the angular frequency w, λ max represents the largest eigenvalue of the Laplace matrix L.

[0024] Furthermore, in step 3, when the multi-robot system with multiple time delays is a directed graph and a spanning tree exists, the relationship between the system delay and noise of the multi-robot system with multiple time delays and the position proportional coefficient and the speed proportional coefficient is:

[0025]

[0026] in, w i represents the angular frequency w of the ith robot in the frequency domain analysis, λ i represents the nonzero eigenvalues ​​of the Laplacian matrix L.

[0027] The beneficial effects of the present invention include:

[0028] The present invention solves the consistency problem of the second-order multi-robot system under various time delay and noise conditions. The system characteristic equation is converted into a quadratic polynomial of pure imaginary characteristic roots through frequency domain analysis, and then solved to finally obtain its critical stable state.

[0029] The control method proposed in the present invention is aimed at an omnidirectional mobile robot, the horizontal and vertical directions of which can be controlled separately to construct two one-dimensional second-order multi-robot systems, which realize the system consistency under delay and noise conditions. BRIEF DESCRIPTION OF THE DRAWINGS

[0030] Figure 1 It is a structural schematic diagram of a multi-robot formation control method under GPS denial and complex electromagnetic conditions provided by an embodiment of the present invention;

[0031] Figure 2 is a system communication topology diagram provided by Embodiment 1 of the present invention;

[0032] Figure 3 is a white noise amplitude diagram provided by the first embodiment of the present invention;

[0033] Figure 4 It is a MATLAB program structure diagram provided by the first embodiment of the present invention;

[0034] Figure 5 It is a simulation diagram of the trajectory of x changing with time provided by Experiment 1 in Example 1 of the present invention;

[0035] Figure 6 is a simulation diagram of the trajectory of v changing with time provided by Experiment 1 in Example 1 of the present invention;

[0036] Figure 7 It is a simulation diagram of the trajectory of x changing with time provided by Experiment 2 in Example 1 of the present invention;

[0037] Figure 8 It is a simulation diagram of the trajectory of v changing with time provided by Experiment 2 in Example 1 of the present invention;

[0038] Fig. 9 is a system communication topology diagram provided by Embodiment 2 of the present invention;

[0039] Fig.10 It is a MATLAB program structure diagram provided by the second embodiment of the present invention;

[0040] Fig.11 It is a simulation diagram of the trajectory of x changing with time provided in Experiment 1 in Embodiment 2 of the present invention;

[0041] Fig.12 It is a simulation diagram of the trajectory of v changing with time provided by Experiment 1 in Embodiment 2 of the present invention;

[0042] Fig.13 It is a simulation diagram of the trajectory of x changing with time provided by Experiment 2 in Example 2 of the present invention;

[0043] Fig.14It is a simulation diagram of the trajectory of v changing with time provided by Experiment 2 in Embodiment 2 of the present invention;

[0044] Fig.15 This is a schematic diagram of the actual measurement of the UWB positioning system provided in Example 3 of the present invention. DETAILED DESCRIPTION

[0045] The technical solution of the present invention is described in more detail below in conjunction with the accompanying drawings. The present invention includes but is not limited to the following embodiments.

[0046] As attached Figure 1 As shown, the present invention provides a multi-robot formation control method under GPS denial and complex electromagnetic conditions, comprising the following steps:

[0047] Step 1: Establish a general multi-robot system model.

[0048] Assuming that the system consists of n omnidirectional robots, the dynamic characteristics of the i-th omnidirectional robot in the x direction are:

[0049]

[0050] Among them, x i (t) is the position of the ith robot at time t, v i (t) is the speed of the ith robot at time t, u i (t) is the input control quantity of the ith robot at time t. If any ith robot and qth robot in the multi-robot system realizes:

[0051]

[0052]

[0053] Then it is called the control quantity u i (t) The dynamic characteristics of the multi-robot system achieve consistency.

[0054] Assume the state vector of the i-th robot is:

[0055] δ i (t) = [x i (t), v i (t)] T (3)

[0056] Among them, δ i (t) is the state vector of the i-th robot at time t; then the state vector of the multi-robot system is:

[0057] S(t)=[δ1(t), δ2(t), δ3(t),..., δ n (t)] (4)

[0058] Among them, S(t) is the state vector of the multi-robot system.

[0059] Set up a general multi-robot system as:

[0060]

[0061] in, represents the general multi-robot system model, Ψ is the coefficient matrix, is the Kronecker product.

[0062] In the ideal case without noise and delay, the control quantity is as follows:

[0063]

[0064] In the formula, a iq >0 is the communication topology weight between robot i and robot q, k1 is the position proportional coefficient to be designed, k2 is the speed proportional coefficient to be designed, N i is the set of robots that have communication topology with the i-th robot.

[0065] When the communication topology of the multi-robot system is a connected topology graph, the coefficient matrix Ψ has one double zero root and the other characteristic roots are negative real numbers.

[0066] When the communication topology of the multi-robot system is a directed graph and contains a spanning tree, λ i is the eigenvalue of the Laplace matrix L of the multi-robot system. The coefficient matrix Ψ has one double zero root, and the real parts of the other eigenroots are negative.

[0067] Step 2: Adjust the state vector model of the multi-robot formation system in a real environment to generate a multi-robot system with various time delays.

[0068] In the real environment, due to the existence of noise interference and the differences in communication between different robot hardware, the above control quantities need to be improved. Assuming that there are D different time delays in the system, the multi-robot system with multiple time delays is:

[0069]

[0070] Where ζ(t) is the communication noise or measurement noise between robots, τ d is the transmission delay, L d When the delay is τ d When the robot nodes form the sub-topology graph corresponding to the Laplace matrix,

[0071] Step 3: Determine the maximum delay of communication between robots and the maximum amplitude of the noise environment, and then design the position proportional coefficient and speed proportional coefficient based on this.

[0072] Step 31, when the multi-robot system with multiple time delays is an undirected graph, the frequency domain analysis method is used for analysis, and Laplace transform is performed on the multi-robot system with multiple time delays to obtain:

[0073]

[0074] Among them, I 2n is the 2n-order identity matrix, I n is the n-th order identity matrix.

[0075] make So the determinant |G τ The characteristic root of (s)| is the characteristic root of the system.

[0076] The consistency condition of a multi-robot system with multiple time delays relative to a general multi-robot system is how to keep the system characteristic roots in the negative half plane under the interference of time delay τ and noise ζ(t). Because the measurement noise and communication noise ζ(t) in the real environment are uncertain quantities, accurate quantitative analysis cannot be performed. Therefore, it is only necessary to consider the value to which the system delay τ increases under the action of noise ζ(t) so that the non-zero characteristic root of the system appears on the imaginary axis for the first time. The time delay τ at this time is the critical value for the system to remain stable.

[0077] Assume that the system characteristic root is on the imaginary axis, so let s = jw be its characteristic root, j represents the sign of the imaginary part, and w represents the angular frequency; is the eigenvector corresponding to the eigenvalue, and assume that For an n-order symmetric matrix, we have the following formula:

[0078]

[0079] The imaginary characteristic roots of the system appear in pairs in conjugate form. Here we only analyze the case when w>0. Multiply the left side of equation (9) by α H The following formula is obtained:

[0080]

[0081] Among them, α H is the conjugate matrix of α.

[0082] Since every row of the matrix on the left side of the equal sign in formula (9) is 0, jwα1=α2, substituting it into formula (10) yields:

[0083]

[0084] in, By changing w to -w, we can get the following formula:

[0085]

[0086] Taking the modulus on both sides of the equal sign of formula (12) we get:

[0087]

[0088] Among them, λ max is the largest characteristic root.

[0089] make That is, when w≤w max , the above formula holds.

[0090] From formula (12), we can get:

[0091]

[0092] Among them, θ(w) represents the principal value of the argument angle of the complex function, θ(w)∈[0, 2π).

[0093] make

[0094] Taking the derivative of τ(w) we get:

[0095]

[0096] Taking the derivative of M2(w) we get:

[0097]

[0098] So M2(w) is decreasing, so when w>0, M2(w)<M2(0)=0, so M1(w)<0, that is, τ(w) is also decreasing. So τ(w)≥τ(w max )=τ max When τ d <τ max When , we can get:

[0099]

[0100] That is, τ(w)≥τ(w max ), so when τ d <τ max This can keep the system characteristic roots in the left half plane, thereby achieving consistency in a multi-robot system with various time delays.

[0101] When the system delay τ d are all smaller than τ under the action of noise ζ(t) maxWhen the control quantity u i (t), the system can achieve consistency. The relationship between system delay and noise and position proportional coefficient and speed proportional coefficient is:

[0102]

[0103] Step 32, when the multi-robot system with multiple time delays is a directed graph and a spanning tree exists, The coefficient matrix Ψ has one double zero root, and the real parts of the other characteristic roots are negative. The frequency domain analysis method is also used for analysis. It is only necessary to consider when the system delay τ increases to what value under the action of noise ζ(t) so that the non-zero characteristic root of the multi-robot system with multiple delays appears on the imaginary axis for the first time. The delay τ at this time is the critical value for the system to remain stable. The modulus of both sides of the equal sign of equation (12) is obtained, that is:

[0104]

[0105] Let w be a function of ||F(w)||, then equation (19) can be simplified as:

[0106]

[0107] From formula (20), we can get that w is an increasing function of ||F(w)||. From formula (12), we can get:

[0108]

[0109] Among them, τ m represents the maximum value in τ. So:

[0110]

[0111] because so λ i is the non-zero characteristic root of the Laplace matrix L. So ||F(w)||≤||ζ(t)λ i ||. Since w is an increasing function of ||F(w)||, then:

[0112]

[0113] When τ d <τ max When , we can get:

[0114]

[0115] This contradicts formula (22). Therefore, when τ d <τ maxThis can keep the system characteristic roots within the left half plane, so that the multi-robot system with various time delays can achieve consistency.

[0116] When the system delay τ d are all smaller than τ under the action of noise ζ(t) max When, and Then in the control quantity u i (t), the system can achieve consistency. The relationship between system delay and noise and position proportional coefficient and speed proportional coefficient is:

[0117]

[0118] In the formula,

[0119] Embodiment 1

[0120] As attached Figure 2 As shown in Figure 1, the multi-robot system with multiple delays consists of 4 robots. The delay between robot 1 and robot 2 and robot 3 and robot 4 is τ1, and the delay between robot 1 and robot 4 and robot 2 and robot 3 is τ2. Assume that the adjacent communication weight a ij =1, then the Laplace matrix L is:

[0121]

[0122] Then max =4.

[0123] As attached Figure 3 As shown, it is assumed that the communication noise or the measurement noise is white noise with a maximum amplitude of 2.

[0124] Design k1 = 1.2, k2 = 1, then according to Theorem 1 we can get τ max =0.226.

[0125] As attached Figure 4 As shown, the simulation experiment is carried out through Matlab program.

[0126] As attached Figure 5-6 As shown, in the first group of experiments, τ1=0.21, τ2=0.22 are set, and the initial poses are assumed to be (1, 0), (2, 0), (3, 0), and (4, 0).

[0127] As attached Figure 7-8 As shown, in the second group of experiments, τ1=0.23, τ2=0.24 were set under the same conditions, and the other initial positions and initial velocities remained unchanged.

[0128] Comparing the first group of experiments with the second group of experiments, we can see that when all τ dare all less than τ max The multi-robot system with multiple time delays can reach consensus; when all τ d Both are greater than τ max When multiple robot systems contain multiple delays, they diverge and cannot reach consensus.

[0129] Embodiment 2

[0130] As attached Fig. 9 As shown in the figure, the multi-robot system with multiple delays consists of 4 robots. The delay from robot 1 to robot 2 is τ1, the delay from robot 2 to robot 3 is τ2, the delay from robot 3 to robot 4 is τ3, and the delay from robot 4 to robot 1 is τ4. Assume that the adjacent communication weight a ij =1, then the Laplace matrix L is:

[0131]

[0132] but so real(λ i ) represents λ i The real part, imag(λ i ) represents λ i Similar to Experiment 1, assuming that the communication noise or measurement noise is white noise with a maximum amplitude of 2, and setting k1 = 1, k2 = 1, then according to Theorem 2, we can get τ max .

[0133] As attached Fig.10 As shown, the simulation experiment is carried out through Matlab program.

[0134] As attached Figure 11-12 As shown, in the first group of experiments, τ1=0.13, τ2=0.12, τ3=0.11, τ4=0.1 are set, and the initial poses are assumed to be (1, 0), (2, 0), (3, 0), (4, 0).

[0135] As attached Figure 13-14 As shown, in the second group of experiments, under the same conditions, τ1=0.14, τ2=0.141, τ3=0.142, and τ4=0.143 were set.

[0136] Comparing the first group of experiments with the second group of experiments, we can see that when all τ d are all less than τ max The multi-robot system with multiple time delays can reach consensus; when all τ d Both are greater than τ max When multiple robot systems contain multiple delays, they diverge and cannot reach consensus.

[0137] Embodiment 3

[0138] In this embodiment, because the omnidirectional robot is a fully driven robot, the horizontal and vertical directions can be controlled separately, and it can be constructed into two one-dimensional second-order multi-robot systems, so the omnidirectional robot is used to verify the proposed algorithm. It should be pointed out that when applying the proposed algorithm to an actual multi-robot system, it is necessary to first determine the maximum delay of communication between robots and the maximum amplitude of the noise environment in which they are located, and then design the position proportional coefficient and the speed proportional coefficient based on this. It should also be noted that in the above-mentioned proposed algorithm, the collision avoidance behavior of the multi-robot system in the process of reaching consistency is not considered, only whether the system can reach consistency under the control law. Therefore, during the experiment, the collision avoidance algorithm program in the underlying control program of the platform robot is used. When the robot is about to collide, the formation algorithm program is interrupted and the collision avoidance behavior is executed. When the robots reach a safe distance, the formation algorithm program continues to be executed.

[0139] In this embodiment, the adjacent communication weight a ij =1, then The central processing unit remotely logs into each robot via SSH and uses the PING command to obtain the communication weight a ij The communication delay between two robots is not 0. In the actual communication environment, the delay between robots is time-varying. The maximum delay is taken, that is, τ a1 =0.056s, τ a2 =0.043s, τ a3 =0.047s, τ a4 = 0.061s, and the time taken by each robot to receive and process data is τ b1 =0.021s, τ b2 =0.02s, τ b3 =0.021s, τ b4 =0.021s, then the time delay between robots is τ1=0.077s, τ2=0.063s, τ3=0.068s, τ4=0.082s. Since the experiment is carried out in a laboratory environment in this embodiment, it is assumed that the communication noise is white noise with a maximum amplitude of 2. According to formula (25) and the moving speed of the omnidirectional robot, k1=1, k2=1.4 are designed. In the experiment, the four omnidirectional robots are placed in arbitrary positions, namely (0.83, 2.20, 0), (0.35, 1.74, 0), (0.67, 0.88, 0), and (0.56, 0.48, 0). The robots in the experiment in this embodiment cannot converge to one point, so formula (2) is changed to:

[0140]

[0141] Among them, F pis the formation parameter, p = 1, 2, ..., n. Since the system is only equipped with UWB ranging sensors to provide positioning for the robot, the robot will perform posture judgment before the experiment begins and keep the robot's body coordinate system consistent with the global coordinate system.

[0142] As attached Fig.15 As shown, it can be obtained from the experiments in the embodiment that under various time delay and noise environments, the multi-robot system can eventually reach a consensus and form a formation, which verifies the effectiveness of the proposed algorithm.

[0143] The present invention is not limited to the above-mentioned specific implementation modes. A person skilled in the art may implement the present invention in various other specific implementation modes according to the embodiments and the disclosure of the drawings. Therefore, any design that adopts the design structure and concept of the present invention and makes some simple transformations or changes shall fall within the scope of protection of the present invention.

Claims

1. A multi-robot formation control method under GPS denial and complex electromagnetic conditions, characterized in that: The multi-robot formation control method under GPS denial and complex electromagnetic conditions includes the following steps: Step 1, establish a general multi-robot system model; Step 2, adjusting the state vector model of the multi-robot formation system in a real environment to generate a multi-robot system with various time delays; Step 3, determine the maximum delay of communication between robots and the maximum amplitude of the noise environment, and then design the position proportional coefficient and speed proportional coefficient based on this; In step 3, when the multi-robot system with multiple time delays is a directed graph and a spanning tree exists, the coefficient matrix Ψ has one double zero root, and the real parts of other characteristic roots are negative; Consider the value to which the system delay increases under the action of noise ζ(t), so that the non-zero characteristic root of the multi-robot system with various delays appears on the imaginary axis for the first time. The delay at this time is the critical value for the system to remain stable. When τ d <τ max When , we can get: τ d is the system delay, τ max is the maximum system delay, k1 is the position proportional coefficient to be designed, k2 is the speed proportional coefficient to be designed, d is the robot number, D is the number of robots in the system, ζ(t) is the noise, λ i is the eigenvalue of the Laplace matrix L of the multi-robot system, α is the eigenvector corresponding to the eigenvalue, α H is the conjugate matrix of α, I2 is the second-order identity matrix, L d When the delay is τ d When is the Laplace matrix corresponding to the sub-topology graph composed of robot nodes, w is the angular frequency; When τ d <τ max This means that the system characteristic root can be kept in the left half plane, so that the multi-robot system with various time delays can achieve consistency; The relationship between the system delay and noise of the multi-robot system with multiple delays and the position proportional coefficient and the speed proportional coefficient is: in, w i represents the angular frequency w of the ith robot in the frequency domain analysis, λ i represents the nonzero eigenvalues ​​of the Laplace matrix L.

2. The multi-robot formation control method under GPS denial and complex electromagnetic conditions according to claim 1, characterized in that: In step 1, the general multi-robot system model is: in, represents the general multi-robot system model, Ψ is the coefficient matrix, is the Kronecker product, k1 is the position proportional coefficient, k2 is the velocity proportional coefficient, I represents the unit matrix, L represents the Laplace matrix; S(t) is the state vector of the multi-robot system: S(t)=[δ1(t),δ2(t),δ3(t),…,δ i (t)] Among them, δ i (t) is the state vector of the ith robot at time t: δ i (t)=[x i (t),v i (t)] T Among them, x i (t) is the position of the ith robot at time t, v i (t) is the velocity of the ith robot at time t; Among them, u i (t) is the input control variable of the ith robot at time t.

3. The multi-robot formation control method under GPS denial and complex electromagnetic conditions as claimed in claim 2, characterized in that: In step 2, the multi-delay multi-robot system is: Where ζ(t) is the communication noise or measurement noise between robots, τ d is the transmission delay, L d When the delay is τ d When the robot nodes form the sub-topology graph corresponding to the Laplace matrix, 4. The method for controlling a multi-robot formation under GPS denial and complex electromagnetic conditions as claimed in claim 3, characterized in that: In step 3, when the multi-robot system with multiple time delays is an undirected graph, the relationship between the system delay and noise of the multi-robot system with multiple time delays and the position proportional coefficient and the speed proportional coefficient is: Among them, τ max represents the maximum value of the transmission delay τ, w max represents the maximum value of the angular frequency w, λ max represents the largest eigenvalue of the Laplace matrix L.

Images