CN103412564B - A kind of unmanned systems distributed consensus formation control method and system thereof - Google Patents

Abstract

The invention discloses a method and system for unmanned system distributed consistent formation control. The method includes: step 1, setting an initial state for each node; step 2, according to the required formation formation and each node's In the initial state, get the relative position matrix of each node relative to the head node; step 3, set the communication power of each node so that it can only communicate with neighbor nodes; step 4, adjust the positions of all nodes in the node formation; step 5. According to the position of the head node of each node and the relative position matrix, the final required position of each node is obtained. The invention realizes the cooperative control of unmanned formation.

Description

A distributed consistent formation control method and system for unmanned systems

technical field

The invention relates to robot formation control technology in the field of information technology, in particular to a distributed consistent formation control method and system for an unmanned system.

Background technique

In recent years, with the development of computer technology and wireless communication technology, multi-robot coordination and cooperation has become possible and has been widely used. Coordination and cooperation of multiple robots can complete tasks that are difficult for a single robot. Among them, the formation problem is a typical problem in multi-robot coordination and cooperation. The so-called formation control refers to the control technology that multiple robots maintain a certain formation while adapting to environmental constraints during the process of reaching the destination.

The formation control of multi-robots is a hot research topic at home and abroad, and it is also the basis for studying other coordination and cooperation problems. Usually coordination is to resolve conflicts and contradictions between robots. For the formation problem of autonomous mobile robots, the conflict is mainly collision, that is to say, multiple robots cannot be in the same position at the same time. Collaboration means that robots cooperate to complete a task through a mechanism. For the formation problem of autonomous mobile robots, cooperation is to maintain formation, and the positions of each robot at each moment satisfy a mathematical relationship. For robot systems, maintaining a certain formation between multiple robots has many advantages. For example, the realization of a specific formation in the space structure can fully and effectively use multiple mobile robots to complete tasks, shorten the time for performing tasks, and reduce the cost of the system. , to improve the working efficiency of the system, to fully obtain the current environmental information, to enhance the ability to resist external attacks in an adversarial environment, and to improve robustness, etc.

Formation control has a wide range of applications in various fields such as military, entertainment, and production, especially in the military field, such as formation flight of spacecraft, unmanned aerial vehicles, and formation flight of autonomous underwater vehicles. Therefore, realizing a reasonable and effective formation control method will have important theoretical and practical significance.

Contents of the invention

The purpose of the present invention is to provide a distributed consistent formation control method and system for unmanned systems, which are used to realize the coordinated control of unmanned formations.

In order to achieve the above object, the present invention provides a distributed consistent formation control method for unmanned systems, characterized in that it includes:

Step 1, set an initial state for each node;

Step 2, according to the required formation formation and the initial state of each node, obtain the relative position matrix of each node relative to the head node;

Step 3, set the communication power of each node so that it can only communicate with neighbor nodes;

Step 4, adjusting the positions of all nodes in the node formation;

Step 5, according to the position of the head node of each node and the relative position matrix, the final required position of each node is obtained.

The formation control method, wherein, in the step 2, comprising: expressing the relative position matrix of each node in the N nodes with respect to the head node with the following formula:

${\mathrm{<span\; class="notranslate">\; S</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}\\ {\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}\\ {\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}\\ {\mathrm{<span\; class="notranslate">\; \&eta;</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; \&eta;</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}\end{array}\right]$

${\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span>\\ {\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span>\\ {\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span>\\ {\mathrm{<span\; class="notranslate">\; \&eta;</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span>\end{array}\right]<span\; class="notranslate">\; ,</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; ,</span><span\; class="notranslate">\; ...</span>\mathrm{<span\; class="notranslate">\; N</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; ></span>\mathrm{<span\; class="notranslate">\; 0</span>}<span\; class="notranslate">\; )</span>$

in:

S _{i, j} is the relative position coordinate and angle value of node j relative to node i;

X _i (t) is the position vector of node i at time t; x _i (t) is the coordinate of node i in the x-axis direction at time t; y _i (t) is the coordinate of node i in the y-axis direction at time t z _i (t) is the coordinate of node i in the z-axis direction at time t; η _i (t) is the angle value of node i relative to the horizontal plane at time t.

The formation control method, wherein, in the step 2, includes: in the formation with N nodes, one of the nodes is set as the head node, and the shape of the formation is represented by N-1 formation vector matrices, and the formation vector The form of the matrix is as follows:

${\mathrm{<span\; class="notranslate">\; S</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; S</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; S</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; ,</span><span\; class="notranslate">\; \&ForAll;</span>\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; j</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; \&Element;</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; ,</span><span\; class="notranslate">\; ...</span><span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; N</span>}$

S _i,i = [0,0,0,0] ^T

S _i,j =-S _j,i

in:

S _{i, n} is the relative position coordinate and angle value of node n relative to node i; S _{i, j} is the relative position coordinate and angle value of node j relative to node i; S _{j, i} is the relative position coordinate and angle value of node i relative to node j Relative position coordinates and angle values. S _i,i is the relative position coordinate and angle value of node i relative to node i.

The formation control method, wherein, in step 4, includes: adjusting the positions of all nodes in the node formation in a distributed consistency manner.

The formation control method, wherein, in the step 5, includes: obtaining the final required position of each node in the following manner:

$\underset{\mathrm{<span\; class="notranslate">\; j</span>}<span\; class="notranslate">\; \&Element;</span>{\mathrm{<span\; class="notranslate">\; A</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}}{<span\; class="notranslate">\; \&Sigma;</span>}{\mathrm{<span\; class="notranslate">\; c</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}$

${\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; d</span>}}<span\; class="notranslate">\; =</span><span\; class="notranslate">\; \&Sigma;</span>{\mathrm{<span\; class="notranslate">\; c</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; (</span>{\stackrel{<span\; class="notranslate">\; ^</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}}_{\mathrm{<span\; class="notranslate">\; j</span>}}^{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; S</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; )</span>$

in:

x _{i, d} is the final position of node i, c _{i, j} is a weighting coefficient greater than 0, A _i is the set of neighbor nodes of node i, ${\hat{x}}_j^i$ is the position of node j obtained by node i, and S _i,j is the relative position matrix of each node relative to the head node.

In order to achieve the above object, the present invention provides a distributed consistent formation control system for unmanned systems, characterized in that it includes:

A state setting module is used to set an initial state for each node;

The matrix acquisition module is connected to the state setting module, and is used to obtain the relative position matrix of each node relative to the head node according to the required formation formation and the initial state of each node;

The power setting module is used to set the communication power of each node so that it can only communicate with neighbors;

A node adjustment module, connected to the matrix acquisition module and the power setting module, is used to adjust the positions of all nodes in the node formation;

The node control module is connected to the matrix acquisition module and the node adjustment module, and is used to obtain the final required position of each node according to the head node position of each node and the relative position matrix.

The formation control system, wherein the matrix acquisition module represents the relative position matrix of each node in the N nodes with respect to the head node with the following formula:

${\mathrm{<span\; class="notranslate">\; S</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}\\ {\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}\\ {\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}\\ {\mathrm{<span\; class="notranslate">\; \&eta;</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; \&eta;</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}\end{array}\right]$

${\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span>\\ {\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span>\\ {\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span>\\ {\mathrm{<span\; class="notranslate">\; \&eta;</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span>\end{array}\right]<span\; class="notranslate">\; ,</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; ,</span><span\; class="notranslate">\; ...</span>\mathrm{<span\; class="notranslate">\; N</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; ></span>\mathrm{<span\; class="notranslate">\; 0</span>}<span\; class="notranslate">\; )</span>$

in:

S _{i, j} is the relative position coordinate and angle value of node j relative to node i;

X _i (t) is the position vector of node i at time t; x _i (t) is the coordinate of node i in the x-axis direction at time t; y _i (t) is the coordinate of node i in the y-axis direction at time t z _i (t) is the coordinate of node i in the z-axis direction at time t; η _i (t) is the angle value of node i relative to the horizontal plane at time t.

The formation control system, wherein, the matrix acquisition module sets one of the nodes as the head node in the formation with N nodes, and represents the shape of the formation through N-1 formation vector matrices, and the formation vector matrix is in the form of as follows:

${\mathrm{<span\; class="notranslate">\; S</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; S</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; S</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; ,</span><span\; class="notranslate">\; \&ForAll;</span>\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; j</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; \&Element;</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; ,</span><span\; class="notranslate">\; ...</span><span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; N</span>}$

S _i,i = [0,0,0,0] ^T

S _i,j =-S _j,i

in:

S _{i, n} is the relative position coordinate and angle value of node n relative to node i; S _{i, j} is the relative position coordinate and angle value of node j relative to node i; S _{j, i} is the relative position coordinate and angle value of node i relative to node j Relative position coordinates and angle values. S _i,i is the relative position coordinate and angle value of node i relative to node i.

In the formation control system, the node adjustment module adjusts the positions of all nodes in the node formation in a distributed consistency manner.

The formation control system, wherein the node control module obtains the final required position of each node in the following manner:

$\underset{\mathrm{<span\; class="notranslate">\; j</span>}<span\; class="notranslate">\; \&Element;</span>{\mathrm{<span\; class="notranslate">\; A</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}}{<span\; class="notranslate">\; \&Sigma;</span>}{\mathrm{<span\; class="notranslate">\; c</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}$

${\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; d</span>}}<span\; class="notranslate">\; =</span><span\; class="notranslate">\; \&Sigma;</span>{\mathrm{<span\; class="notranslate">\; c</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; (</span>{\stackrel{<span\; class="notranslate">\; ^</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}}_{\mathrm{<span\; class="notranslate">\; j</span>}}^{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; S</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; )</span>$

in:

x _{i, d} is the final state of node i, c _{i, j} is a weighting coefficient greater than 0, A _i is the set of neighbor nodes of node i, is the position of node j obtained by node i, and S _i,j is the relative position matrix of each node relative to the head node.

Compared with the prior art, the beneficial technical effect of the present invention is:

(1) Completely distributed, the algorithm is executed asynchronously.

(2) The calculation is simple and the real-time performance is good.

(3) High adaptability, suitable for dynamic network.

(4) Reduce network transmission delay.

(5) Mitigate the impact of communication failures and fast motion on overall performance.

The invention introduces the distributed consensus algorithm into the robot formation control, and studies how to realize the cooperative control of the unmanned formation based on the distributed consensus method. This method introduces the distributed consensus algorithm in the one-dimensional space into the four-dimensional space, and uses the characteristic that the distributed consensus algorithm can converge within a limited time to realize the distributed formation control of the unmanned system. Therefore, only the neighbor node information in the network can be used to make the entire network converge to the same state, which greatly reduces the problems of communication delays and system crashes caused by node failures.

Description of drawings

Fig. 1 is the flow chart of the unmanned system distributed consistent formation control method of the present invention;

Fig. 2 is a structural diagram of the distributed consistent formation control system of the unmanned system of the present invention.

detailed description

The present invention will be described in detail below in conjunction with the accompanying drawings and specific embodiments, but not as a limitation of the present invention. (Please carefully confirm the correctness of all the following contents)

As shown in FIG. 1 , it is a flow chart of the distributed consistent formation control method of the unmanned system of the present invention. The specific steps of the process are as follows:

Step 101, setting an initial state for each node, this initial state can be specified arbitrarily, refer to formula 1;

Step 102, according to the required formation and the initial state of each node, obtain the position matrix of each node relative to the head node, that is, the relative position matrix, referring to formula 2;

Step 103, setting the communication transmission power of each node so that it can only communicate with neighbors, so that the distributed characteristics of this method can be fully reflected;

Step 104, use the distributed consensus algorithm (refer to formulas 3, 4, 5) to make all nodes in the node array adjust their positions in space, that is, x _i,d at a certain moment, refer to formula 5;

Step 105, according to the position of the head node obtained by each node and the position matrix relative to the head node, obtain the final required position of each node, refer to formula 5.

In the present invention, the position of the node in space is described by the angle of X, Y, Z and η relative to the horizontal plane, as shown in the following formula 1. Among them, Xi (t) is the position vector of node i at time t; x _i (t) is the coordinate of node _{i in the x-axis direction at time t; y i (} t) is the coordinate of node i in the y-axis direction at time t z _i (t) is the coordinate of node i in the z-axis direction at time t; η _i (t) is the angle value of node i relative to the horizontal plane at time t.

${\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span>\\ {\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span>\\ {\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span>\\ {\mathrm{<span\; class="notranslate">\; \&eta;</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span>\end{array}\right]<span\; class="notranslate">\; ,</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; ,</span><span\; class="notranslate">\; ...</span>\mathrm{<span\; class="notranslate">\; N</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; ></span>\mathrm{<span\; class="notranslate">\; 0</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span>$

The shape of a formation can be described by the relative position vectors and relative angles of a set of nodes. In the present invention, the 4X1 formation vector matrix S _{i, j} of the following formula 2 is used to describe the relative position coordinates and angle values of two nodes, that is, S _{i, j} is the relative position coordinates and angle values of node j relative to node i, namely Relative position matrix.

${\mathrm{<span\; class="notranslate">\; S</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}\\ {\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}\\ {\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}\\ {\mathrm{<span\; class="notranslate">\; \&eta;</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; \&eta;</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}\end{array}\right]<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; )</span>$

Among them, x _i is the coordinate of node i in the x-axis direction, x _j is the coordinate of node j in the x-axis direction, y _i is the coordinate of node i in the y-axis direction, y _j is the node j in the y-axis direction z _i is the coordinate of node i in the z-axis direction, z _j is the coordinate of node j in the z-axis direction, η _i is the angle value of node i relative to the horizontal plane, η _j is the angle value of node j relative to the horizontal plane The angle value of , the above parameters all correspond to a certain moment (such as t moment), in order to simplify the expression of the formula, this moment is omitted, and the following expression is the same.

In a formation with N nodes, there are N formation vector matrices as in formula 2, and these matrices have the following properties:

${\mathrm{<span\; class="notranslate">\; S</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; S</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; S</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; ,</span><span\; class="notranslate">\; \&ForAll;</span>\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; j</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; \&Element;</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; ,</span><span\; class="notranslate">\; ...</span><span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; N</span>}$

S _i,i = [0,0,0,0] ^T (3)

S _i,j =-S _j,i

Among them, S _{i, n} is the relative position coordinate and angle value of node n relative to node i; S _{i, j} is the relative position coordinate and angle value of node j relative to node i; S _{j, i} is the relative position coordinate and angle value of node i relative to node j The relative position coordinates and angle values of . S _i,i is the relative position coordinate and angle value of node i relative to node i.

If a node is designated as the head node, then a (N-1)X4 oblique symmetric vector matrix (N-1 formation vector matrix S _i,j ) can be used to describe the shape of the formation, and the oblique symmetric vector matrix is used to represent N position of a node. Different from other formation control methods, the nodes in this method can only communicate with their own neighbor nodes, so that node i can obtain at most N-1 formation vector matrices S _i,j ,j∈A _i , where A _i is the node The set of neighbor nodes of i. Assume that node i obtains an estimated formation vector matrix for node j through communication or direct estimation and the position of node j Since the neighbor nodes of node i are not at the same distance from i, the quality of communication between them and node i is also different. Give node i an estimate of all its neighbor nodes Choose a weighting coefficient c _i,j greater than 0, which satisfies the following conditions:

$\underset{\mathrm{<span\; class="notranslate">\; j</span>}<span\; class="notranslate">\; \&Element;</span>{\mathrm{<span\; class="notranslate">\; A</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}}{<span\; class="notranslate">\; \&Sigma;</span>}{\mathrm{<span\; class="notranslate">\; c</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 4</span>}<span\; class="notranslate">\; )</span>$

Here, formula 5 is used to describe the final state x _i,d of node i (that is, the required state of node i):

${\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; d</span>}}<span\; class="notranslate">\; =</span><span\; class="notranslate">\; \&Sigma;</span>{\mathrm{<span\; class="notranslate">\; c</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; (</span>{\stackrel{<span\; class="notranslate">\; ^</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}}_{\mathrm{<span\; class="notranslate">\; j</span>}}^{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; S</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 5</span>}<span\; class="notranslate">\; )</span>$

It can be seen from formula 5 that the trajectory of node i can be obtained only by obtaining the state of x _{i ,d} from the position of Xi of node i. Keep the entire formation in the required formation when moving according to the established trajectory. Due to the acquisition of _xi,d , nodes only need to communicate with their neighbor nodes, so even if a node fails or the communication link is disconnected, the accuracy of _xi,d is not greatly affected.

As shown in FIG. 2 , it is a structural diagram of the distributed consistent formation control system of the unmanned system of the present invention. With reference to Fig. 1, the formation control system 200 specifically includes:

State setting module 10, is used for setting an initial state for each node;

Matrix acquisition module 20, connection state setting module 10, is used to obtain the position matrix of each node relative to the head node, i.e. the relative position matrix, according to the required formation formation and the initial state of each node;

The power setting module 30 is used to set the communication transmission power of each node so that it can only communicate with neighbor nodes;

The node adjustment module 40, the connection matrix acquisition module 20, and the power setting module 30 are used to use the distributed consensus algorithm to make all nodes in the node formation adjust their positions in space, that is, x _i,d at a certain moment , refer to formula 5;

The node control module 50, the connection matrix acquisition module 20, and the node adjustment module 40 are used to obtain the final required position of each node according to the head node position and relative position matrix obtained by each node, refer to formula 5 above.

The matrix acquisition module 20 designates a node as the head node in a formation with N nodes, then a (N-1)X4 oblique symmetric vector matrix (N-1 formation vector matrix S _i,j ) can be used to describe this The shape of the formation, a skew-symmetric vector matrix is used to represent the positions of the N nodes.

In order to make the object, technical solution and advantages of the present invention clearer, the present invention will be further described in detail below in conjunction with the specific implementation process and results of the MK2.0 quadrotor aircraft of the present invention, and with reference to the accompanying drawings.

The following implementation process is in the MK2.0 quadrotor aircraft (hereinafter referred to as the node), using IOT-NODE433 to realize the communication between the nodes. The main flow of the formation control of the distributed consensus algorithm of the present invention will be further described in detail below in conjunction with specific embodiments and with reference to the accompanying drawings 1 and 2 . The method includes the following main steps:

Step A: First, refer to Formula 1 to set an initial state XI for each node;

$\mathrm{<span\; class="notranslate">\; x</span>}\mathrm{<span\; class="notranslate">\; I</span>}<span\; class="notranslate">\; =</span>\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}\\ {\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}\\ {\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}\\ {\mathrm{<span\; class="notranslate">\; \&eta;</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}\end{array}\right]<span\; class="notranslate">\; ,</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; ,</span><span\; class="notranslate">\; ...</span>\mathrm{<span\; class="notranslate">\; N</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; .</span>$

Step B: According to the required formation and the initial state of each node, obtain the position matrix S _i,j of each node relative to the head node, where it is assumed that node i is the head node.

Step C: Set the transmit power of each node so that it can only communicate with its neighbors.

Step D: Use the distributed consensus algorithm to enable all nodes in the node formation to adjust their positions in space, that is, x _i,d at a certain moment, refer to formula 5; the proposal of the distributed consensus algorithm effectively promotes development of formation control. The distributed consensus algorithm is that multiple nodes in the node array have an initial time, and each node can only communicate with its own neighbors, and each node adjusts itself by continuously running the distributed consensus algorithm with its neighbor nodes The local time of , all nodes in this node array can reach the same time.

The distributed consensus algorithm only analyzes a 1-dimensional variable, and the present invention extends the 1-dimensional variable in the distributed consensus algorithm to a 4-dimensional variable, allowing each node to run the distributed consensus algorithm.

Step E: Each node can get the position of the head node, and according to the relative position matrix, refer to formula 2, you can get the final required position of each node, refer to formula 5, so as to ensure that the entire formation is moving. The relative position remains unchanged.

The present invention can regard the formation composed of multiple robots as a distributed system. In the distributed system based on the message passing communication model, the following errors will inevitably occur: the process may be slow, crashed, restarted, and the message may be delayed, lost, repeat. Traditional robot formation control methods include: behavior-based method, artificial potential field method, follow-lead method, cycle method, etc. These traditional methods have the following problems:

(1) Based on a certain topology, once the communication link is disturbed, the control is interrupted.

(2) Centralized or partially centralized structure, the roles of nodes are different, and the failure of key nodes leads to the failure of the algorithm protocol.

(3) The control protocol is complicated, the calculation time is long, and the real-time performance is poor.

The problem solved by the distributed consensus algorithm is how to reach a consensus on a certain value in a distributed system where the above abnormalities may occur, so as to ensure that no matter any of the above abnormalities occur, the consistency of the resolution will not be destroyed. For example, in a distributed database system, if the initial state of each node is consistent, and each node performs the same sequence of operations, then they can finally get a consistent state. In order to ensure that each node executes the same sequence of commands, a consensus algorithm needs to be executed on each instruction to ensure that the instructions seen by each node are consistent.

Compared with traditional methods, the distributed consensus algorithm proposed by the present invention has the following advantages for robot formation control:

(1) Completely distributed, the algorithm is executed asynchronously.

(2) The calculation is simple and the real-time performance is good.

(3) High adaptability, suitable for dynamic network.

(4) Reduce network transmission delay.

(5) Mitigate the impact of communication failures and fast motion on overall performance.

The invention introduces the distributed consensus algorithm into the robot formation control, and studies how to realize the cooperative control of the unmanned formation based on the distributed consensus method. This method can only use the information of neighbor nodes in the network to make the entire network converge to the same state, which greatly reduces the problems of communication delays and system crashes caused by node failures.

Of course, the present invention can also have other various embodiments, and those skilled in the art can make various corresponding changes and deformations according to the present invention without departing from the spirit and essence of the present invention. All changes and deformations should belong to the protection scope of the appended claims of the present invention.

Claims (4)

1. A distributed consistent formation control method for unmanned systems, characterized in that it comprises: Step 1, set an initial state for each node; Step 2, according to the required formation formation and the initial state of each node, obtain the relative position matrix of each node relative to the head node; Step 3, set the communication power of each node so that it can only communicate with neighbor nodes; Step 4, adjusting the positions of all nodes in the node formation; Step 5, according to the position of the head node of each node and the relative position matrix, obtain the final required position of each node; In the step 2, including: expressing the relative position matrix of each node in the N nodes with respect to the head node with the following formula: ${\mathrm{<span\; class="notranslate">\; S</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}\\ {\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}\\ {\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}\\ {\mathrm{<span\; class="notranslate">\; \&eta;</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; \&eta;</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}\end{array}\right]$ ${\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span>\\ {\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span>\\ {\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span>\\ {\mathrm{<span\; class="notranslate">\; \&eta;</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span>\end{array}\right]<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; ,</span><span\; class="notranslate">\; ...</span>\mathrm{<span\; class="notranslate">\; N</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; ></span>\mathrm{<span\; class="notranslate">\; 0</span>}$ in: S _{i, j} is the relative position coordinate and angle value of node j relative to node i; X _i (t) is the position vector of node i at time t; x _i (t) is the coordinate of node i in the x-axis direction at time t; y _i (t) is the coordinate of node i in the y-axis direction at time t coordinates; z _i (t) is the coordinates of node i in the z-axis direction at time t; η _i (t) is the angle value of node i at time t relative to the horizontal plane; In the step 2, including: in the formation with N nodes, one of the nodes is set as the head node, and the shape of the formation is represented by N-1 formation vector matrices. The form of the formation vector matrix is as follows: ${\mathrm{<span\; class="notranslate">\; S</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; S</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; S</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; ,</span><span\; class="notranslate">\; \&ForAll;</span>\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; j</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; \&Element;</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; ,</span><span\; class="notranslate">\; ...</span><span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; N</span>}$ S _i,i = [0,0,0,0] ^T S _i,j =-S _j,i in: S _{i, n} is the relative position coordinate and angle value of node n relative to node i; S _{i, j} is the relative position coordinate and angle value of node j relative to node i; S _{j, i} is the relative position coordinate and angle value of node i relative to node j Relative position coordinates and angle values, S _i,i is the relative position coordinates and angle values of node i relative to node i; In said step 5, comprising: obtaining the final required position of each node in the following manner: $\underset{\mathrm{<span\; class="notranslate">\; j</span>}<span\; class="notranslate">\; \&Element;</span>{\mathrm{<span\; class="notranslate">\; A</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}}{<span\; class="notranslate">\; \&Sigma;</span>}{\mathrm{<span\; class="notranslate">\; c</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}$ ${\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; d</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; \&Sigma;c</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; (</span>{\stackrel{<span\; class="notranslate">\; ^</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}}_{\mathrm{<span\; class="notranslate">\; j</span>}}^{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; S</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; )</span>$ in: x _{i, d} is the final position of node i, c _{i, j} is a weighting coefficient greater than 0, A _i is the set of neighbor nodes of node i, is the position of node j obtained by node i, and S _i,j is the relative position matrix of each node relative to the head node. 2. The formation control method according to claim 1, characterized in that, in step 4, comprising: adjusting the positions of all nodes in the node formation in a distributed consistency manner. 3. A distributed consistent formation control system for unmanned systems, characterized in that it comprises: A state setting module is used to set an initial state for each node; The matrix acquisition module is connected to the state setting module, and is used to obtain the relative position matrix of each node relative to the head node according to the required formation formation and the initial state of each node; The power setting module is used to set the communication power of each node so that it can only communicate with neighbors; A node adjustment module, connected to the matrix acquisition module and the power setting module, is used to adjust the positions of all nodes in the node formation; The node control module is connected to the matrix acquisition module and the node adjustment module, and is used to obtain the final required position of each node according to the head node position and the relative position matrix of each node; The matrix acquisition module expresses the relative position matrix of each node in the N nodes with respect to the head node with the following formula: ${\mathrm{<span\; class="notranslate">\; S</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}\\ {\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}\\ {\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}\\ {\mathrm{<span\; class="notranslate">\; \&eta;</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; \&eta;</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}\end{array}\right]$ ${\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span>\\ {\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span>\\ {\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span>\\ {\mathrm{<span\; class="notranslate">\; \&eta;</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span>\end{array}\right]<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; ,</span><span\; class="notranslate">\; ...</span>\mathrm{<span\; class="notranslate">\; N</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; ></span>\mathrm{<span\; class="notranslate">\; 0</span>}$ in: S _{i, j} is the relative position coordinate and angle value of node j relative to node i; X _i (t) is the position vector of node i at time t; x _i (t) is the coordinate of node i in the x-axis direction at time t; y _i (t) is the coordinate of node i in the y-axis direction at time t coordinates; z _i (t) is the coordinates of node i in the z-axis direction at time t; η _i (t) is the angle value of node i relative to the horizontal plane at time t; The matrix acquisition module sets one of the nodes as the head node in the formation of N nodes, and represents the shape of the formation by N-1 formation vector matrices, and the formation vector matrix is in the form of: ${\mathrm{<span\; class="notranslate">\; S</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; S</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; S</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; ,</span><span\; class="notranslate">\; \&ForAll;</span>\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; j</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; \&Element;</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; ,</span><span\; class="notranslate">\; ...</span><span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; N</span>}$ S _i,i = [0,0,0,0] ^T S _i,j =-S _j,i in: S _{i, n} is the relative position coordinate and angle value of node n relative to node i; S _{i, j} is the relative position coordinate and angle value of node j relative to node i; S _{j, i} is the relative position coordinate and angle value of node i relative to node j Relative position coordinates and angle values, S _i,i is the relative position coordinates and angle values of node i relative to node i; Described node control module obtains the final required position of each node in the following manner: $\underset{\mathrm{<span\; class="notranslate">\; j</span>}<span\; class="notranslate">\; \&Element;</span>{\mathrm{<span\; class="notranslate">\; A</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}}{<span\; class="notranslate">\; \&Sigma;</span>}{\mathrm{<span\; class="notranslate">\; c</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}$ ${\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; d</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; \&Sigma;c</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; (</span>{\stackrel{<span\; class="notranslate">\; ^</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}}_{\mathrm{<span\; class="notranslate">\; j</span>}}^{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; S</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; )</span>$ in: x _{i, d} is the final state of node i, c _{i, j} is a weighting coefficient greater than 0, A _i is the set of neighbor nodes of node i, is the position of node j obtained by node i, and S _i,j is the relative position matrix of each node relative to the head node. 4. The formation control system according to claim 3, wherein the node adjustment module adjusts the positions of all nodes in the node formation in a distributed consistency manner.