A multi-modal pseudolite multi-source fusion positioning method based on factor graph optimization

Abstract

The present invention relates to the field of satellite navigation technology, and specifically discloses a multi-modal pseudolite multi-source fusion positioning method based on factor graph optimization, comprising the following steps: S1, using an improved visual inertial odometry system (VIO) to predict the carrier's position; S2, using the carrier's position prediction value to assist the antenna array pseudolite in angle calculation, while simultaneously calculating position information using a traditional pseudolite positioning method; S3, constructing a fusion positioning system based on factor graph optimization to solve all state variables within a sliding window; S4, optimizing and solving the state variables by minimizing the sum of all residuals within the sliding window, solving the objective function using a nonlinear optimization library, and obtaining the optimal estimated values ​​of the system state variables. The present invention adopts the above-mentioned multi-modal pseudolite multi-source fusion positioning method based on factor graph optimization, effectively overcoming the drawbacks of a single positioning method and improving the reliability and accuracy of the positioning system in complex environments.

Description

Technical Field

[0001] The present invention relates to the field of satellite navigation technology, and in particular to a multi-modal pseudolite multi-source fusion positioning method based on factor graph optimization. Background Art

[0002] Visual Inertial Odometry (VIO) is widely used in robot navigation, autonomous drone flight, virtual reality, and augmented reality due to its advantages of high precision, low cost, and miniaturization. The VIO system obtains environmental feature information through a camera and combines it with the motion information of the Inertial Measurement Unit (IMU) to achieve local high-precision pose estimation. However, due to the influence of environmental interference and measurement noise, the VIO system will produce large cumulative errors during long-term operation, making it difficult to meet the requirements of high-precision positioning of the carrier, long-term stable navigation, and autonomous operation in complex environments. In order to improve the positioning accuracy and robustness of the system, integrating the global constraint information provided by other global sensors has become an effective solution.

[0003] The Global Navigation Satellite System (GNSS) provides global coverage and drift-free absolute position information. Fusion of GNSS information with visual inertial (VIO) information is a common approach to improving system positioning accuracy and long-term stability. By fusing the local pose information calculated by VIO with the global position information provided by GNSS during pose graph optimization, cumulative errors are eliminated, achieving locally accurate and globally drift-free pose estimation. However, GNSS requires signals from at least four satellites to resolve position, and insufficient availability in complex environments can severely impact positioning accuracy. To address this issue, tightly coupled fusion of GNSS raw observations with visual and inertial information can improve the availability and robustness of positioning systems in complex urban environments. However, GNSS cannot provide effective global constraints in indoor and underground spaces where signals are denied. Therefore, researchers have proposed methods to integrate other global information sources, such as magnetometers, ultra-wideband (UWB), and pseudolites.

[0004] By fusing visual and inertial measurements with magnetometer information, geomagnetic information is used to suppress the navigation system's heading drift, resulting in more accurate positioning. While magnetometers can provide global attitude constraints, they are not suitable for indoor environments because magnetic field information is easily interfered with by metal objects. Fusing a single UWB ranging result into a VIO system reduces positioning drift. Position information is first calculated using multiple UWB base stations, and then the UWB position information is fused with visual odometry (VO) and IMU information to improve the system's positioning accuracy. However, UWB is incompatible with GNSS in terms of signal structure, requiring positioning systems in indoor and outdoor environments to rely on different hardware. In contrast, pseudolite technology uses the same signal system as GNSS and is directly compatible with existing GNSS satellite receivers, avoiding additional hardware modification costs. Outdoors, GNSS / visual / inertial fusion has become the mainstream solution for high-precision positioning, and pseudolite can provide stable global constraint information in environments with limited GNSS signals. The integration of pseudolite information enables the entire system to maintain a consistent hardware architecture in indoor and outdoor environments, thereby achieving locally high-precision, globally drift-free, seamless indoor and outdoor positioning.

[0005] Existing technologies eliminate VIO's cumulative errors by fusing the relative pose information calculated by VIO with the global position and velocity information calculated by pseudolites, effectively improving the accuracy and reliability of indoor positioning. However, this method cannot guarantee positioning performance when the number of visible pseudolites is insufficient or the Dilution of Precision (DOP) value is high. Traditional pseudolites positioning methods use multiple pseudolites to calculate carrier position information, but cannot provide reliable measurement information when the number of visible satellites is insufficient or the geometric configuration is poor. Pseudolites positioning methods based on fingerprint matching technology have high data acquisition and storage costs and limited generalization capabilities, making it difficult to provide global drift-free positioning information. Summary of the Invention

[0006] The purpose of this invention is to provide a multimodal pseudolite multi-source fusion positioning method based on factor graph optimization. A uniform circular antenna array structure is designed to estimate the azimuth and elevation angles of a satellite receiver under single-point deployment conditions, utilizing IMU information to improve the accuracy and robustness of azimuth and elevation calculations. Simultaneously, the satellite receiver's position information is estimated using traditional pseudolite positioning methods. Furthermore, a multimodal pseudolite / visual / inertial fusion positioning algorithm is designed based on factor graph optimization technology. When the number of visible pseudolites is sufficient and the DOP value is low, pseudolite position information is fused with visual and inertial information. When the number of visible pseudolites is insufficient or the DOP value is high, pseudolite angle measurements are fused with visual and inertial measurements. Practical experiments validate the algorithm's positioning capabilities in indoor environments, as well as its seamless indoor and outdoor positioning capabilities after integrating GNSS information.

[0007] To achieve the above object, the present invention provides a multi-modal pseudolite multi-source fusion positioning method based on factor graph optimization, comprising the following steps:

[0008] S1. Use the improved visual inertial odometry system VIO to predict the carrier's position and posture;

[0009] S2, using the carrier posture prediction value to assist the antenna array pseudolite to perform angle calculation, and at the same time calculate the position information using the traditional pseudolite positioning method;

[0010] S3. Build a fusion positioning system based on factor graph optimization to solve all state variables in the sliding window;

[0011] S4. The state variables are optimized by minimizing the sum of all residuals in the sliding window. The objective function is solved by the nonlinear optimization library CeresSolver to obtain the optimal estimate of the system state variables.

[0012] Preferably, in S1, the improved VIO is obtained by integrating multimodal pseudo-satellite information in the visual inertial odometry system VIO, where the multimodal pseudo-satellite includes antenna array pseudo-satellite and traditional pseudo-satellite. The visual camera is used to obtain environmental feature information, and the high-frequency characteristics of the inertial measurement unit IMU output are used, combined with the state estimation result of the previous moment to predict the carrier posture.

[0013] Preferably, in S1, in the antenna array pseudo-satellite system, the multi-channel pseudo-satellite base station synchronously transmits signals with the same carrier phase through the antenna array, and uses the carrier phase difference information, combined with the signal wavelength and the geometric structure of the antenna array, to estimate the signal's arrival angle, including azimuth and elevation angle, under single-point deployment conditions.

[0014] Preferably, in S1, a uniform circular antenna array is used, and the array elements are evenly distributed in a radius of R On the circle, the distance between adjacent array elements is less than half the wavelength of the pseudo-satellite signal;

[0015] Assume that there are three different and non-collinear array elements in the uniform circular array 、 、 The carrier phases of the transmitted pseudo-satellite signals when they reach the satellite receiver are 、 、 , then the array element 、 , array element 、 The carrier phase difference between 、 Calculate using formula (1):

[0016] (1);

[0017] Where, 、 Array elements 、 , array element 、 The carrier phase difference ambiguity between them.

[0018] Preferably, in S2, the carrier phase difference between the two groups of array elements is used to solve the azimuth angle of the satellite receiver in the pseudo-satellite array antenna coordinate system through formula (2): and pitch angle information:

[0019] (2);

[0020] in is the signal wavelength, is the total number of array elements of pseudolite array antenna, 、 Array elements 、 , array element 、 The spacing between.

[0021] Preferably, in S2, the inertial information is used to assist in optimizing the pseudo-satellite angle solution of the antenna array. The state prediction process between two adjacent frames of inertial measurement is shown in formula (3):

[0022] (3);

[0023] Where, 、 、 They are The carrier position, speed, and attitude prediction values ​​at the moment, 、 、 They are The carrier position, speed, and attitude prediction values ​​at the moment, 、 are the median acceleration and median angular velocity of the two frames of inertial measurement, is the time difference between two frames of inertial measurement, Represents quaternion multiplication operations;

[0024] Using an iterative approach k Momentary visual frame and j All IMU data between pseudolite measurement frames at time are calculated by formula (3) to obtainj The carrier position prediction value at time ; Combined with the position and attitude of the pseudo-satellite array antenna in the navigation coordinate system, calculate the azimuth prediction value of the satellite receiver in the pseudo-satellite array antenna coordinate system , pitch angle prediction value ; And the angle prediction value is converted into 、 The solution obtained in formula (2) 、 to integrate.

[0025] Preferably, in S3, a fusion positioning system based on factor graph optimization is constructed, including constructing state variables to be solved for the system and constructing factors connecting the state variables, and the factors connecting the state variables include pseudo-satellite position factors, antenna array pseudo-satellite angle factors, IMU pre-integration factors and vision factors.

[0026] Preferably, in S3, the pseudolite position factor is defined as the difference between the carrier position and the pseudolite position. k The pseudolite position factor between the frame carrier position and the pseudolite position is shown in formula (4):

[0027] (4);

[0028] Where, For the k Frame carrier position, For the k The frame carrier state variable corresponds to the pseudolite position measurement at the moment, 、 The rotation matrix and translation vector between the navigation coordinate system and the traditional pseudolite positioning coordinate system;

[0029] When the number of visible pseudolites is sufficient and the DOP value is below a certain threshold, the pseudolites position factor is added to the fusion positioning system optimized by the factor graph;

[0030] The pseudo-satellite angle factor of the antenna array is defined as the difference between the predicted azimuth and elevation angles of the carrier and the measured azimuth and elevation angles of the pseudo-satellite of the antenna array. k The antenna array pseudolite angle factor between the frame carrier state and the antenna array pseudolite angle measurement is shown in formula (5):

[0031] (5);

[0032] Where, For the k The position of the frame carrier in the pseudolite array antenna coordinate system is specifically expressed as shown in formula (6):

[0033] (6);

[0034] Indicates the position of the k-th frame carrier, Indicates the position and posture of the pseudolite array antenna in the navigation coordinate system; 、 are the azimuth and elevation angle measurements of the antenna array pseudolite at the time corresponding to the carrier state variable of the kth frame;

[0035] When the number of visible pseudolites is insufficient or the DOP value is higher than a certain threshold, the pseudolites angle factor of the antenna array is added to the fusion positioning system of factor graph optimization;

[0036] The IMU pre-integration factor is defined as the difference between the pre-integration prediction value and the pre-integration estimate value. k Frame and k The IMU pre-integration factor between +1 frame images is shown in formula (7):

[0037] (7);

[0038] Where, For the k The rotation matrix of the frame carrier pose, 、 Respectively k Frame and k +1 frame carrier position, 、 Respectively k Frame and k +1 frame carrier speed, Two image frames k and k +1 time difference between 、 Respectively k Frame and k +1 frame carrier pose quaternion, Indicates the extraction of quaternion The vector part of is used to represent the three-dimensional rotation residual; 、 and are the estimated values ​​of position pre-integration, velocity pre-integration and rotation pre-integration respectively;

[0039] In the feature point method VO, the visual factor is defined as the reprojection error. There are two ways to parameterize the feature points in the visual reprojection error: the three-dimensional position parameterization of the feature points in the navigation system and the inverse depth parameterization of the feature points in the image frame.

[0040] In the inverse depth parameterization method, the feature point is modeled as the inverse of the feature depth in a certain image frame. Feature points on the frame image The inverse depth of , then the visual factor is as shown in formula (8):

[0041] (8);

[0042] Where, 、 Feature points In the Frame and j Frame camera coordinates on the normalized image plane, Feature points From The frame is transformed into the j After the frame, in the camera coordinate system z The axis weight, 、 Respectively Frame and j The carrier pose of the frame, is the external parameter between the camera and IMU.

[0043] Preferably, in S3, the state variables of the fusion positioning system based on factor graph optimization include all carrier states in the sliding window, IMU zero bias and feature point inverse depth, as shown in formula (9):

[0044] (9);

[0045] Where, Indicates the k Frame variables to be optimized, including the position of the carrier in the navigation coordinate system ,speed , attitude quaternion , accelerometer bias and gyroscope bias , is the inverse depth of the feature point.

[0046] Preferably, in S4, the state variables in formula (9) are optimized by minimizing the sum of all residuals in the sliding window, and the objective function is defined as shown in formula (10):

[0047] (10);

[0048] Where, is the marginalized prior information, is the pseudolite position factor constructed by formula (4), is the set of all pseudolite position measurements corresponding to the state variable time within the sliding window, is the covariance matrix of pseudolite position factors; is the pseudo-satellite angle factor of the antenna array constructed by formula (5), is the set of all antenna array pseudolite angle measurements corresponding to the state variable time within the sliding window, is the covariance matrix of the pseudolite angle factor of the antenna array;

[0049] is the IMU pre-integration factor constructed by formula (7), is the set of all IMU measurements within the sliding window, is the covariance matrix of the IMU pre-integration factor; is the visual factor constructed by formula (8), is the set of feature points observed by at least two frames of images within the sliding window, is the covariance matrix of the visual factor; Formula (10) is solved by the nonlinear optimization library CeresSolver to obtain the optimal estimate of the system state variables.

[0050] Therefore, the present invention adopts the above-mentioned multi-modal pseudolite multi-source fusion positioning method based on factor graph optimization, and the beneficial effects are as follows:

[0051] (1) The present invention addresses the problem that the VIO system generates large cumulative errors due to environmental interference and measurement noise during long-term operation, making it difficult to meet the requirements of high-precision positioning, long-term stable navigation, and autonomous operation in complex environments. By integrating multi-modal pseudo-satellite information, the present invention effectively improves the positioning accuracy and robustness, and meets the positioning requirements of relevant application scenarios.

[0052] (2) The present invention adopts a factor graph optimization method to fuse multiple measurement information. Compared with the filtering-based method, it can perform multiple state linearizations and estimate all system states in combination with historical information. It can solve all state variables in the sliding window, improve the state estimation accuracy, and achieve high-precision, drift-free estimation of the carrier posture.

[0053] The technical solution of the present invention is further described in detail below through the accompanying drawings and embodiments. BRIEF DESCRIPTION OF THE DRAWINGS

[0054] Figure 1 It is the overall architecture of an embodiment of a multi-modal pseudolite multi-source fusion positioning method based on factor graph optimization of the present invention;

[0055] Figure 2 A pseudolite uniform circular antenna array model of an embodiment of a multi-modal pseudolite multi-source fusion positioning method based on factor graph optimization of the present invention;

[0056] Figure 3 It is a data fusion diagram based on factor graph optimization of an embodiment of a multi-modal pseudolite multi-source fusion positioning method based on factor graph optimization of the present invention. DETAILED DESCRIPTION

[0057] The technical solution of the present invention is further described below with reference to the accompanying drawings and embodiments.

[0058] Unless otherwise defined, the technical or scientific terms used in the present invention shall have the usual meanings understood by persons of ordinary skill in the field to which the present invention belongs. The words "first", "second" and similar terms used in the present invention do not indicate any order, quantity or importance, but are only used to distinguish different components. Words such as "include" or "comprise" mean that the elements or objects preceding the word include the elements or objects listed after the word and their equivalents, without excluding other elements or objects. Words such as "connect" or "connected" are not limited to physical or mechanical connections, but may include electrical connections, whether direct or indirect. "Up", "down", "left", "right" and the like are only used to indicate relative positional relationships. When the absolute position of the object being described changes, the relative positional relationship may also change accordingly.

[0059] like Figure 1 As shown, a multi-modal pseudolite multi-source fusion positioning method based on factor graph optimization includes the following steps:

[0060] S1. Use the improved visual inertial odometry system VIO to predict the carrier's position and posture;

[0061] The improved VIO is obtained by integrating multimodal pseudolites into the traditional visual inertial odometry system (VIO). The multimodal pseudolites include antenna array pseudolites and traditional pseudolites. The visual camera is used to obtain environmental feature information, and the high-frequency characteristics of the inertial measurement unit (IMU) output are used to predict the carrier's position and posture in combination with the state estimation results of the previous moment.

[0062] In an antenna array pseudolite system, multi-channel pseudolite base stations transmit signals with the same carrier phase synchronously through the antenna array. Due to the positional differences between the antenna array elements, the signals arrive at the receiver with different carrier phases. By leveraging this carrier phase difference information, combined with the signal wavelength and the geometric structure of the antenna array, the signal's angle of arrival (AoA), including both azimuth and elevation, can be estimated under single-point deployment conditions.

[0063] like Figure 2 As shown, the present invention adopts a uniform circular antenna array, and the array elements are evenly distributed in a radius of ROn the circle, the distance between adjacent array elements is less than half the wavelength of the pseudo-satellite signal.

[0064] Assume that there are three different and non-collinear array elements in the uniform circular array 、 、 The carrier phases of the transmitted pseudo-satellite signals when they reach the satellite receiver are 、 、 , then the array element 、 , array element 、 The carrier phase difference between 、 It can be calculated by formula (1):

[0065] (1);

[0066] Where, 、 Array elements 、 , array element 、 The carrier phase difference ambiguity between the two array elements needs to be resolved when the distance between different array elements is greater than half the wavelength of the signal.

[0067] S2. The carrier posture prediction value is used to assist the antenna array pseudo-satellite to perform angle calculation, and the position information is calculated by the traditional pseudo-satellite positioning method.

[0068] Using the carrier phase difference between the two groups of array elements, the azimuth angle of the satellite receiver in the pseudo-satellite array antenna coordinate system can be solved by formula (2): and pitch angle information:

[0069] (2);

[0070] in is the signal wavelength, is the total number of array elements of pseudolite array antenna, 、 Array elements 、 , array element 、 The spacing between.

[0071] In practical applications, the angle estimation of pseudolites using antenna arrays is susceptible to factors such as multipath signals and environmental noise, which can cause jumps in the angle measurement results and affect the stability of azimuth and elevation angle measurements. In contrast, IMUs have a high output frequency and are not subject to external interference, providing highly accurate pose estimation results in a short period of time.

[0072] Using inertial information to assist in optimizing the pseudo-satellite angle solution of the antenna array can effectively improve the accuracy and robustness of the angle solution. The state prediction process between two adjacent frames of inertial measurement is shown in formula (3):

[0073] (3);

[0074] Where, 、 、 They are The carrier position, speed, and attitude prediction values ​​at the moment, 、 、 They are The carrier position, speed, and attitude prediction values ​​at the moment, 、 are the median acceleration and median angular velocity of the two frames of inertial measurement, is the time difference between two frames of inertial measurement, Represents a quaternion multiplication operation.

[0075] Using an iterative approach k Momentary visual frame and j All IMU data between pseudolite measurement frames at time are calculated by formula (3) to obtain j The carrier position prediction value at time ; Combined with the position and attitude of the pseudo-satellite array antenna in the navigation coordinate system, calculate the azimuth prediction value of the satellite receiver in the pseudo-satellite array antenna coordinate system , pitch angle prediction value ; And the angle prediction value is converted into 、 The solution obtained in formula (2) 、 Fusion is performed to ensure the stability of angle information.

[0076] S3. Build a fusion positioning system based on factor graph optimization to solve all state variables in the sliding window.

[0077] Filtering-based methods perform only one linearization during data fusion and estimate only the current state variables. Therefore, errors generated during state estimation accumulate in the system. Optimization-based methods, on the other hand, can perform multiple state linearizations and estimate all system states using historical information, resulting in higher accuracy. Therefore, this paper employs a factor graph optimization method to fuse traditional pseudolite position measurements, pseudolite angle measurements from antenna arrays, IMU measurements, and visual measurements to solve for all state variables within a sliding window.

[0078] like Figure 3 As shown in the figure, a fusion positioning system based on factor graph optimization is constructed, including constructing the state variables to be solved of the system and constructing the factors connecting the state variables. The factors connecting the state variables include pseudolite position factors, antenna array pseudolite angle factors, IMU pre-integration factors and vision factors. The constructed factors are added to the fusion positioning system optimized by the factor graph to achieve the optimal estimation of the system state within the sliding window.

[0079] Pseudo-satellite position factor:

[0080] The pseudolite position factor is defined as the difference between the carrier position and the pseudolite position. k The pseudolite position factor between the frame carrier position and the pseudolite position is shown in formula (4):

[0081] (4);

[0082] Where, For the k Frame carrier position, For the k The frame carrier state variable corresponds to the pseudolite position measurement at the moment, 、 is the rotation matrix and translation vector between the navigation coordinate system and the traditional pseudolite positioning coordinate system.

[0083] When the number of visible pseudolites is sufficient and the DOP value is lower than a certain threshold, the pseudolites position factor is added to the fusion positioning system of factor graph optimization.

[0084] Antenna array pseudolite angle factor:

[0085] The pseudo-satellite angle factor of the antenna array is defined as the difference between the predicted azimuth and elevation angles of the carrier and the measured azimuth and elevation angles of the pseudo-satellite of the antenna array. k The antenna array pseudolite angle factor between the frame carrier state and the antenna array pseudolite angle measurement is shown in formula (5):

[0086] (5);

[0087] Where, For the k The position of the frame carrier in the pseudolite array antenna coordinate system is specifically expressed as shown in formula (6):

[0088] (6);

[0089] Indicates the position of the k-th frame carrier, Indicates the position and posture of the pseudolite array antenna in the navigation coordinate system; 、 They are the azimuth and elevation angle measurements of the antenna array pseudo-satellite at the corresponding moment of the carrier state variable in the kth frame.

[0090] When the number of visible pseudolites is insufficient or the DOP value is higher than a certain threshold, the pseudolites angle factor of the antenna array is added to the fusion positioning system of factor graph optimization.

[0091] IMU pre-integration factor:

[0092] The IMU pre-integration factor is defined as the difference between the pre-integration prediction value and the pre-integration estimate value. k Frame and k The IMU pre-integration factor between +1 frame images is shown in formula (7):

[0093] (7);

[0094] Where, For the k The rotation matrix of the frame carrier pose, 、 Respectively k Frame and k +1 frame carrier position, 、 Respectively k Frame and k +1 frame carrier speed, Two image frames k and k +1 time difference between 、 Respectively k Frame and k +1 frame carrier pose quaternion, Indicates the extraction of quaternion The vector part of is used to represent the three-dimensional rotation residual; 、 and They are the estimated values ​​of position pre-integration, velocity pre-integration and rotation pre-integration respectively.

[0095] Visual Factor:

[0096] In the feature point method (VO), the visual factor is defined as the reprojection error. There are two ways to parameterize feature points in the visual reprojection error: parameterizing the feature points' 3D positions in the navigation frame and parameterizing the feature points' inverse depth in the image frame.

[0097] In the inverse depth parameterization method, the feature point is modeled as the inverse of the feature depth in a certain image frame. Feature points on the frame image The inverse depth of , then the visual factor is as shown in formula (8):

[0098] (8);

[0099] Where, 、 Feature points In the Frame and j Frame camera coordinates on the normalized image plane, is a feature point From The frame is transformed into the j After the frame, in the camera coordinate system z The axis weight, 、 Respectively Frame and j The carrier pose of the frame, is the external parameter between the camera and IMU.

[0100] System state variables and objective function:

[0101] The state variables of the fusion positioning system based on factor graph optimization include all carrier states in the sliding window, IMU zero bias, and inverse depth of feature points, as shown in formula (9):

[0102] (9);

[0103] Where, Indicates the k Frame variables to be optimized, including the position of the carrier in the navigation coordinate system ,speed , attitude quaternion , accelerometer bias and gyroscope bias , is the inverse depth of the feature point.

[0104] S4. The state variables are optimized by minimizing the sum of all residuals in the sliding window. The objective function is solved by the nonlinear optimization library CeresSolver to obtain the optimal estimate of the system state variables.

[0105] The present invention optimizes and solves the state variables in formula (9) by minimizing the sum of all residuals in the sliding window. The objective function is defined as shown in formula (10):

[0106] (10);

[0107] Where, is the marginalized prior information, is the pseudolite position factor constructed by formula (4), is the set of all pseudolite position measurements corresponding to the state variable time within the sliding window, is the covariance matrix of pseudolite position factors; is the pseudo-satellite angle factor of the antenna array constructed by formula (5), is the set of all antenna array pseudolite angle measurements corresponding to the state variable time within the sliding window, is the covariance matrix of the pseudolite angle factor of the antenna array.

[0108] is the IMU pre-integration factor constructed by formula (7), is the set of all IMU measurements within the sliding window, is the covariance matrix of the IMU pre-integration factor; is the visual factor constructed by formula (8), is the set of feature points observed by at least two frames of images within the sliding window, is the covariance matrix of the visual factors.

[0109] The pseudolite position factor, antenna array pseudolite angle factor, IMU pre-integration factor, and vision factor need to be converted into Mahalanobis distances by taking the inverse of their respective covariance matrices and then weighted summed. Equation (10) is solved using the nonlinear optimization library CeresSolver to obtain the optimal estimates of the system state variables.

[0110] Therefore, the present invention adopts the above-mentioned multi-modal pseudolite multi-source fusion positioning method based on factor graph optimization, unifies the indoor and outdoor positioning hardware architecture, and overcomes the limitations of traditional methods in complex scenarios; by designing a uniform circular antenna array and using IMU-assisted optimization, the accuracy and robustness of pseudolite angle solution are improved, and information can be flexibly fused according to the number of pseudolites and DOP values, thereby enhancing the applicability of the algorithm.

[0111] Finally, it should be noted that 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 preferred embodiments, those skilled in the art should understand that they can still modify or replace the technical solutions of the present invention with equivalents, and these modifications or equivalent replacements cannot cause the modified technical solutions to deviate from the spirit and scope of the technical solutions of the present invention.

Claims

1. A multi-modal pseudolite multi-source fusion positioning method based on factor graph optimization, characterized in that: The following steps are involved: S1. Use the improved visual inertial odometry system VIO to predict the carrier's position and posture; S2, using the carrier posture prediction value to assist the antenna array pseudolite to perform angle calculation, and at the same time calculate the position information using the traditional pseudolite positioning method; S3. Build a fusion positioning system based on factor graph optimization to solve all state variables in the sliding window; S4. Optimize the state variables by minimizing the sum of all residuals in the sliding window, and solve the objective function using the nonlinear optimization library CeresSolver to obtain the optimal estimate of the system state variables; In S3, a fusion positioning system based on factor graph optimization is constructed, including constructing the state variables to be solved in the system and constructing the factors connecting the state variables. The factors connecting the state variables include pseudolite position factors, antenna array pseudolite angle factors, IMU pre-integration factors and vision factors. In S3, the pseudolite position factor is defined as the difference between the carrier position and the pseudolite position. The pseudolite position factor between the frame carrier position and the pseudolite position is shown in formula (1): （1）； Where, is the set of all pseudolite position measurements corresponding to the state variable time within the sliding window, is the pseudolite position factor; For the Frame carrier position, For the The frame carrier state variable corresponds to the pseudolite position measurement at the moment, 、 The rotation matrix and translation vector between the navigation coordinate system and the traditional pseudolite positioning coordinate system; When the number of visible pseudolites is sufficient and the DOP value is below a certain threshold, the pseudolites position factor is added to the fusion positioning system optimized by the factor graph; The pseudo-satellite angle factor of the antenna array is defined as the difference between the predicted azimuth and elevation angles of the carrier and the measured azimuth and elevation angles of the pseudo-satellite of the antenna array. The antenna array pseudolite angle factor between the frame carrier state and the antenna array pseudolite angle measurement is shown in formula (2): （2）； Where, is the set of all antenna array pseudolite angle measurements corresponding to the state variable time within the sliding window; is the pseudolite angle factor of the antenna array; For the The position of the frame carrier in the pseudolite array antenna coordinate system is specifically expressed as shown in formula (3): （3）； Indicates the position of the k-th frame carrier, Indicates the position and posture of the pseudolite array antenna in the navigation coordinate system; 、 are the azimuth and elevation angle measurements of the antenna array pseudolite at the time corresponding to the carrier state variable of the kth frame; When the number of visible pseudolites is insufficient or the DOP value is higher than a certain threshold, the pseudolites angle factor of the antenna array is added to the fusion positioning system of factor graph optimization; The IMU pre-integration factor is defined as the difference between the pre-integration prediction value and the pre-integration estimate value. Frame and The IMU pre-integration factor between frame images is shown in formula (4): （4）； Where, is the IMU pre-integration factor, is the set of all IMU measurements within the sliding window, For the The rotation matrix of the frame carrier pose, 、 Respectively Frame and The carrier position of the frame, 、 Respectively Frame and The carrier speed of the frame, Two image frames and The time difference between 、 Respectively Frame and The quaternion of the frame carrier's attitude, Indicates the extraction of quaternion The vector part of is used to represent the three-dimensional rotation residual; 、 and are the estimated values ​​of position pre-integration, velocity pre-integration and rotation pre-integration respectively; In the feature point method VO, the visual factor is defined as the reprojection error. There are two ways to parameterize the feature points in the visual reprojection error: the three-dimensional position parameterization of the feature points in the navigation system and the inverse depth parameterization of the feature points in the image frame. In the inverse depth parameterization method, the feature point is modeled as the inverse of the feature depth in a certain image frame. Feature points on the frame image The inverse depth of , then the visual factor is as shown in formula (5): （5）； Where, is the visual factor, is the set of feature points observed by at least two frames of images within the sliding window, 、 Feature points In the Frame and Frame camera coordinates on the normalized image plane, Feature points From The frame is transformed into the After the frame, in the camera coordinate system The axis weight, 、 Respectively Frame and The carrier pose of the frame, is the external parameter between the camera and IMU.

2. The multi-modal pseudolite multi-source fusion positioning method based on factor graph optimization according to claim 1 is characterized in that: In S1, the improved VIO is obtained by integrating multimodal pseudo-satellite information into the visual inertial odometry system VIO. The multimodal pseudo-satellite includes antenna array pseudo-satellites and traditional pseudo-satellites. The visual camera is used to obtain environmental feature information, and the high-frequency characteristics of the inertial measurement unit (IMU) output are used to combine the state estimation results of the previous moment to predict the carrier's posture.

3. The multi-modal pseudolite multi-source fusion positioning method based on factor graph optimization according to claim 2 is characterized in that: In S1, in the antenna array pseudo-satellite system, the multi-channel pseudo-satellite base station synchronously transmits signals with the same carrier phase through the antenna array. The carrier phase difference information is combined with the signal wavelength and the geometric structure of the antenna array to estimate the signal's arrival angle, including azimuth and elevation, under single-point deployment conditions.

4. The multi-modal pseudolite multi-source fusion positioning method based on factor graph optimization according to claim 3 is characterized in that: In S1, a uniform circular antenna array is used, and the array elements are evenly distributed in a radius of R On the circle, the distance between adjacent array elements is less than half the wavelength of the pseudo-satellite signal; Assume that there are three different and non-collinear array elements in the uniform circular array 、 、 The carrier phases of the transmitted pseudo-satellite signals when they reach the satellite receiver are 、 、 , then the array element 、 , array element 、 The carrier phase difference between 、 Calculate using formula (6): （6）； Where, 、 Array elements 、 , array element 、 The carrier phase difference ambiguity between them.

5. The multi-modal pseudolite multi-source fusion positioning method based on factor graph optimization according to claim 4 is characterized in that: In S2, the carrier phase difference between the two groups of array elements is used to solve the azimuth angle of the satellite receiver in the pseudo-satellite array antenna coordinate system through formula (7): and pitch angle information: （7）； in is the signal wavelength, is the total number of array elements of pseudolite array antenna, 、 Array elements 、 , array element 、 The spacing between.

6. The multi-modal pseudolite multi-source fusion positioning method based on factor graph optimization according to claim 5, characterized in that: In S2, inertial information is used to assist in optimizing the pseudo-satellite angle solution of the antenna array. The state prediction process between two adjacent frames of inertial measurement is shown in formula (8): （8）； Where, 、 、 They are The carrier position, speed, and attitude prediction values ​​at the moment, 、 、 They are The carrier position, speed, and attitude prediction values ​​at the moment, 、 are the median acceleration and median angular velocity of the two frames of inertial measurement, is the time difference between two frames of inertial measurement, Represents quaternion multiplication operations; Using an iterative approach Momentary visual frame and All IMU data between pseudolite measurement frames at time are calculated by formula (8) to obtain The carrier position prediction value at time ; Combined with the position and attitude of the pseudo-satellite array antenna in the navigation coordinate system, calculate the azimuth prediction value of the satellite receiver in the pseudo-satellite array antenna coordinate system , pitch angle prediction value ; And the angle prediction value is converted into 、 The solution obtained in formula (7) 、 to integrate.

7. The multi-modal pseudolite multi-source fusion positioning method based on factor graph optimization according to claim 6, characterized in that: In S3, the state variables of the fusion positioning system based on factor graph optimization include all carrier states in the sliding window, IMU zero bias, and inverse depth of feature points, as shown in formula (9): （9）； Where, Indicates the Frame variables to be optimized, including the position of the carrier in the navigation coordinate system ,speed , attitude quaternion , accelerometer bias and gyroscope bias , is the inverse depth of the feature point.

8. The multi-modal pseudolite multi-source fusion positioning method based on factor graph optimization according to claim 7, characterized in that: In S4, the state variables in formula (9) are optimized by minimizing the sum of all residuals in the sliding window. The objective function is defined as shown in formula (10): （10）； Where, is the marginalized prior information, is the covariance matrix of pseudolite position factors; is the covariance matrix of the pseudolite angle factor of the antenna array; is the covariance matrix of the IMU pre-integration factor; is the covariance matrix of the visual factor; Formula (10) is solved by the nonlinear optimization library CeresSolver to obtain the optimal estimate of the system state variables.

Images