Static target distributed positioning and formation control method based on local azimuth information

Abstract

The invention discloses a static target distributed positioning and formation control method based on local azimuth information, belongs to the technical field of multi-agent positioning and formation control, and solves the problem of how to realize rapid convergence formation control of a multi-agent system under a local reference framework. The method comprises the following steps: establishing a local reference framework of an agent by adopting a Henneberg configuration based on a directed topological graph, constructing a single integrator agent model, and constructing a mathematical reference model for realizing preset time positioning and formation; designing a distributed orientation estimator for estimating the azimuth angle of the intelligent agent; designing an initial position estimator for estimating initial position information of the following agent; a specified time distributed formation controller is designed, so that a follower accurately moves to an expected position according to global position information of a leader and given ideal azimuth information; according to the invention, distributed positioning estimation and formation control are carried out only based on the local azimuth angle, and rapid convergence formation control of a multi-agent system under a local reference framework can be realized.

Description

Technical Field

[0001] The present invention belongs to the technical field of multi-agent positioning and formation control, and relates to a static target distributed positioning and formation control method based on local orientation information. Background Art

[0002] Positioning estimation and formation control are two key research topics in the consensus collaborative control of multi-agent systems, and have received widespread attention from academia and industry. According to the different information processing and storage methods, the existing positioning and formation control algorithms can be divided into centralized methods and distributed methods. Distributed methods are more popular than centralized methods due to their advantages in reliability, safety, efficiency, energy consumption, etc. Distributed positioning, that is, ensuring that each agent can estimate its own global position by designing an estimation algorithm. Accurate positioning is a prerequisite for multi-agent systems to perform complex tasks. Distributed formation, that is, driving all agents, such as drones, unmanned vehicles, and unmanned boats, to form the desired formation by designing control algorithms. However, in the exploration of the integration of positioning estimation and formation control of multi-agent systems, there are a series of challenges in the study of local information interaction and group relations of agents. Therefore, the integration of positioning and formation has become the focus of current researchers.

[0003] In the past three decades, researchers in academia and industry have conducted systematic research on distributed positioning based on measurement information and proposed many classic positioning methods. With the rapid development of sensors such as optical cameras and lidar, orientation information has become easy to obtain and has high accuracy, so distributed positioning algorithms based on orientation information have received more and more attention. Although a lot of progress has been made in pure orientation formation control, there are still many important problems in this field that have not been solved: the existing orientation-based formation control algorithms are all proposed under the condition that the global reference frame is known, and the global azimuth information is used in the control algorithm; in actual applications, each agent has its own local reference frame, and the acquisition of local azimuth information is much easier than that of global azimuth information, which is more in line with actual application needs. Therefore, the distributed positioning estimation and formation control of multi-agent systems under the local reference frame is a topic worthy of in-depth study. Summary of the invention

[0004] The technical solution of the present invention is used to solve the problem of how to achieve fast convergence formation control of a multi-agent system under a local reference frame.

[0005] The present invention solves the above technical problems through the following technical solutions:

[0006] The method for distributed positioning and formation control of static targets based on local position information comprises the following steps:

[0007] Step 1: Based on the directed topological graph, the Henneberg configuration is used to establish the local reference frame of the intelligent agent, construct a single integrator intelligent agent model, and construct a mathematical reference model to achieve preset time positioning and formation;

[0008] Step 2: Design a distributed orientation estimator to estimate the orientation of the agent;

[0009] Step 3: Design an initial position estimator to estimate the initial position information of the following agent;

[0010] Step 4: Design a time-specified distributed formation controller so that the followers can move accurately to the desired position based on the leader's global position information and the given ideal orientation information.

[0011] Furthermore, the method of establishing a local reference frame of an intelligent agent based on a directed topological graph using a Henneberg configuration is specifically as follows:

[0012] The communication topology and relative position relationship between agents are represented by a directed graph To represent the directed graph Contains a finite set of vertices and a finite set of edges, where the vertex set ν={ν1,ν2,…,ν n}, edge set If (ν α ,ν β )∈ε, representing the agent ν α is the agent ν β Neighbors, that is

[0013] Directed topology graph By agent ν i and ν k and a directed edge (ν k ,ν i ); a new directed topological graph In the figure By adding new agents and splitting directed edges;

[0014] Consider a system of N agents on a two-dimensional plane, each of which has its own local reference frame and is related to the global reference frame g Σ is not aligned, the perception variable measured by the agent using the sensor is the local azimuth; definition i Σ is the local reference frame of follower agent i, θ i ∈[0,2π] is the agent i relative to the global frame g The azimuth of Σ; the displacement vector between agents i and k is defined as p ik =p k -pi , where p k and p i Represent the global coordinate values ​​of agents k and i respectively; is the unit direction vector between agents i and k;

[0015] Agent i in its local reference frame i The relative position vector of agent k measured under Σ is expressed as definition Local reference frame i Σ relative to the global reference frame g Σ rotation matrix; Based on the rotation matrix, the direction vectors of agent i and agent k in the global frame are obtained For the unit direction vector g ik , the orthographic projection matrix Defined as: Where I is the identity matrix; the orthogonal projection matrix Project any vector to vector g ik On the orthogonal complement vector of is a positive semidefinite matrix and satisfies Q gik g ik =0.

[0016] Furthermore, the single integrator agent model is constructed as follows:

[0017]

[0018] Among them, p i =[x i ,y i ]∈R 2 represents the coordinates of agent i in the global reference frame, u i It is the control input that needs to be designed.

[0019] Furthermore, the control input that needs to be designed is the speed of the agent.

[0020] Furthermore, the method for constructing a mathematical reference model for realizing preset time positioning and formation is as follows:

[0021] A continuously differentiable function f(t) is selected as a mathematical reference model for realizing preset time positioning and formation. The formula of the continuously differentiable function f(t) is as follows:

[0022]

[0023] Among them, (·)′ represents the first-order derivative, d1, t∈[t0,∞), τ>0, μ(t) is a time-varying function, which makes the continuously differentiable function f(t) converge to zero after the specified convergence time T.

[0024] Furthermore, the distributed position estimator is designed as follows:

[0025]

[0026] in, is the estimated global unit vector between agents j and k, is the orthographic projection matrix, is the local position information directly measured in the local reference frame of agent i, d2, Indicates a local reference i Σ relative to the global reference frame g The estimated rotation matrix of Σ.

[0027] Furthermore, the initial position estimator is designed as follows:

[0028]

[0029] in, is the estimated initial position information of agent i, and are the estimated initial position information of the two neighbors j and k of agent i, respectively. and are all orthogonal projection matrices, d3,

[0030] Furthermore, the designated time distributed formation controller is designed as follows:

[0031]

[0032] in, and is the constant expected approaching direction, and are all orthographic projection matrices, is the estimated global unit vector between agents i and j, is the estimated global unit vector between agents j and k, d4,

[0033] The present invention also provides an electronic device, comprising a memory and a processor, wherein the memory is used to store a program that supports the processor to execute the above-mentioned static target distributed positioning and formation control method based on local orientation information, and the processor is configured to execute the program stored in the memory.

[0034] The present invention also provides a storage medium having a computer program stored thereon. When the computer program is executed by a processor, the steps of the above-mentioned static target distributed positioning and formation control method based on local orientation information are executed.

[0035] The advantages of the present invention are:

[0036] The technical solution of the present invention adopts Henneberg configuration based on a directed topological graph to establish a local reference frame of an intelligent agent, constructs a single integrator intelligent agent model, and constructs a mathematical reference model for realizing preset time positioning and formation; a distributed azimuth estimator is designed to estimate the azimuth of the intelligent agent; an initial position estimator is designed to estimate the initial position information of the following intelligent agent; a designated time distributed formation controller is designed to enable the follower to accurately move to the desired position according to the global position information of the leader and the given ideal azimuth information; the control method of the present invention can perform distributed positioning estimation and formation control based only on the local azimuth, and in view of the fact that the formation control of a multi-agent system often has certain requirements on the task time, the fast convergence formation control of the multi-agent system under the local reference frame can be realized, and the constraint that the initial azimuth is less than π and the Gram-Schmidt orthogonalization is not required are eliminated. BRIEF DESCRIPTION OF THE DRAWINGS

[0037] Figure 1 This is an overall flow chart of a method for distributed positioning and formation control of static targets based on local position information according to a first embodiment of the present invention;

[0038] Figure 2 A schematic diagram of a Henneberg configuration of a static target distributed positioning and formation control method based on local position information according to Embodiment 1 of the present invention;

[0039] Figure 3 This is an example diagram of local orientation measurement of a static target distributed positioning and formation control method based on local orientation information according to the first embodiment of the present invention;

[0040] Figure 4 It is a convergence trajectory diagram of the estimated starting position of the static target distributed positioning and formation control method based on local orientation information according to the first embodiment of the present invention;

[0041] Figure 5 A curve diagram of the azimuth estimation error of the static target distributed positioning and formation control method based on local azimuth information according to the first embodiment of the present invention;

[0042] Figure 6 A graph showing the error in the initial position estimation of the static target distributed positioning and formation control method based on local position information according to the first embodiment of the present invention;

[0043] Figure 7This is a diagram of a static agent formation process of a static target distributed positioning and formation control method based on local position information according to the first embodiment of the present invention;

[0044] Figure 8 It is a curve diagram of the intelligent agent control error of the static target distributed positioning and formation control method based on local position information according to the first embodiment of the present invention;

[0045] Fig. 9 A communication topology diagram between UAVs of the static target distributed positioning and formation control method based on local position information according to the first embodiment of the present invention;

[0046] Fig.10 This is a screenshot of the UAV formation formation process of the static target distributed positioning and formation control method based on local orientation information according to the first embodiment of the present invention. DETAILED DESCRIPTION

[0047] In order to make the purpose, technical solution and advantages of the embodiments of the present invention clearer, the technical solution in the embodiments of the present invention will be clearly and completely described below in combination with the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0048] The technical solution of the present invention is further described below in conjunction with the accompanying drawings and specific embodiments:

[0049] Embodiment 1

[0050] like Figure 1 As shown, it is an overall flow chart of the static target distributed positioning and formation control method based on local position information according to an embodiment of the present invention, which includes the following contents:

[0051] 1. Based on the directed topological graph, the Henneberg configuration is used to establish the local reference frame of the intelligent agent, build a single integrator intelligent agent model, and build a mathematical reference model to achieve preset time positioning and formation

[0052] The communication topology and relative position relationship between agents can be represented by a directed graph To represent a directed graph Contains a finite set of vertices and a finite set of edges, where the vertex set ν={ν1,ν2,…,ν n}, edge set If (ν α ,ν β )∈ε, which means the agent ν α is the agent νβ Neighbors, that is Specifically, agent ν β We can observe the agent ν α , and can receive agent ν α Transmitted information.

[0053] The embodiment of the present invention is based on a directed topological graph and adopts the Henneberg configuration. By agent ν i and ν k and a directed edge (ν k ,ν i ). A new directed topological graph Can be found in Figure It is generated by adding new agents and splitting directed edges. Figure 2 An example of a Henneberg configuration is given in , where the red dots represent the leader agent and the dots of other colors are the follower agents. For example, consider a multi-agent system consisting of ten agents as the simulation object, where two agents are leaders and the remaining eight agents are followers.

[0054] Consider a system of N agents on a two-dimensional plane, each of which has its own local reference frame and is related to the global reference frame g Σ is not aligned, and the perception variable measured by the agent using the sensor is the local azimuth. Definition i Σ is the local reference frame of follower agent i, θ i ∈[0,2π] is the agent i relative to the global frame g The displacement vector between agents i and k is defined as p ik =p k -p i , where p k and p i Represent the global coordinate values ​​of agents k and i respectively. is the unit direction vector between agents i and k.

[0055] Agent i in its local reference frame i The relative position vector of agent k measured under Σ can be expressed as definition Local reference frame i Σ relative to the global reference frame g Based on the rotation matrix, we can get the direction vectors of agent i and agent k in the global frame. For the unit direction vector g ik , the orthographic projection matrix Defined as: Where I is the identity matrix. Orthogonal projection matrix You can project any vector onto the vector g ik Orthogonal projection matrix is a positive semidefinite matrix and satisfies An example of local orientation measurement is shown in the figure below. Figure 3 shown.

[0056] A single integrator agent model is constructed, and the single integrator agent model is as follows:

[0057]

[0058] Among them, p i =[x i ,y i ]∈R 2 represents the coordinates of agent i in the global reference frame, u i It is the control input that needs to be designed, generally representing the speed of the agent.

[0059] Each follower agent has its own local reference frame, and the only measurement information it can obtain is the local azimuth. For follower agent i, its initial coordinates p i =[x i ,y i ] is unknown. At the same time, the angle between the reference frame of agent i and the global reference frame, i.e., the azimuth angle θ i , is also unknown.

[0060] Construct a mathematical reference model to achieve preset time positioning and formation, the method is as follows:

[0061] A continuously differentiable function f(t) is selected as a mathematical reference model for realizing preset time positioning and formation. The formula of the continuously differentiable function f(t) is as follows:

[0062]

[0063] Among them, (·)′ represents the first-order derivative, d1, t∈[t0,∞), τ>0, μ(t) is a time-varying function. The time-varying function μ(t) in the embodiment of the present invention can enable the continuous differentiable function f(t) to converge to zero after a specified convergence time T.

[0064] The subsequent distributed orientation estimator, initial position estimator, and specified time distributed formation controller are all designed based on the form of the continuously differentiable function f(t), and all utilize the property that the continuously differentiable function f(t) can converge to zero after the specified time T.

[0065] 2. Estimate the position of the agent

[0066] Before position estimation and dynamic control of multiple agents, accurate orientation information must be determined.

[0067] A distributed azimuth estimator is designed to estimate the azimuth. The distributed azimuth estimator is designed as follows:

[0068]

[0069] in, is the estimated global unit vector between agents j and k, is the orthographic projection matrix, is the local position information directly measured in the local reference frame of agent i, d2, Indicates a local reference i Σ relative to the global reference frame g The estimated rotation matrix of Σ.

[0070] By using the time-varying gain function and combining the orthogonal projection matrix with the global azimuth, when the right side of the equation is equal to zero, the correct orientation of the agent can be successfully obtained, and the convergence of the algorithm has been proven. The parameters of the algorithm for estimating orientation and estimating initial position are as follows: d2 = 0.45, e2 = 3.5, τ = 10 in μ(t), and the upper limit of the specified convergence time is set to T2 = 5 seconds.

[0071] 3. Design the agent’s initial position estimator

[0072] After obtaining accurate position information, the initial position information of the agent i needs to be further obtained. In the embodiment of the present invention, an initial position estimator is designed to estimate the initial position information of the following agent. The initial position estimator is designed as follows:

[0073]

[0074] in, is the estimated initial position information of agent i, and are the estimated initial position information of the two neighbors j and k of agent i, respectively. and are all orthogonal projection matrices, d3,

[0075] Since the azimuth angle θ iThe estimation of depends on the estimation of the initial position, so only the global initial position estimation of the follower is needed. The principle is the same as that of equation (1) above. When the value on the right side of the equation is zero, the initial position information of agent i can be determined. The parameters of the estimator in this experiment are as follows: d3 = 0.45, e3 = 3.5, τ3 = 10 in μ(t), and the upper limit of the specified convergence time is set to T3 = 5 seconds.

[0076] 4. Design a time-specified distributed formation controller to enable followers to accurately move to the desired position based on the leader's global position information and the given ideal orientation information.

[0077] The designated time distributed formation controller is designed as follows:

[0078]

[0079] in, and is the constant expected approaching direction, and are all orthographic projection matrices, is the estimated global unit vector between agents i and j, is the estimated global unit vector between agents j and k, d4,

[0080] In the embodiment of the present invention, the parameters of the designated time distributed formation controller are selected as follows: d4=1, e4=2, τ4=10 in μ(t), and the upper limit of the designated convergence time is set to T4=15 seconds. By controlling the designated time distributed formation controller, when as well as It means that the follower moves precisely to the desired position.

[0081] Simulation and Experiment

[0082] 1) Simulate and verify the static target positioning and formation control algorithm on the MATLAB platform

[0083] In the simulation, the directions of all followers are randomly given, and the direction of the leader is consistent with the global reference system. The communication topology of the multi-agent system satisfies the Henneberg structure. The initial estimate is given arbitrarily, and the estimated convergence trajectory of the agent's starting position is as follows: Figure 4 As shown, the green triangle is the initial estimate, the colored curve is the convergence trajectory of the estimated position, and the blue circle is the actual initial position of the agent. Figure 5 Given the position estimation error curve of the follower agent, it can be found that the estimation error It converges to zero at T = 15 seconds. The estimated error curve of the initial position is as follows: Figure 6As shown, we can see the estimation error It can also converge to zero in the 5th second.

[0084] The formation process of intelligent agents is as follows Figure 7 As shown in the figure, the blue circle is the initial position of the agent, the colored curve is the moving trajectory of the agent under the action of the controller, and the purple star is the expected position of the agent. The expected static coordinate position is: [-10, 10, 0, 0, -20, 20, -20, 20, -40, 40; 0, 0, 20, -20, 20, -20, -20, 0, 0].

[0085] Figure 8 Given the control error curve of the follower agent, we can find the control error It can converge to zero in the 15th second.

[0086] 2) Testing with a small quad-rotor drone

[0087] The OptiTrack optical motion capture system is used to obtain the real-time position of the drone. The motion capture system and the base station communicate based on the protocol defined by ROS. Ten small quadrotor drones with passive spherical markers, Crazyffies, are used for experimental verification. The global absolute position of the drone satisfies the Henneberg configuration. The communication topology between drones is as follows: Fig. 9 As shown, L represents the leader and F represents the follower.

[0088] The initial estimated position of the follower drone is randomly given. The parameters are selected as follows: τ = 10, d = 0.45, e = 3.5, and the upper limit of the specified convergence time is set to T = 20 seconds. Fig.10 The formation process of the UAV formation is shown, and it can be found that the UAVs can form the desired formation. The experimental results show that the method of the embodiment of the present invention can ensure the precise orientation alignment of the multi-agent system and realize the static target formation control of the multi-agent system in the local reference frame.

[0089] Embodiment 2

[0090] An electronic device includes a memory and a processor, wherein the memory is used to store a program that supports the processor to execute the static target distributed positioning and formation control method based on local orientation information in Example 1, and the processor is configured to execute the program stored in the memory.

[0091] Embodiment 3

[0092] A storage medium stores a computer program, which, when executed by a processor, executes the steps of the method for distributed positioning and formation control of static targets based on local position information in embodiment 1.

[0093] The above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit the same. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that the technical solutions described in the aforementioned embodiments may still be modified, or some of the technical features may be replaced by equivalents. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A static target distributed positioning and formation control method based on local position information, characterized in that: The following steps are involved: Step 1: Based on the directed topological graph, the Henneberg configuration is used to establish the local reference frame of the intelligent agent, construct a single integrator intelligent agent model, and construct a mathematical reference model to achieve preset time positioning and formation; Step 2: Design a distributed orientation estimator to estimate the orientation of the agent; Step 3: Design an initial position estimator to estimate the initial position information of the following agent; Step 4: Design a time-specified distributed formation controller so that the followers can move accurately to the desired position based on the leader's global position information and the given ideal orientation information.

2. The method for distributed positioning and formation control of static targets based on local position information according to claim 1 is characterized in that: The method of establishing a local reference frame of an intelligent agent based on a directed topological graph using a Henneberg configuration is specifically as follows: The communication topology and relative position relationship between agents are represented by a directed graph To represent the directed graph Contains a finite set of vertices and a finite set of edges, where the vertex set v = {v1, v2, ..., v n }, edge set If (v α , v β )∈ε, representing the agent v α is the agent v β Neighbors, that is Directed topology graph By agent v i and v k and a directed edge (v k , v i ); a new directed topological graph In the figure By adding new agents and splitting directed edges; Consider a system of N agents on a two-dimensional plane, each of which has its own local reference frame and is related to the global reference frame g ∑ misalignment, the perception variable measured by the agent using the sensor is the local azimuth; definition i ∑ is the local reference frame of follower agent i, θ i ∈[0, 2π] is the agent i relative to the global frame g ∑ azimuth; the displacement vector between agents i and k is defined as p ik =p k -p i , where p k and p i Represent the global coordinate values ​​of agents k and i respectively; is the unit direction vector between agents i and k; Agent i in its local reference frame i The relative position vector of agent k measured under ∑ is expressed as definition Local reference frame i ∑ relative to the global reference frame g ∑ rotation matrix; Based on the rotation matrix, we get the direction vectors of agent i and agent k in the global frame. For the unit direction vector g ik , the orthographic projection matrix Defined as: Where I is the identity matrix; the orthogonal projection matrix Project any vector onto vector g ik On the orthogonal complement vector of is a positive semidefinite matrix and satisfies 3. The method for distributed positioning and formation control of static targets based on local position information according to claim 2 is characterized in that: The described construction of a single integrator agent model is as follows: Among them, p i =[x i ,y i ]∈R 2 represents the coordinates of agent i in the global reference frame, u i It is the control input that needs to be designed.

4. The method for distributed positioning and formation control of static targets based on local position information according to claim 3 is characterized in that: The control input that needs to be designed is the speed of the agent.

5. The method for distributed positioning and formation control of static targets based on local position information according to claim 3 is characterized in that: The method for constructing a mathematical reference model for realizing preset time positioning and formation is as follows: A continuously differentiable function f(t) is selected as a mathematical reference model for realizing preset time positioning and formation. The formula of the continuously differentiable function f(t) is as follows: Among them, (·)′ represents the first-order derivative, τ>0, μ(t) is a time-varying function, which makes the continuously differentiable function f(t) converge to zero after the specified convergence time T.

6. The method for distributed positioning and formation control of static targets based on local position information according to claim 5 is characterized in that: The distributed position estimator is designed as follows: in, is the estimated global unit vector between agents j and k, is the orthographic projection matrix, is the local orientation information directly measured in the local reference frame of agent i, Indicates a local reference i Σ relative to the global reference frame g The estimated rotation matrix of Σ.

7. The method for distributed positioning and formation control of static targets based on local position information according to claim 5, characterized in that: The initial position estimator is designed as follows: in, is the estimated initial position information of agent i, and are the estimated initial position information of the two neighbors j and k of agent i, respectively. and are all orthographic projection matrices, 8. The method for distributed positioning and formation control of static targets based on local position information according to claim 6, characterized in that: The designated time distributed formation controller is designed as follows: in, and is the constant expected approaching direction, and are all orthographic projection matrices, is the estimated global unit vector between agents i and j, is the estimated global unit vector between agents j and k, 9. An electronic device, comprising a memory and a processor, characterized in that: The memory is used to store a program that supports the processor to execute the static target distributed positioning and formation control method based on local position information as described in any one of claims 1 to 8, and the processor is configured to execute the program stored in the memory.

10. A storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the steps of the static target distributed positioning and formation control method based on local position information as described in any one of claims 1 to 8 are executed.

Images