Multi-sensor vision-assisted cooperative localization method based on factor graph optimization

Abstract

The invention relates to the field of aircraft cluster cooperative localization, and discloses a multi-sensor vision-assisted cooperative localization method based on factor graph optimization, and each aircraft constructs an inertial navigation pre-integration information resolving model by using an inertial measurement unit add table and gyro measurement information. A visible light camera is utilized to realize synchronous observation of the same area from different visual angles, observation images are transmitted through wireless communication, a relative distance information measurement model between aircrafts is constructed, and based on a scale invariant feature transform (SIFT) algorithm, the same feature point in the observation images is identified and matched, and the relative distance information measurement model between the aircrafts is obtained. And constructing a sight line information measurement model of the relative feature points of the aircraft. In addition, the wireless communication device can communicate with star chain satellites, and a distance information measurement model between the aircraft and the satellites is constructed. By constructing a factor graph model, available navigation information is fused by utilizing a factor graph optimization algorithm, and state information, namely position, speed and attitude information, of the aircraft is estimated.

Description

Technical Field

[0001] The present invention discloses a multi-sensor vision-assisted cooperative positioning method optimized based on a factor graph, which relates to the field of cooperative positioning of aircraft clusters, and particularly aims at the cooperative positioning of lightweight and miniaturized aircraft clusters. Background Art

[0002] With the rapid development of aircraft technology, aircraft clusters have been widely used in many fields such as military reconnaissance, logistics transportation, and terrain exploration. An aircraft cluster usually consists of multiple aircraft and completes complex tasks through cooperative operations. Among them, the accuracy of aircraft navigation and positioning is crucial.

[0003] Currently, the positioning of aircraft mainly relies on the Global Navigation Satellite System (GNSS). Although GNSS can provide accurate positioning information, satellite signals are easily blocked and interfered, resulting in a serious decline in positioning accuracy. Equipping aircraft with multiple sensors and fusing multi-source information is a method to improve the navigation accuracy of the system. However, existing fusion algorithms are often designed for specific types of sensors, lacking generality and flexibility, and it is difficult to adapt to the rapid changes in the state of sensors. In addition, the positioning system of aircraft should make full use of the information among the members of the aircraft cluster, regard the entire cluster as a unified whole, and achieve cooperative positioning.

[0004] In summary, when GNSS satellite signals are denied, it has become an urgent need to make full use of the information among the members of the aircraft cluster, effectively fuse data from different sensors, obtain more accurate and robust state estimation, and have a plug-and-play function. Summary of the Invention

[0005] Object of the Invention: Aiming at the above problems, the present invention proposes a multi-sensor vision-assisted cooperative positioning method optimized based on a factor graph. Based on an Inertial Measurement Unit (IMU), a wireless communication device and a visible light camera are introduced to achieve efficient data sharing among cluster members and synchronous observation of the same area from different perspectives. Based on the factor graph optimization algorithm, the navigation accuracy and robustness of the cooperative positioning system are improved.

[0006] To achieve the above object, the present invention adopts the following technical solutions: A multi-sensor vision-assisted cooperative positioning method optimized based on a factor graph, wherein multi-sensors are installed on each aircraft in a flight cluster, and the multi-sensors include an inertial measurement unit, a wireless communication device and a visible light camera installed on the aircraft. Each aircraft performs the following processing:

[0007] Construct an inertial navigation pre-integration information solution model based on the accelerometer and gyroscope measurement information of the inertial measurement unit; construct a Starlink distance information measurement model for the distance information between the aircraft and the satellite based on the wireless communication device and Starlink satellite communication; use a visible light camera to perform synchronous observations of the same area from different perspectives and capture observation images;

[0008] Mutually transmit the own state information, captured observation images, and time within the cluster through the wireless communication device; construct a relative distance information measurement model between aircraft;

[0009] Based on the scale-invariant feature transform algorithm, identify and match the same feature points in the observation images, and construct a line-of-sight information measurement model of the aircraft relative to the feature points;

[0010] Combine the above-constructed inertial navigation pre-integration information solution model, Starlink distance information measurement model, relative distance information measurement model, and line-of-sight information measurement model to construct a factor graph, and use the factor graph optimization algorithm to estimate the state information of the aircraft.

[0011] Preferably, construct an inertial navigation pre-integration information solution model based on the accelerometer and gyroscope measurements of the inertial measurement unit. Specifically: According to the inertial measurement unit at time t i the accelerometer measurement value f b (t i ) and the gyroscope measurement value perform direct pre-integration to obtain the pre-integration information at time t i+1 . The results are as follows:

[0012]

[0013] In the formula, P(t i ), V(t i ), φ(t i ) are the states of the aircraft at time t i , namely position, velocity, and attitude. g n is the acceleration in the navigation system, is the attitude transition matrix at time t i . The change amount is expressed as

[0014]

[0015] In the formula, is the coordinate transformation matrix from time t i+1 to time t i . is the Euler angle rate matrix from time t i+1 to time t i ; ε is the measurement error of the accelerometer and gyroscope, and τ is the intermediate variable of the double integral.

[0016] Preferably, in the construction of the star - chain distance information measurement model, measurement errors caused by the deviation between the clock of the wireless communication device and the star - chain system clock are considered on the basis of the distance information between the aircraft and the satellite.

[0017] Preferably, in the construction of the relative - distance information measurement model, measurement noise is considered on the basis of the relative distance between aircraft.

[0018] Preferably, assume that at time t i the position state of the aircraft is (x, y, z), and the position state of feature point k is (x k , y k , z k ). Then the line - of - sight information measurement model is:

[0019]

[0020] In the formula, α k , β k are the pitch angle and azimuth angle, and are measurement noises, and l is the number of recognized feature points.

[0021] Preferably, in combination with navigation information, a factor graph is constructed, and the factor - graph optimization algorithm is used to estimate the state information of the aircraft. The specific process is as follows:

[0022] Determine the nodes and factors in the factor - graph optimization algorithm: Take the state of the aircraft itself at time t i , that is, the position, velocity, and attitude information, as node x i . The factors include the inertial - navigation pre - integration factor, the star - chain distance factor, the relative - distance factor, and the relative line - of - sight factor;

[0023] Construct the error functions of the inertial - navigation pre - integration factor f INS (x i ), the star - chain distance factor error function f ST (x i ), the relative - distance factor error function f D (x i ), and the relative line - of - sight factor error function f LOS (x i );

[0024] If there are the latest star - chain distance factor, relative - distance factor, and relative line - of - sight factor, then perform factor - graph optimization, solve the objective function, and estimate the state information of the lightweight and miniaturized aircraft;

[0025] The objective function is minF(x i ) = f INS (x i ) + fST (x i ) + f D (x i ) + f LOS (x i )。

[0026] Preferably, the inertial navigation pre-integration factor error function f i at time t INS (x i ), the Starlink distance factor error function f ST (x i ), the relative distance factor error function f D (x i ) and the relative line-of-sight factor error function f LOS (x i ) are respectively:

[0027]

[0028] In the formula, L(·) is the square of the Mahalanobis distance, h INS (·) is based on the state x i-1 at time t i-1 and the measurement value of the inertial measurement unit to recursively derive the state pre-integration information at time t i , is the observation of the Starlink distance information, h ST (·) is the measurement model of the Starlink distance information volume, is the observation of the relative distance information between aircraft, h D (·) is the measurement model of the relative distance information volume, is the observation of the line-of-sight information of the relative aircraft relative to the feature point, h LOS (·) is the measurement model of the line-of-sight information volume of the relative aircraft relative to the feature point.

[0029] A multi-sensor vision-assisted cooperative positioning system based on factor graph optimization, including a flight cluster, and an inertial navigation module, a wireless communication module, a visible light camera module, and an information fusion module are arranged in each aircraft in the cluster;

[0030] The inertial navigation module constructs an inertial navigation pre-integration information solution model based on the acceleration meter and gyro measurement information of the inertial measurement unit;

[0031] The visible light camera module synchronously observes the same area from different perspectives, takes observation images; based on the scale-invariant feature transform algorithm, identifies and matches the same feature points in the observation images, and constructs a measurement model of the line-of-sight information volume of the aircraft relative to the feature point;

[0032] A wireless communication module communicates with Starlink satellites to build a Starlink distance information measurement model for calculating the distance information between the computing aircraft and the satellites; and transmits its own status information, the captured observation images, and the time within the cluster through the wireless communication device; builds a relative distance information measurement model between aircraft.

[0033] An information fusion module combines the inertial navigation pre-integration information solution model, the Starlink distance information measurement model, the relative distance information measurement model, and the line-of-sight information measurement model constructed above to build a factor graph, and uses the factor graph optimization algorithm to estimate the status information of the aircraft.

[0034] Preferably, in combination with the navigation information, a factor graph is built, and the factor graph optimization algorithm is used to estimate the status information of the aircraft. The specific process is as follows:

[0035] Determine the nodes and factors in the factor graph optimization algorithm: Take the status of the aircraft itself at time t i , that is, the position, velocity, and attitude information, as the node x i , and the factors include the inertial navigation pre-integration factor, the Starlink distance factor, the relative distance factor, and the relative line-of-sight factor;

[0036] Build the inertial navigation pre-integration factor error function f INS (x i ), the Starlink distance factor error function f ST (x i ), the relative distance factor error function f D (x i ), and the relative line-of-sight factor error function f LOS (x i );

[0037] If there are the latest Starlink distance factors, relative distance factors, and relative line-of-sight factors, then perform factor graph optimization, solve the objective function, and estimate the status information of the lightweight and miniaturized aircraft;

[0038] The objective function is minF(x i ) = f INS (x i ) + f ST (x i ) + f D (x i ) + f LOS (x i ).

[0039] Preferably, the inertial navigation pre-integration factor error function f i at time t INS (x i ), the Starlink distance factor error function f ST (x i) Relative distance factor error function f D (x i ) and relative line-of-sight factor error function f LOS (x i ) are respectively:

[0040]

[0041] In the formula, L(·) is the square of the Mahalanobis distance, h INS (·) is based on the state x i-1 at time t i-1 and the measurement value of the inertial measurement unit to recursively derive the state at time t i , is the observable of the star chain distance information, h ST (·) is the star chain distance information measurement model, is the observable of the relative distance information, h D (·) is the relative distance information measurement model, is the observable of the relative line-of-sight information, h LOS (·) is the relative line-of-sight information measurement model.

[0042] The advantages of the present invention are as follows:

[0043] 1. High integration: The present invention can seamlessly integrate various sensor data, including inertial measurement units (IMUs), wireless communication devices, and visible light cameras, ensuring reliable navigation information can be obtained in various environments.

[0044] 2. Excellent scalability and flexibility: This method supports plug-and-play functionality, allowing sensors to be added or removed without major modifications to the existing system, thus adapting to different task requirements and environmental changes, and improving the system adaptability and flexibility.

[0045] 3. Excellent autonomous navigation ability: In an environment with limited satellite signals, the method of the present invention can significantly improve the reliability of autonomous navigation and reduce the dependence on external navigation signals. This not only improves the positioning accuracy of lightweight and small aircraft clusters in complex environments but also enhances the autonomy and decision-making ability during mission execution, ensuring the completion of the mission. BRIEF DESCRIPTION OF THE DRAWINGS

[0046] Figure 1 is the system structure diagram of the present invention;

[0047] Figure 2 is the system framework diagram of the present invention;

[0048] Figure 3 is the factor graph of the present invention. DETAILED DESCRIPTION OF THE INVENTION

[0049] To make the objectives, technical solutions and advantages of the present invention clearer, the present invention will be described in more detail below in conjunction with embodiments and the accompanying drawings.

[0050] The detailed process includes: The structure of the present invention is as Figure 1 shown, and it includes a navigation computer, a GNSS module, an inertial measurement unit, a wireless communication device, and a visible light camera device. Among them, the visible light camera device includes devices such as a visible light camera, a lens light shield, and a shutter. The framework diagram of the present invention is as Figure 2 shown. Based on the acceleration meter and gyro measurement information of the inertial measurement unit, an inertial navigation pre-integration information solution model is constructed; based on the wireless communication device communicating with Starlink satellites, a Starlink distance information measurement model for the distance information between the aircraft and the satellite is constructed; using the visible light camera, synchronous observations of the same area are made from different perspectives, and observation images are taken; the own state information, the taken observation images, and the time are mutually transmitted within the cluster through the wireless communication device; a relative distance information measurement model between aircraft is constructed; based on the scale-invariant feature transform algorithm, the same feature points in the observation images are identified and matched, and a line-of-sight information measurement model of the aircraft relative to the feature points is constructed; combining the above-constructed inertial navigation pre-integration information solution model, Starlink distance information measurement model, relative distance information measurement model, and line-of-sight information measurement model, a factor graph is constructed, and the factor graph optimization algorithm is used to estimate the state information of the aircraft.

[0051] 1. Based on the acceleration meter and gyro measurement of the inertial measurement unit, an inertial navigation pre-integration information solution model is constructed. The specific process is as follows:

[0052] According to the acceleration meter measurement value f i in the body frame output by the inertial measurement unit at time t b (t i ) and the gyro measurement value , directly perform pre-integration to obtain the pre-integration information at time t i+1 . The results are as follows:

[0053]

[0054] In the formula, P(t i ), V(t i ), φ(t i ) are the position, velocity, and attitude of the lightweight and miniaturized aircraft at time t i , g n is the acceleration in the navigation frame, is the attitude transition matrix at time t i , and the change amount can be expressed as

[0055]

[0056] In the formula, is the coordinate transformation matrix from time t i+1 to time t i , and is the Euler angle rate matrix from time t i+1 to time t i . ε is the measurement error of the accelerometer and gyroscope, and τ is the intermediate variable of the double integral.

[0057] 2. Communicate with the "Starlink" satellite through a wireless communication device to construct a distance information measurement model between the aircraft and the satellite. In the construction of the Starlink distance information measurement model, the measurement error caused by the deviation between the clock of the wireless communication device and the clock of the Starlink system is considered based on the distance information between the aircraft and the satellite.

[0058] The lightweight and miniaturized aircraft receives the Starlink satellite signal through a wireless communication device and measures the distance between the lightweight and miniaturized aircraft and the satellite. However, considering the factor of the unknown time difference between the clock of the wireless communication device and the clock of the Starlink satellite, assuming that the position state of the lightweight and miniaturized aircraft at time t i is (x, y, z), the distance between the lightweight and miniaturized aircraft and the satellite is:

[0059]

[0060] In the formula, (xi, yi, zi) is the position state of Starlink satellite i, and ε d is the measurement error caused by the deviation between the clock of the wireless communication device and the clock of the Starlink system, and m is the number of Starlink satellites.

[0061] 3. The aircraft between clusters communicate through a wireless communication device to construct a relative distance information measurement model, and measurement noise is considered based on the relative distance between the aircraft.

[0062] The lightweight and miniaturized aircraft communicate with each other through a wireless communication device and measure the distance between the lightweight and miniaturized aircraft. Assuming that the position state of the lightweight and miniaturized aircraft at time t i is (x, y, z), and the position state of the lightweight and miniaturized aircraft j is (x j , y j , z j ), then the relative distance information is expressed as:

[0063]

[0064] In the formula, δ j is the measurement noise, and n is the number of lightweight and miniaturized aircraft in the cluster.

[0065] 4. By using a visible light camera, synchronous observation of the same area from different perspectives is achieved. Based on the Scale-Invariant Feature Transform (SIFT) algorithm, the same feature points in the observed images are identified and matched to obtain the line-of-sight information of the aircraft relative to the feature points. The specific process is as follows:

[0066] The SIFT algorithm is roughly divided into the following 4 steps: First, generate the scale space and perform extreme point detection; then, accurately locate the key points; then, generate the main direction parameters of the key points; finally, generate the SIFT feature vectors and perform matching.

[0067] After the feature point matching and recognition are completed, the line-of-sight information of the aircraft relative to the feature points can be obtained, that is, the pitch angle α k and the azimuth angle β k , assuming that the position state of the lightweight and miniaturized aircraft at time ti is (x, y, z), and the position state of feature point k is (x k , y k , z k ), then the line-of-sight information is expressed as:

[0068]

[0069] In the formula, and are measurement noises, and l is the number of identified feature points.

[0070] 5. As Figure 3 shown, combining the navigation information, a factor graph is constructed, and the factor graph optimization algorithm is used to estimate the state information of the aircraft. The specific process is as follows:

[0071] The construction of the factor graph is mainly the configuration of factor graph nodes and factors. In the present invention, the state of the lightweight and miniaturized aircraft itself at time t i , that is, the position, velocity, and attitude information, is used as the node x i , and the factors include the inertial navigation pre-integration factor, the Starlink distance factor, the relative distance factor, and the relative line-of-sight factor

[0072] First, the error function of the inertial navigation pre-integration factor at time t i is:

[0073]

[0074] In the formula, h INS (·) is based on the state at time t i-1 and the measurement values of the inertial measurement unit to recursively deduce the pre-integration information at time t i . For the noise that follows a Gaussian distribution, L(·) is defined as the square of the Mahalanobis distance, that is ∑ is the measurement noise covariance matrix.

[0075] After that, judge t i Whether the Starlink distance information at the moment is valid. If the condition is met, add it to the objective function. The error function of the Starlink distance factor is:

[0076]

[0077] In the formula, Is the observed value of the Starlink distance information, h ST (·) is the measurement model of the Starlink distance information.

[0078] Then, judge t i Whether the relative distance information between the lightweight and miniaturized aircraft at the moment is valid. If the condition is met, add it to the objective function. The error function of the relative line-of-sight factor is:

[0079]

[0080] In the formula, Is the observed value of the relative distance information between the aircraft, h D (·) is the measurement model of the relative distance information.

[0081] Next, judge t i Whether the line-of-sight information of the lightweight and miniaturized aircraft relative to the feature points at the moment is valid. If the condition is met, add it to the objective function. The error function of the relative line-of-sight factor is:

[0082]

[0083] In the formula, Is the observed value of the line-of-sight information of the aircraft relative to the feature points, h LOS (·) is the measurement model of the line-of-sight information of the aircraft relative to the feature band.

[0084] Finally, if there are the latest Starlink distance factor, relative distance factor and relative line-of-sight factor, perform factor graph optimization, solve the objective function, and estimate the state information of the lightweight and miniaturized aircraft.

[0085] minF(x i ) = f INS (x i ) + f ST (x i ) + f D (x i ) + f LOS (x i )(10)

[0086] Combining all the above available navigation information, construct a factor graph, and use the factor graph optimization algorithm to estimate the state information of the lightweight and miniaturized aircraft, such asFigure 3 As shown, the specific process is as follows:

[0087] The construction of the factor graph mainly involves factor graph nodes and factor configurations. In the present invention, the state of the lightweight miniaturized aircraft itself at time t i , that is, the position, velocity, and attitude information, is taken as the node x i , and the factors include the inertial navigation pre-integration factor, the Starlink distance factor, the relative distance factor, and the relative line-of-sight factor, which are specifically as follows:

[0088] First, the error function of the inertial navigation pre-integration factor at time t i is:

[0089]

[0090] In the formula, h INS (·) is based on the state at time t i-1 and the measurement value of the inertial measurement unit to recursively derive the state at time t i . For noise that follows a Gaussian distribution, L(·) is defined as the square of the Mahalanobis distance, that is ∑ is the measurement noise covariance matrix.

[0091] After that, it is judged whether the Starlink distance information at time t i is valid and available. If the condition is met, it is added to the objective function. The error function of the Starlink distance factor is:

[0092]

[0093] In the formula, is the observed value of the Starlink distance information, and h ST (·) is the measurement model of the Starlink distance information.

[0094] Then, it is judged whether the relative distance information between lightweight miniaturized aircraft at time t i is valid and available. If the condition is met, it is added to the objective function. The error function of the relative line-of-sight factor is:

[0095]

[0096] In the formula, is the observed value of the relative distance information, and h D (·) is the measurement model of the relative distance information.

[0097] Next, it is judged whether the line-of-sight information of the lightweight miniaturized aircraft relative to the feature point at time t i is valid and available. If the condition is met, it is added to the objective function. The error function of the relative line-of-sight factor is:

[0098]

[0099] In the formula, is the observation of the relative line-of-sight information, and h LOS (·) is the measurement model of the relative line-of-sight information.

[0100] Finally, if the satellite chain distance factor, relative distance factor, and relative line-of-sight factor are available, factor graph optimization is performed to solve the objective function and estimate the state information of the lightweight and miniaturized aircraft.

[0101] minF(x i ) = f INS (x i ) + f ST (x i ) + f D (x i ) + f LOS (x i )(10)

[0102] As Figure 2 shown, the present invention also provides a multi-sensor vision-assisted cooperative positioning system based on factor graph optimization, including a flight cluster. An inertial navigation module, a wireless communication module, a visible light camera module, and an information fusion module are arranged in each aircraft in the cluster;

[0103] The inertial navigation module constructs an inertial navigation pre-integration information solution model based on the acceleration meter and gyro measurement information of the inertial measurement unit;

[0104] The visible light camera module performs synchronous observations of the same area from different perspectives, captures observation images; based on the scale-invariant feature transform algorithm, identifies and matches the same feature points in the observation images, and constructs a line-of-sight information measurement model of the aircraft relative to the feature points;

[0105] The wireless communication module communicates with the satellite chain satellite, constructs a satellite chain distance information measurement model for calculating the distance information between the aircraft and the satellite; and mutually transmits its own state information, the captured observation images, and the time within the cluster through a wireless communication device; constructs a relative distance information measurement model between the aircraft;

[0106] The information fusion module combines the inertial navigation pre-integration information solution model, the satellite chain distance information measurement model, the relative distance information measurement model, and the line-of-sight information measurement model constructed above to construct a factor graph, and uses the factor graph optimization algorithm to estimate the state information of the aircraft.

[0107] The same functions in the system and the method are not described in detail here, and the specific implementation methods in the method can be referred to.

[0108] In summary, the multi-sensor vision-aided collaborative positioning method and system based on factor graph optimization proposed by the present invention have excellent scalability and plug-and-play functions, enabling significant improvement in the reliability of the autonomous navigation of the aircraft in an environment with limited satellite signals, thus significantly enhancing the reliability of the autonomous navigation of the aircraft in an environment with limited satellite signals. At the same time, the positioning accuracy of the aircraft cluster in complex environments is improved, and the autonomy and flexibility during mission execution are enhanced.

[0109] The above is only one embodiment of the present invention, which is illustrative rather than restrictive to the present invention. Those skilled in the art understand that many changes, modifications, and even equivalents can be made within the spirit and scope defined by the claims of the present invention, but all will fall within the protection scope of the present invention.

[0110] The parts not detailed in the present invention belong to the common general knowledge of those skilled in the art.

Claims

1. A multi-sensor vision-assisted collaborative positioning method based on factor graph optimization, characterized in that A multi-sensor is installed on each aircraft in the flying cluster. The multi-sensor includes an inertial measurement unit, a wireless communication device, and a visible light camera installed on the aircraft. Each aircraft performs the following processing: Construct an inertial navigation pre-integration information solution model based on the acceleration meter and gyro measurement information of the inertial measurement unit; communicate with Starlink satellites based on the wireless communication device to construct a Starlink distance information measurement model for the distance information between the aircraft and the satellite; use the visible light camera to perform synchronous observations of the same area from different perspectives and capture observation images; Mutually transmit its own state information, the captured observation images, and the time within the cluster through the wireless communication device; construct a relative distance information measurement model between aircraft; Based on the scale-invariant feature transform algorithm, identify and match the same feature points in the observation images to construct a line-of-sight information measurement model of the aircraft relative to the feature points; Combine the inertial navigation pre-integration information solution model, Starlink distance information measurement model, relative distance information measurement model, and line-of-sight information measurement model constructed above to construct a factor graph, and use the factor graph optimization algorithm to estimate the state information of the aircraft.

2. The multi-sensor vision-assisted collaborative positioning method based on factor graph optimization according to claim 1, wherein Based on the acceleration and gyro measurements of the inertial measurement unit, an inertial navigation pre-integration information calculation model is constructed. Specifically: According to the acceleration measurement value f i in the body frame output by the inertial measurement unit at time t b (t i ) and the gyro measurement value , directly perform pre-integration to obtain the pre-integration information at time t i+1 . The results are as follows: where P(t i ), V(t i ), φ(t i ) are the states of the aircraft at time t i , i.e., position, velocity and attitude, g n is the acceleration in the navigation system, is the attitude transition matrix at time t i , and the change is expressed as In the formula, is the coordinate transformation matrix from time t i+1 to time t i , and is the Euler angle rate matrix from time t i+1 to time t i ; ε is the measurement error of the accelerometer and gyroscope, and τ is the intermediate variable of the double integral.

3. A multi-sensor vision-assisted collaborative positioning method optimized based on factor graph according to claim 1, characterized in that In the construction of the Starlink distance information measurement model, measurement errors caused by the deviation between the clock of the wireless communication device and the clock of the Starlink system are considered based on the distance information between the aircraft and the satellite.

4. A multi-sensor vision-assisted collaborative positioning method optimized based on factor graph according to claim 1, characterized in that In the construction of the relative distance information measurement model, measurement noise is considered based on the relative distance between aircraft.

5. A multi-sensor vision-aided collaborative positioning method based on factor graph optimization according to claim 1, characterized in that, Assume t i At time t, the position state of the aircraft is (x, y, z), and the position state of feature point k is (x k , y k , z k ). Then the line-of-sight information measurement model is as follows: where α k , β k are the pitch angle and the azimuth angle, and are the measurement noises, and l is the number of recognized feature points.

6. A multi-sensor vision-aided collaborative positioning method based on factor graph optimization according to claim 1, characterized in that, Combine the navigation information to construct a factor graph, and use the factor graph optimization algorithm to estimate the state information of the aircraft. The specific process is as follows: Determine the nodes and factors in the factor graph optimization algorithm: At time t i the state of the aircraft itself, that is, the position, velocity, and attitude information, is the node x i , and the factors include the inertial navigation pre-integration factor, the Starlink distance factor, the relative distance factor, and the relative line-of-sight factor; Construct the error function \(f\) of the inertial navigation pre-integration factor INS (x i ), the error function \(f\) of the Starlink distance factor ST (x i ), the error function \(f\) of the relative distance factor D (x i ), and the error function \(f\) of the relative line-of-sight factor LOS (x i ); If there are the latest Starlink distance factors, relative distance factors, and relative line-of-sight factors, then perform factor graph optimization, solve the objective function, and estimate the state information of the lightweight and miniaturized aircraft; The objective function is minF(x i ) = f INS (x i ) + f ST (x i ) + f D (x i ) + f LOS (x i ).

7. A multi-sensor vision-assisted collaborative positioning method based on factor graph optimization according to claim 6, characterized in that, t i The error function f of the moment inertial navigation pre-integration factor INS (x i ), the error function f of the Starlink distance factor ST (x i ), the error function f of the relative distance factor D (x i ), and the error function f of the relative line-of-sight factor LOS (x i ) are respectively as follows: where \(L(\cdot)\) is the square of the Mahalanobis distance, and \(h\) INS (\cdot)\) is based on \(t\) i-1 the state \(x\) at time i-1 and the measurement value of the inertial measurement unit recursively derive the state pre-integration information at time \(t\) i , and \(\hat{z}_{s}\) is the observable of the star-chain distance information, and \(h\) ST (\cdot)\) is the star-chain distance information measurement model, \(\hat{z}_{r}\) is the observable of the relative distance information between aircraft, and \(h\) D (\cdot)\) is the relative distance information measurement model, \(\hat{z}_{v}\) is the observable of the line-of-sight information of the relative aircraft with respect to the relative feature point, and \(h\) LOS (\cdot)\) is the line-of-sight information measurement model of the relative aircraft with respect to the relative feature point.

8. A multi-sensor vision-assisted collaborative positioning system based on factor graph optimization, characterized in that It includes a flying cluster, and an inertial navigation module, a wireless communication module, a visible light camera module, and an information fusion module are set in each aircraft within the cluster; The inertial navigation module constructs an inertial navigation pre-integration information solution model based on the acceleration meter and gyro measurement information of the inertial measurement unit; The visible light camera module performs synchronous observations of the same area from different perspectives and captures observation images; Based on the scale-invariant feature transform algorithm, identify and match the same feature points in the observation images to construct a line-of-sight information measurement model of the aircraft relative to the feature points; The wireless communication module communicates with Starlink satellites to construct a Starlink distance information measurement model for calculating the distance information between the aircraft and the satellite; and mutually transmits its own state information, the captured observation images, and the time within the cluster through the wireless communication device; constructs a relative distance information measurement model between aircraft; The information fusion module combines the inertial navigation pre-integration information solution model, Starlink distance information measurement model, relative distance information measurement model, and line-of-sight information measurement model constructed above to construct a factor graph, and uses the factor graph optimization algorithm to estimate the state information of the aircraft.

9. A multi-sensor vision-assisted collaborative positioning system optimized based on factor graph according to claim 8, characterized in that, Combine the navigation information to construct a factor graph, and use the factor graph optimization algorithm to estimate the state information of the aircraft. The specific process is as follows: Determine the nodes and factors in the factor graph optimization algorithm: At time t i the state of the aircraft itself, i.e., position, velocity, and attitude information, is the node x i The factors include the inertial navigation pre-integration factor, the Starlink distance factor, the relative distance factor, and the relative line-of-sight factor; Construct the error function f of the inertial navigation pre-integration factor INS (x i ), the error function f of the Starlink distance factor ST (x i ), the error function f of the relative distance factor D (x i ), and the error function f of the relative line-of-sight factor LOS (x i ); If there are the latest Starlink distance factors, relative distance factors, and relative line-of-sight factors, then perform factor graph optimization, solve the objective function, and estimate the state information of the lightweight and miniaturized aircraft; The objective function is minF(x i ) = f INS (x i ) + f ST (x i ) + f D (x i ) + f LOS (x i ).

10. A multi-sensor vision-aided collaborative positioning system optimized based on a factor graph according to claim 9, characterized in that, t i Moment inertial navigation pre-integration factor error function f INS (x i ), satellite chain distance factor error function f ST (x i ), relative distance factor error function f D (x i ), and relative line-of-sight factor error function f LOS (x i ) are respectively as follows: where \(L(\cdot)\) is the square of the Mahalanobis distance, and \(h\) INS (\cdot)\) is based on \(t\) i-1 -moment state \(x\) i-1 and the measurement value of the inertial measurement unit to recursively derive the state at \(t\) i -moment, is the observable of the star - chain distance information, and \(h\) ST (\cdot)\) is the star - chain distance information measurement model, is the observable of the relative distance information, and \(h\) D (\cdot)\) is the relative distance information measurement model, is the observable of the relative line - of - sight information, and \(h\) LOS (\cdot)\) is the relative line - of - sight information measurement model.

Images