A formation integrated navigation method based on relative geometric measurement information between agents

Abstract

The present invention provides a formation combined navigation method based on relative geometric measurement information between agents, belonging to the field of formation navigation technology. The technical problem of reduced combined navigation positioning performance caused by limited GPS communication is solved. Considering the geometric constraint characteristics of the current main formation types, combined with the unique constraint of the corresponding formation expected configuration, that is, rigidity, the relative geometric quantities that need to be measured between agents of each type of formation are first given; secondly, combined with the relationship between each relative geometric quantity and the position of each agent, an observation equation that integrates the relative measurement information of the agent and its own inertial navigation information is constructed; finally, based on the principle of weighted least squares, a data processing method that integrates multi-source measurement information and improves the positioning performance of each agent is proposed. The present invention does not rely on external GPS communication, has the significant advantage of strong independence, and will effectively improve the formation positioning control performance under conditions of limited GPS communication, and enhance the safety and reliability of formation task execution.

Description

Technical Field

[0001] The invention belongs to the technical field of formation navigation and discloses a formation combined navigation method based on relative geometric measurement information between intelligent bodies. Background Art

[0002] Navigation is a long-term research problem in the field of automation. In a narrow sense, navigation is to solve the problem of accurately positioning "where am I"; in a broad sense, navigation is to solve the problem of route planning "how to get there". Positioning is the basis of route planning, so broad navigation is an application derivative of narrow navigation.

[0003] Combined navigation is a common means for intelligent bodies to obtain accurate positioning information. The traditional navigation method is inertial navigation, which mainly determines the position information based on Newton's second law combined with the physical characteristics of components. It has the advantages of low cost, simple use and good independence, and has been widely used in various intelligent mobile platforms, such as drones, unmanned vehicles and unmanned boats. However, inertial components generally have the defects of low positioning accuracy and increasing error over time due to installation errors, zero drift and other reasons. In order to ensure the positioning accuracy, the inertial navigation information needs to be corrected. Combined navigation is an important means to improve the positioning performance of intelligent bodies. It is often based on inertial navigation information and is positioned by fusing multi-source positioning information, such as image navigation, geomagnetic navigation and GPS navigation. Since each positioning method uses different principles and components and has different error characteristics, they can correct each other to improve accuracy. At present, the combined navigation formed by GPS and inertial navigation is the most widely used due to mature technology, high positioning accuracy and low cost. However, in indoor, underwater or electromagnetic interference environments where GPS signals are restricted, the combined navigation performance that relies on GPS positioning information will be difficult to guarantee.

[0004] Formation is an important development direction of current research on intelligent agent applications. Through mutual information exchange, agents achieve coordination in time, space and function, realize the transformation from simple individual structure and function to diversified group structure and function, and form the ability to complete complex tasks and adapt to complex environments. Spatial coordination is the most basic and intuitive form of formation coordination, and is currently the most in-depth research. It achieves formation coordination by defining mutual geometric constraints between agents, such as distance constraint, azimuth constraint and similarity constraint.

[0005] The precise positioning of each agent is the key to achieving the desired coordination effect of the formation. During the control process, each agent adjusts its own state based on the position of the adjacent agents and its own position to achieve the desired spatial coordination effect. If the position of each agent is inaccurate, each agent will generate corresponding control instructions based on the position information with deviations, and ultimately cause control deviations of the entire formation, which will not only affect the coordination effect of the formation, but also cause collision risks between agents, affecting the safety and reliability of the formation mission, such as the drone commercial performance crashes that have occurred many times in recent years.

[0006] Therefore, a new formation combined navigation method is proposed, which can ensure the performance of combined navigation positioning even when GPS communication is limited. Summary of the invention

[0007] The purpose of the present invention is to provide a formation integrated navigation method based on relative geometric measurement information between intelligent agents, so as to overcome the technical problem of reduced integrated navigation positioning performance caused by limited GPS communication.

[0008] In order to achieve the above objectives and solve the above technical problems, the technical solutions of the present invention are as follows:

[0009] A formation integrated navigation method based on relative geometric measurement information between intelligent agents comprises the following steps:

[0010] Step 1: Determine the information perception device and information interaction device of each intelligent agent in combination with the formation task operation environment and type;

[0011] Step 2: According to the requirements of formation rigidity and observability of formation state, determine the geometric constraints that the relative geometric quantities of the formation need to satisfy;

[0012] Assume that the team consists of n agents, each of which is numbered from 1 to n in sequence, p i Denote as the inertial navigation measurement value of the position of agent i in the formation (1≤i≤n), define p as the formation state vector, which is the superposition of the position vectors of each agent;

[0013] p=[p 1 ,p 2 ,...,p n ] T (1)

[0014] 2.1 Distance-constrained formation

[0015] Note ij represents the relative position vector between agent i and agent j, l ij Vector l ij The modulus of , i.e., the distance between the two agents;

[0016]

[0017] Define the relative distance between agents as the element, and the vector formed by superposition is the rigid function F l :

[0018] F l =[...,l ij ,l ik ,l kl ,....] T (3)

[0019] In order to efficiently utilize the relative distance between agents to correct the formation inertial navigation information, for the distance-constrained formation, the designed distance constraint set should satisfy: the expected configuration of the formation under the combined distance constraint is unique, that is, the formation distance rigidity; if the rigidity function F l The expected configuration of the formation corresponding to the distance constraint is already unique, so further adding distance measurement information between agents will not significantly improve the formation navigation performance;

[0020] According to the distance-constrained formation rigidity, for a plane formation, if there is

[0021]

[0022] Then the formation configuration under this set of distance constraints is unique, where rank represents the rank of the matrix;

[0023] For a three-dimensional formation, if there is:

[0024]

[0025] Then the formation configuration under this set of distance constraints is unique, where rank represents the rank of the matrix;

[0026] 2.2 Azimuth-constrained formation

[0027] Remember ij is the vector l ij The unit direction vector of :

[0028] e ij = l ij / ||l ij || (6)

[0029] For the orientation-constrained plane formation, the azimuth angle θ between the direction vector and the agent is ij One to one correspondence:

[0030] e ij =[cosθ ij ,sinθ ij ] T (7)

[0031] If it is a three-dimensional formation, the height angle σ between the direction vector and the agent ij and azimuth angle θ ij One to one correspondence:

[0032] e ij =[cosσ ij cosθ ij ,cosσ ij sinθ ij ,sinσ ij ] T (8)

[0033] The unit direction vector is used to represent the relative position information between agents, and the rigid function F is defined e The vector formed by measuring the distance between adjacent agents for each agent in the distance-constrained formation;

[0034] F e =[...,e ij ,e ik ,e kl ,....] T (9)

[0035] Similarly, the orientation-constrained formation should make the designed distance constraint set satisfy: the expected configuration of the formation under the combined distance constraint is unique, that is, the formation distance rigidity; if the rigidity function F e The formation's desired configuration corresponding to the included orientation constraints is already unique, so further adding orientation measurement information between agents will not significantly improve the formation's navigation performance;

[0036] According to the rigidity of the azimuth-constrained formation, if there is

[0037]

[0038] Then the formation configuration under this set of orientation constraints is unique;

[0039] Step 3: Determine the formation state observation equation based on the formation rigidity function and measurement information

[0040] 3.1 For distance-constrained formations

[0041] The formation state measurement includes the inertial navigation information of each agent and the relative distance information between agents. The formation state observation Y is defined as:

[0042] Y=[F l T ,p T ] T (11)

[0043] According to the definition of rigid function, the formation state observation quantity Y is a nonlinear function of the formation state quantity, denoted by p o is the state of the neighborhood of the position information p of the formation agent, then the Taylor expansion is used, ignoring the higher-order terms above the second order, and the formation state observation quantity Y is approximated as:

[0044]

[0045] According to the above formula, the formation state observation matrix H can be defined as:

[0046]

[0047] Where I nd represents a unit diagonal matrix, the dimension of which is the product of the number of agents in the formation n and the spatial dimension d;

[0048] 3.2 For Azimuth-Constrained Formations

[0049] If there is only the direction of each edge in the rigid function and the inertial navigation position information of each agent, that is, the measurement quantity

[0050] Y=[F e T ,p T ] T (14)

[0051] The corresponding observation matrix is:

[0052]

[0053] Step 4: Formation state measurement information and measurement deviation, determine the optimal navigation state based on weighted least squares method, and calculate the positioning correction

[0054] Assuming that the geometric measurement of the formation and the position error characteristics of each agent's inertial navigation measurement obey Gaussian distribution, the measurement F formed by the combination of each agent's position and the distance between agents is l 、F e , p, e and l corresponding deviation distances will all be diagonal square matrices, denoted by R Fl , R Fe , R p , R e and R l ;

[0055] Considering the combination of different measurement information For the distance-constrained formation, which includes the relative distance and the inertial position measurement deviation, R is defined as

[0056] R = diag(R Fl ,R p ) (16)

[0057] Where R Fl Determined by the accuracy of the distance measuring device, R p Determined by the inertial navigation positioning accuracy of each intelligent agent; for the orientation-constrained formation, which includes the relative orientation and inertial navigation position measurement deviation, R is defined

[0058] R = diag(R Fe ,R p ) (17)

[0059] Where R Fe Determined by the accuracy of the sight angle measurement device; considering that the direct information of the azimuth measurement between agents is the sight angle, the sight azimuth measurement deviation from agent i to agent j in the plane formation is ε σij , the azimuth and elevation deviations of the line of sight from agent i to agent j in the three-dimensional formation are ε σij and ε θij , then according to the definition of the sight unit direction vector, for the plane formation sight unit direction vector e ij The deviation matrix R eij Should be:

[0060]

[0061] In the formula, the symbol · represents the dot product of the vector, diag represents the matrix with the vector elements as the diagonal elements, and for the three-dimensional formation line of sight unit direction vector e ij The bias variance matrix R eij Should be:

[0062]

[0063] where s ij =[σ ij ,θ ij ] T , ε sij =[ε σij ,ε θij ] T , F e R corresponding to each unit direction vector eij Constitutes R Fe Diagonal small square matrix;

[0064] According to the principle of weighted least squares method, it can be realized

[0065] J=(YY(p * )) T R(YY(p * )) (20)

[0066] Minimize the optimal estimate of the formation state, where Y * is the true value of the measurement, p * is the optimal estimate of the state;

[0067] When the above formula is minimum, there should be:

[0068]

[0069] Right now:

[0070] (Y * -Y(p * )) T RH(p * )=(Y * -Y(p 0 )-H| p0 (p * -p 0 )) T RH| p0 =0 (22)

[0071] The corresponding ones are:

[0072] p * =p 0 +(H T | p0 RH| p0 ) T H T | p0 R[YY(p 0 )] (twenty three)

[0073] The above formula gives the expression for iteratively solving the optimal formation state;

[0074] In the initial iteration, the measured value of the formation state variable can be taken as p 0 , that is, let p 0 (1) = p to calculate H T | p0 , substitute into the last calculation to get p * (1); then the p * Assign to p 0 , denoted as p 0 (2), recalculate H T | p0 , and then substitute it into the above formula to get the new p * , denoted as p * (2) Repeat this process and continuously update p 0 and p * until there is a value of

[0075] ||p 0 -p *||<ε (24)

[0076] Where ε is a small quantity greater than zero, which can be selected according to the control accuracy required by the formation mission;

[0077] The optimal estimate is taken as the formation state determined by the integrated navigation, and Δp is recorded as the correction amount of the integrated navigation to the inertial navigation information, that is:

[0078] Δp=p * -p (25)

[0079] Step 5: Periodically return to step 4 and continuously correct the inertial navigation measurement information until the mission is completed.

[0080] The effective benefits of the present invention are as follows:

[0081] The present invention provides a method for correcting the inertial navigation deviation of a formation under conditions where GPS positioning is limited. The present invention comprehensively considers the measurement characteristics of the relative geometric quantities of the formation and the design of the data fusion method, and realizes the combined navigation positioning correction based only on the measurement and interaction information of each intelligent body within the formation, which does not rely on external GPS communication and has the significant advantage of strong independence and autonomy. It will effectively improve the positioning control performance of the formation under conditions where GPS communication is limited, and enhance the safety and reliability of the execution of the formation's tasks. BRIEF DESCRIPTION OF THE DRAWINGS

[0082] Figure 1 is a schematic diagram of correcting position measurement performance based on relative distance information;

[0083] Figure 2 It is a schematic diagram of the process of implementing the present invention. DETAILED DESCRIPTION

[0084] The present invention is explained and described in detail below with reference to the accompanying drawings.

[0085] In order to overcome the problem of reduced integrated navigation positioning performance caused by limited GPS communication, the present invention proposes a method for combined navigation positioning correction using relative geometric measurement information between agents for formations. Multi-agent formation control requires not only the use of each agent's own state information, but also the use of relative geometric information between agents. The two often use different sensitive devices for measurement and have different error characteristics, so they can be integrated to improve measurement performance. Figure 1 As shown in the figure, the circular envelope is the envelope of the inertial navigation measurement deviation of the position point to be measured. At the same time, the distance deviation envelope between the position point to be measured and the reference position points 1 and 2 can be adopted as the width Δr in the figure. 1 and Δr 2It can be seen that after the relative distance measurement information is comprehensively utilized, the position deviation envelope of the position point to be measured is reduced to the dark part in the figure, which significantly improves the inertial navigation measurement performance. Based on this feature, the present invention proposes a formation combined navigation method using relative geometric measurement information between intelligent agents.

[0086] Taking into account the geometric constraint characteristics of the current main formation types and combining the unique constraint of the corresponding formation's expected configuration, namely rigidity, the present invention first gives the relative geometric quantities that need to be measured between agents of each type of formation; secondly, combining the relationship between each relative geometric quantity and the position of each agent, an observation equation that integrates the relative measurement information of the agent and its own inertial navigation information is constructed; finally, based on the principle of weighted least squares method, a data processing method is proposed to integrate multi-source measurement information and improve the positioning performance of each agent.

[0087] The following are the steps of the method:

[0088] Step 1: Determine the information perception and interaction devices of each agent based on the formation mission operation environment and type

[0089] The type of formation measurement information is the basis for the design of the combined navigation algorithm. Due to the constraints of the formation operating environment and formation type, it is necessary to specifically select information perception devices such as lasers, visible light pods and inertial navigation, as well as appropriate communication frequency bands to achieve stable information interaction between intelligent agents.

[0090] First, consider the perception of agent information, including the measurement of its own state and relative state information. Inertial navigation devices are a common means of measuring the agent's own state information such as position and posture. They do not rely on external information. Although the accuracy may drift over time, they have good independence and autonomy. The measurement of relative state information between agents depends on the selection of the current formation type. Distance-constrained formations and azimuth-constrained formations are the most studied space-constrained formations. Distance-constrained formations define the expected configuration of the formation by defining the relative distance between agents. If the distance between agents is in the order of hundreds of meters to kilometers, a laser rangefinder should be selected; if the distance is within a hundred meters, a visible light pod or other device can be used to estimate the distance based on the imaging characteristics of the target image. Azimuth-constrained formations define the formation configuration by defining the relative orientation between agents. Optical pods are a conventional means of measuring azimuth angles.

[0091] Secondly, information interaction is considered, which is mainly used for sharing of intelligent agent perception information. For electromagnetic interference working environments where GPS information is denied, the communication frequency band between intelligent agents should be far away from the frequency band of GPS communication; for GPS signal denial environments formed by natural barriers such as indoors and underwater, the appropriate frequency band should be selected to ensure reliable communication between intelligent agents within a short distance. The selection of communication devices also needs to consider the limited load carrying capacity and power consumption limit of intelligent agents.

[0092] Step 2: Based on the rigidity of the formation, design the relative state measurement between agents (determine the geometric constraints that the relative geometric quantities of the formation need to satisfy based on the requirements of formation rigidity and formation state observability;)

[0093] Each intelligent agent first uses an inertial navigation device to measure its own position information, and then designs the measurement of the relative geometric relationship between intelligent agents based on the formation type and the rigidity of the formation.

[0094] Assume that the team consists of n agents, each of which is numbered from 1 to n in sequence, p i Denote it as the inertial navigation measurement value of the position of agent i in the formation (1≤i≤n), and define p as the formation state vector, which is the superposition of the position vectors of each agent.

[0095] p=[p 1 ,p 2 ,...,p n ] T (1)

[0096] Condition 1: Distance-constrained formation

[0097] Note ij represents the relative position vector between agent i and agent j, l ij Vector l ij The modulus is the distance between the two agents.

[0098]

[0099] Define the relative distance between agents as the element, and the vector formed by superposition is the rigid function F l :

[0100] F l =[...,l ij ,l ik ,l kl ,....] T (3)

[0101] In order to efficiently use the relative distance between agents to correct the formation inertial navigation information, for the distance-constrained formation, the designed distance constraint set should satisfy: the expected configuration of the formation under the combined distance constraint is unique, that is, the formation distance rigidity. If the rigidity function F l The expected configuration of the formation corresponding to the included distance constraint is already unique, and further adding distance measurement information between agents will not significantly improve the formation navigation performance.

[0102] According to the distance-constrained formation rigidity, for a plane formation, if there is

[0103]

[0104] Then the formation configuration under this set of distance constraints is unique, where rank represents the rank of the matrix.

[0105] For a three-dimensional formation, if there is:

[0106]

[0107] Then the formation configuration under this set of distance constraints is unique, where rank represents the rank of the matrix.

[0108] Condition 1: Azimuth-constrained formation

[0109] Remember ij is the vector l ij The unit direction vector of :

[0110] e ij = l ij / ||l ij || (6)

[0111] For the orientation-constrained plane formation, the azimuth angle θ between the direction vector and the agent is ij One to one correspondence:

[0112] e ij =[cosθ ij ,sinθ ij ] T (7)

[0113] If it is a three-dimensional formation, the height angle σ between the direction vector and the agent ij and azimuth angle θ ij One to one correspondence:

[0114] e ij =[cosσ ij cosθ ij ,cosσ ij sinθ ij ,sinσ ij ] T (8)

[0115] Therefore, the relative position information between agents can be represented by the unit direction vector. Define the rigidity function F e The vector formed by the distance between adjacent agents is measured for each agent in the distance-constrained formation.

[0116] F e =[...,e ij ,e ik ,e kl ,....] T (9)

[0117] Similarly, the orientation-constrained formation should make the designed distance constraint set satisfy: the expected configuration of the formation under the combined distance constraint is unique, that is, the formation distance rigidity. If the rigidity function F e The expected configuration of the formation corresponding to the included orientation constraints is already unique, and further adding orientation measurement information between agents will not significantly improve the formation navigation performance.

[0118] According to the rigidity of the azimuth-constrained formation, if there is

[0119]

[0120] Then the formation configuration under this set of orientation constraints is unique.

[0121] Step 3: Determine the formation state observation equation based on the formation rigidity function and measurement information

[0122] For the distance-constrained formation, the formation state measurement includes the inertial navigation information of each agent and the relative distance information between agents, so the formation state observation Y can be defined as:

[0123] Y=[F l T ,p T ] T (11)

[0124] According to the definition of rigid function, the measurement quantity Y is a nonlinear function of the formation state quantity. o is the state of the neighborhood of the position information p of the formation agent, then the Taylor expansion can be used to ignore the higher-order terms above the second order, and the state observation quantity Y can be approximated as:

[0125]

[0126] According to the above formula, the formation state observation matrix H can be defined as:

[0127]

[0128] Where I nd Represents a unit diagonal matrix, whose dimension is the product of the number of agents in the formation n and the spatial dimension d.

[0129] For the orientation-constrained formation, if only the directions of the edges in the rigid function and the inertial navigation position information of each agent are available, that is, the measurement quantity

[0130]

[0131] The corresponding observation matrix is:

[0132]

[0133] In the actual operation process, the position of the intelligent agent formation calculated at the last moment can be selected as p 0 , complete the calculation of the observation matrix.

[0134] Step 4: According to the formation status measurement information and measurement deviation, the optimal navigation state is determined based on the weighted least squares method, and the positioning correction is calculated;

[0135] Assuming that the geometric measurement of the formation and the position error characteristics of each agent's inertial navigation measurement obey Gaussian distribution, the measurement F formed by the combination of each agent's position and the distance between agents is l 、F e , p, e and l corresponding deviation distances will all be diagonal square matrices, denoted by R Fl , R Fe , R p , R e and R l .

[0136] Considering the combination of different measurement information For the distance-constrained formation, which includes the relative distance and the inertial position measurement deviation, R is defined as

[0137] R = diag(R Fl ,R p ) (16)

[0138] Where R Fl Determined by the accuracy of the distance measuring device, R p Determined by the inertial navigation positioning accuracy of each intelligent agent.

[0139] For the azimuth-constrained formation, which includes the relative azimuth and inertial position measurement deviation, R is defined as

[0140] R = diag(R Fe ,R p ) (17)

[0141] Where R Fe Determined by the accuracy of the sight angle measurement device. Considering that the direct information of the azimuth measurement between agents is the sight angle, the sight angle measurement deviation from agent i to agent j in the plane formation is ε σij , the azimuth and elevation deviations of the line of sight from agent i to agent j in the three-dimensional formation are ε σij and ε θij , then according to the definition of the sight unit direction vector, for the plane formation sight unit direction vector e ij The deviation matrix R eij Should be:

[0142]

[0143] In the formula, the symbol · represents the dot product of the vector, and diag represents a matrix with the vector elements as diagonal elements. For the three-dimensional formation line of sight unit direction vector e ij The bias variance matrix R eij Should be:

[0144]

[0145] where s ij =[σ ij ,θ ij ] T , ε sij =[ε σij ,ε θij ] T . F e R corresponding to each unit direction vector eij Constitutes R Fe A matrix of diagonal small squares.

[0146] According to the principle of weighted least squares method, it can be realized

[0147] J=(YY(p * )) T R(YY(p * )) (20)

[0148] Minimize the optimal estimate of the formation state, where Y * is the true value of the measurement, p * is the optimal estimated value of the state. When the above formula is minimum, there should be:

[0149]

[0150] Right now:

[0151] (Y * -Y(p * )) T RH(p * )=(Y * -Y(p 0 )-H| p0 (p * -p 0 )) T RH| p0 =0 (22)

[0152] The corresponding ones are:

[0153] p * =p 0 +(H T | p0 RH| p0 ) T H T| p0 R[YY(p 0 )] (twenty three)

[0154] The above formula gives the expression for iterative solution of the optimal formation state. In the initial iteration, the measured value of the formation state variable can be taken as p 0 , that is, let p 0 (1) = p to calculate H T | p0 , substitute into the last calculation to get p * (1). Then the p * Assign to p 0 , denoted as p 0 (2), recalculate H T | p0 , and then substitute it into the above formula to get the new p * , denoted as p * (2) Repeat this process and continuously update p 0 and p * until there is a value of

[0155] ||p 0 -p * ||<ε (24)

[0156] Here, ε is a small quantity greater than zero and can be selected according to the control accuracy required by the formation mission.

[0157] The optimal estimate is used as the formation state determined by the integrated navigation. Let Δp be the correction amount of the integrated navigation to the inertial navigation information, that is:

[0158] Δp=p * -p (25)

[0159] Step 5: Periodically return to step 4 and continuously correct the inertial navigation measurement information until the mission is completed

[0160] The inertial navigation deviation has the characteristic of time accumulation, and the error is not large in a short time. When the formation control accuracy requirement is not high, in order to save communication and computing resources, the correction interval can be appropriately relaxed in combination with the inertial navigation error characteristics and mission requirements, that is, the cycle of combined navigation can be made shorter than the inertial navigation measurement cycle. For the measurement information that is not corrected by the combined navigation method, the correction amount generated by the inertial navigation measurement value p can be used as the formation state value, and p is recorded. Δ is the formation status value calculated using this method.

[0161] p Δ =p+Δp (26)

[0162] The present invention is explained and illustrated in detail above in combination with the accompanying drawings and the specific implementation process. After comprehensively considering the measurement characteristics of the relative geometric quantities of the formation and the design of the data fusion method, the present invention does not rely on external GPS communication and realizes combined navigation, positioning and correction based only on the measurements and interaction information of each intelligent body within the formation. It has the significant advantage of strong independence and autonomy, and improves the safety and reliability of the execution of the formation's tasks.

Claims

1. A formation combined navigation method based on relative geometric measurement information between agents, characterized in that, it includes the following steps: Step 1: Combine the formation mission operation environment and type to determine the information perception devices and information interaction devices of each agent; Step 2: According to the requirements of formation rigidity and formation state observability, determine the geometric constraints that the formation relative geometric quantities need to satisfy; Assume that the formation consists of n agents, and the agents are numbered from 1 to n in sequence, p i Denote the inertial measurement value of the position of the i-th agent in the formation (1 ≤ i ≤ n), and define p as the formation state vector, which is formed by superimposing the position vectors of each agent; p = [p 1 , p 2 ,..., p n T (1)​ 2.1 Distance constraint type formation Denote \(l\) ij represents the relative position vector from the \(i\)-th agent to the \(j\)-th agent, \(l\) ij For the vector \(l\) ij its modulus, which is the distance between the two agents; Define the vector formed by superimposing the relative distances between agents as elements as the rigid function F l : F l ＝[...,l ij ,l ik ,l kl ,....] T (3) To efficiently utilize the relative distances between agents to correct formation inertial information, for distance-constrained formations, the designed set of distance constraints should satisfy: the expected formation configuration under this combined distance constraint is unique, i.e., the formation distance is rigid; if the rigidity function F l has a unique expected formation configuration corresponding to the included distance constraints, then continuing to increase the distance measurement information between agents will not significantly improve the formation navigation performance; According to the formation rigidity of the distance constraint type formation, for a planar formation, if there is then the formation configuration under this set of distance constraints is unique, where rank represents the rank of the matrix; For a three-dimensional formation, if there is: then the formation configuration under this set of distance constraints is unique, where rank represents the rank of the matrix; 2.2 Azimuth constraint type formation Denote e ij as the unit direction vector of vector l ij : e ij ＝l ij / ||l ij || (6) For the azimuth-constrained planar formation, the direction vector corresponds one-to-one with the azimuth angle θ between agents ij one-to-one: e ij = [cosθ ij , sinθ ij T (7)​ If it is a three-dimensional formation, the direction vector corresponds one-to-one with the elevation angle σ ij and the azimuth angle θ ij between agents: e ij = [cosσ ij cosθ ij , cosσ ij sinθ ij , sinσ ij T (8)​ Using the unit direction vector to represent the relative orientation information between agents, define the rigidity function F e is the vector formed by the distances measured by each agent in the distance-constrained formation to its adjacent agents; F e = [..., e ij , e ik , e kl ,....] T (9) Similarly, the formation with orientation constraints should ensure that the designed set of distance constraints satisfies that the expected formation configuration under the combined distance constraints is unique, that is, the formation distance is rigid. If the rigid function F e contains the orientation constraints corresponding to the unique expected formation configuration, then continuing to increase the orientation measurement information between agents will not significantly improve the formation navigation performance; According to the formation rigidity of the azimuth constraint type formation, if there is then the formation configuration under this set of azimuth constraints is unique; Step 3: According to the formation rigidity function and measurement information, determine the formation state observation equation 3.1 For the distance constraint type formation The formation state measurement includes the inertial navigation information of each agent and the relative distance information between agents. Define the formation state observable quantity Y: Y = [F l T , p T T (11)​ According to the definition of the rigidity function, the formation state observation quantity Y is a non-linear function of the formation state quantity. Denote p o as the state of the neighborhood of the formation agent position information p. Then, using the Taylor expansion and ignoring the high-order terms above the second order, the formation state observation quantity Y is approximated as: According to the above formula, the formation state observation matrix H can be defined as: where I nd represents an identity diagonal matrix, and the dimension of the matrix is the product of the number of formation agents n and the spatial dimension d; 3.2 For the azimuth constraint type formation If only the directions of the sides in the rigid function and the inertial navigation position information of each agent, that is, the measured quantities Y = [F e T , p T T (14)​ The corresponding observation matrix is: Step 4: Based on the formation state measurement information and measurement deviation, determine the optimal navigation state based on the weighted least squares method, and calculate the positioning correction amount Assume that the formation geometric measurement quantities and the position error characteristics measured by the inertial navigation of each agent both follow Gaussian distributions. The measurement quantities F formed by combining the positions of each agent and the distances between agents l and F e , p, e, and l will all be diagonal square matrices for the corresponding deviation distances, denoted as R Fl and R Fe , R p , R e and R l ; Considering the combination of different measurement information to form For a distance-constrained formation, which includes relative distance and inertial position measurement deviation, define R R = diag(R Fl , R p ) (16) where R Fl is determined by the accuracy of the distance measurement device, R p is determined by the inertial navigation positioning accuracy of each agent; for the azimuth constraint formation, which includes the relative azimuth and the measurement deviation of the inertial navigation position, then R is defined R = diag(R Fe , R p ) (17) where R Fe is determined by the accuracy of the line-of-sight angle measurement device; considering that the direct information for azimuth measurement between agents is the line-of-sight angle, let the measurement deviation of the line-of-sight azimuth angle from agent i to agent j in the planar formation be ε σij , and the measurement deviations of the line-of-sight azimuth angle and elevation angle from agent i to agent j in the three-dimensional formation be ε σij and ε θij , respectively. Then, according to the definition of the line-of-sight unit direction vector, for the line-of-sight unit direction vector e ij in the planar formation, the deviation matrix R eij should be: where the symbol · represents the dot product of vectors, diag represents a matrix with vector elements as diagonal elements, and for the three-dimensional formation line-of-sight unit direction vector e ij the deviation variance matrix R eij should be: where s ij = [σ ij , θ ij T , ε sij = [ε σij , ε θij T , F e The Rs corresponding to the unit direction vectors in each case eij constitute the R Fe diagonal small block matrix;​​ According to the principle of the weighted least squares method, it is to achieve J = (Y - Y(p * )) T R(Y - Y(p * )) (20) Determine the optimal estimate of the formation state with the minimum as the goal, where Y * is the true value of the measurement, p * is the optimal estimated value of the state; When the above formula is the smallest, there should be: That is: (Y * -Y(p * )) T RH(p * )=(Y * -Y(p 0 )-H| p0 (p * -p 0 )) T RH| p0 =0 (22) Correspondingly there is: p * = p 0 +(H T | p0 RH| p0 ) T H T | p0 R[Y - Y(p 0 )] (23) The above formula gives the expression for iteratively solving the optimal formation state; At the initial iteration, the measured value of the formation state variable can be taken as p 0 , that is, let p 0 (1) = p to calculate H T | p0 , substitute the p * (1) obtained from the previous calculation; then assign this p * to p 0 , denoted as p 0 (2), recalculate H T | p0 , substitute it into the above formula again to get the new p * , denoted as p * (2); repeat in turn, continuously iterate and update the values of p 0 and p * until there is ||p 0 -p * ||<ε (24) where ε is a small quantity greater than zero, which can be selected according to the control accuracy required by the formation mission; Take the optimal estimated value as the formation state determined by the combined navigation, and record Δp as the correction amount of the combined navigation to the inertial navigation information, that is: Δp = p * -p (25) Step 5: Periodically return to Step 4, and continuously correct the inertial navigation measurement information until the mission ends.

2. The formation combined navigation method based on relative geometric measurement information between agents according to claim 1, characterized in that, The agent information perception device described in Step 1 includes the agent's own state measurement device and the measurement device for relative state information; The measurement of the agent's own state includes the measurement of the agent's position and attitude information, which is measured by an inertial navigation device; The measurement of the relative state information between agents depends on the selection of the current formation type: For the distance constraint type formation, if the distance between agents is in the range of hundreds of meters to kilometers, a laser rangefinder should be selected; if the distance is within hundreds of meters, devices such as visible light pods can be used to estimate the distance according to the imaging characteristics of the target image; For the azimuth constraint type formation, an optical pod is used to measure the direction angle.

3. The formation combined navigation method based on relative geometric measurement information between agents according to claim 1, characterized in that, The information interaction device described in step 1 is used for sharing the information sensed by the agents; for the electromagnetic interference operation environment with GPS information denial, the communication frequency band between agents should be far away from the GPS communication frequency band; for the GPS signal denial environment formed by natural barriers such as indoors and underwater, a suitable frequency band should be selected for the purpose of ensuring reliable communication between agents within a short distance. The selection of the communication device also needs to consider the limited payload carrying capacity and power consumption limit of the agents.

4. A formation combined navigation method based on the relative geometric measurement information between agents according to claim 1, characterized in that In step 3.1, select the formation position of the agent calculated at the previous moment as p 0 , and complete the calculation of the observation matrix.

5. A formation combined navigation method based on the relative geometric measurement information between agents according to any one of claims 1-4, characterized in that In the periodic return correction step of step 5: When the formation control accuracy requirement is not high, in order to save communication and computing resources, combined with the inertial navigation error characteristics and mission requirements, the correction interval is appropriately relaxed, and the period of the combined navigation is less than the inertial navigation measurement period; For the measurement information not corrected by the combined navigation method, the inertial navigation measurement value p is directly used as the formation state value by successively combining the correction amounts generated by the combined navigation in the nearest order. p Δ = p + Δp (26) Denote p Δ as the formation state value calculated using this method.

Images