Multi-modal pseudo-satellite multi-source fusion positioning method based on factor graph optimization

Abstract

The invention relates to the technical field of satellite navigation, and particularly discloses a multi-modal pseudo-satellite multi-source fusion positioning method based on factor graph optimization, which comprises the following steps: S1, predicting a carrier pose by using an improved visual inertial odometer system VIO; s2, assisting an antenna array pseudo satellite to carry out angle calculation through the carrier pose predicted value, and calculating position information through a traditional pseudo satellite positioning method; s3, constructing a fusion positioning system based on factor graph optimization, and solving all state variables in the sliding window; and S4, performing optimization solution on the state variable by minimizing the sum of all residual errors in the sliding window, and solving the objective function through the nonlinear optimization library to obtain an optimal estimation value of the system state variable. According to the multi-modal pseudo-satellite multi-source fusion positioning method based on factor graph optimization, the defect of a single positioning means is effectively overcome, and the reliability and precision of a positioning system in a complex environment are improved.

Description

Technical Field

[0001] The present invention relates to the technical field of satellite navigation, 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) has been widely used in fields such as robot navigation, unmanned aerial vehicle autonomous flight, virtual reality, and augmented reality due to its advantages such as high precision, low cost, and miniaturization. The VIO system obtains environmental feature information through a camera and combines the motion information of an Inertial Measurement Unit (IMU) to achieve high-precision local pose estimation. However, due to the influence of environmental interference and measurement noise, the VIO system will generate large cumulative errors 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. To improve the positioning accuracy and robustness of the system, fusing global constraint information provided by other global sensors has become an effective solution.

[0003] The Global Navigation Satellite System (GNSS) can provide globally covered and drift-free absolute position information. Fusing GNSS information with VIO information is a common method to improve the positioning accuracy and long-term stability of the system. By fusing the local pose information solved by VIO with the global position information provided by GNSS in pose graph optimization, cumulative errors are eliminated, and local accurate and globally drift-free pose estimation is achieved. However, GNSS needs to receive at least four satellite signals to calculate the position. Insufficient available satellites in complex environments will seriously affect the positioning accuracy. To address this problem, the raw GNSS observables can be tightly coupled with visual and inertial information to improve the usability and robustness of the positioning system in complex urban environments. However, GNSS cannot provide effective global constraints in indoor and underground spaces with signal rejection. Therefore, researchers have proposed methods to fuse other global information sources, such as magnetometers, Ultra Wide Band (UWB), pseudolites, etc.

[0004] By fusing visual, inertial measurement, and magnetometer information, the heading drift of the navigation system is suppressed using geomagnetic information, enabling more accurate positioning. Although the magnetometer can provide global attitude constraints, in indoor environments, the magnetic field information is susceptible to interference from metal objects and is thus not applicable. Fusing the results of individual UWB ranging into the VIO system can reduce positioning drift. First, the position information is calculated using multiple UWB base stations, and then the UWB position information is fused with visual odometry (VO) and IMU information to improve the positioning accuracy of the system. However, UWB is not compatible with GNSS in terms of signal format, making the positioning system in indoor and outdoor environments rely on different hardware. In contrast, the pseudolite technology uses the same signal format as GNSS and can be directly compatible with existing GNSS satellite receivers, avoiding additional hardware modification costs. In the outdoor environment, GNSS / visual / inertial fusion has become the mainstream solution for high-precision positioning, and pseudolites can provide stable global constraint information in environments where GNSS signals are limited. Fusing pseudolite information can enable the entire system to maintain a consistent hardware architecture in indoor and outdoor environments, thus achieving seamless indoor and outdoor positioning with local high precision and global drift-free performance.

[0005] Existing technologies can fuse the relative pose information calculated by VIO with the global position and velocity information calculated by pseudolites to eliminate the cumulative error of VIO and effectively improve the accuracy and reliability of indoor positioning. However, this method cannot guarantee the positioning performance when the number of visible pseudolites is insufficient or the dilution of precision (DOP) value is high. Traditional pseudolite positioning methods calculate the position information of the carrier using multiple pseudolites and cannot provide reliable measurement information when the number of visible satellites is insufficient or the geometric configuration is poor; the pseudolite positioning method based on fingerprint matching technology has high data acquisition and storage costs and limited generalization ability, making it difficult to provide global drift-free positioning information. Summary of the Invention

[0006] The objective of the present invention is to provide a multi-modal pseudolite multi-source fusion positioning method based on factor graph optimization. A uniform circular antenna array structure is designed, which can estimate the azimuth and elevation angles of the satellite receiver under single-point deployment conditions, and uses IMU information to improve the accuracy and robustness of the azimuth and elevation angle calculations; at the same time, the position information of the satellite receiver is estimated using traditional pseudolite positioning methods. Based on factor graph optimization technology, a multi-modal pseudolite / visual / inertial fusion positioning algorithm is designed. When the number of visible pseudolites is sufficient and the DOP value is low, the pseudolite position information is fused with visual and inertial information; when the number of visible pseudolites is insufficient or the DOP value is high, the pseudolite angle measurement is fused with visual and inertial measurements. The positioning ability of the algorithm in indoor environments and its seamless indoor and outdoor positioning ability after fusing GNSS information are verified through actual tests.

[0007] To achieve the above object, the present invention provides a multi-modal pseudolite multi-source fusion positioning method based on factor graph optimization, including the following steps: S1. Use an improved visual inertial odometry system VIO to predict the pose of the carrier; S2. Assist the antenna array pseudolite to perform angle calculation through the predicted value of the carrier pose, and at the same time solve the position information through the traditional pseudolite positioning method; S3. Construct a fusion positioning system based on factor graph optimization to solve all state variables within the sliding window; S4. Optimize and solve the state variables by minimizing the sum of all residuals within the sliding window, and solve the objective function through the non-linear optimization library CeresSolver to obtain the optimal estimated value of the system state variables.

[0008] Preferably, in S1, in the visual inertial odometry system VIO, multi-modal pseudolite information is fused to obtain the improved VIO. The multi-modal pseudolites include antenna array pseudolites and traditional pseudolites. The visual camera is used to obtain environmental feature information, and the high-frequency characteristics output by the inertial measurement unit IMU are used to predict the pose of the carrier in combination with the state estimation result of the previous moment.

[0009] Preferably, in S1, in the antenna array pseudolite system, the multi-channel pseudolite base stations synchronously transmit signals with the same carrier phase through the antenna array. Using the carrier phase difference information and combining the signal wavelength and the geometric structure of the antenna array, the angle of arrival of the signal, including the azimuth angle and the elevation angle, is estimated under the single-point deployment condition.

[0010] Preferably, in S1, a uniform circular antenna array is adopted, and the array elements are evenly distributed on a circle with a radius of R , and the distance between adjacent array elements is less than half of the pseudolite signal wavelength; Suppose three different and non-collinear array elements , , in the uniform circular array transmit pseudolite signals, and the carrier phases when the signals reach the satellite receiver are , , , respectively. Then the carrier phase differences , between array elements , are calculated by formula (1): , (1); (1); In the formula, , They are array elements respectively , , the carrier phase difference ambiguity between array elements , .

[0011] Preferably, in S2, the azimuth angle and elevation angle information of the satellite receiver in the pseudo-satellite array antenna coordinate system are solved by using the carrier phase difference between two groups of array elements through formula (2): (2); wherein is the signal wavelength, is the total number of array elements of the pseudo-satellite array antenna, , are array elements , respectively, and the spacing between array elements , is

[0012] Preferably, in S2, the inertial information is used to assist in optimizing the pseudo-satellite angle calculation result of the antenna array. The state prediction process between two adjacent inertial measurements is shown in formula (3): (3); In the formula, , , are the predicted values of the carrier position, velocity, and attitude at time respectively, , , are the predicted values of the carrier position, velocity, and attitude at time respectively, , are the median acceleration and median angular velocity of two frames of inertial measurements respectively, is the time difference between two frames of inertial measurements, represents the quaternion multiplication operation; The iterative method is used to perform operations on all IMU data between the visual frame at time k and the pseudo-satellite measurement frame at time j through formula (3) to obtain the predicted value j of the carrier position at time ; combined with the position and attitude of the pseudo-satellite array antenna in the navigation coordinate system, the predicted value of the azimuth angle and the predicted value of the elevation angle of the satellite receiver in the pseudo-satellite array antenna coordinate system are calculated; and the angle predicted values , Fuse with the solution calculated in formula (2). and Perform fusion.

[0013] Preferably, in S3, construct a fusion positioning system optimized based on a factor graph, including constructing state variables to be solved in the system and 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.

[0014] Preferably, 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 carrier position and the pseudolite position in the k th frame within the sliding window is shown in formula (4): (4); In the formula, is the carrier position in the k th frame, is the pseudolite position measurement corresponding to the carrier state variable at the moment of the k th frame, and are 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 lower than a certain threshold, add the pseudolite position factor to the fusion positioning system optimized by the factor graph; The antenna array pseudolite angle factor is defined as the difference between the predicted azimuth and elevation angles of the carrier and the measured azimuth and elevation angles of the antenna array pseudolite. The antenna array pseudolite angle factor between the carrier state and the antenna array pseudolite angle measurement in the k th frame within the sliding window is shown in formula (5): (5); In the formula, is the position of the carrier in the kth frame in the pseudolite array antenna coordinate system, and the specific representation is shown in formula (6): k (6); represents the carrier position in the kth frame, represents the pose of the pseudolite array antenna in the navigation coordinate system; and are the measured azimuth and elevation angles of the antenna array pseudolite corresponding to the carrier state variable at the kth frame respectively; When the number of visible pseudolites is insufficient or the DOP value is higher than a certain threshold, add the antenna array pseudolite angle factor to the fusion positioning system optimized by the factor graph; The IMU pre-integration factor is defined as the difference between the pre-integration predicted value and the pre-integration estimated value. The IMU pre-integration factor between the k th k frame and the th + 1 frame in the sliding window is shown in Equation (7): In the formula, is the rotation matrix of the carrier attitude at the k th frame, , are the carrier positions at the k th frame and the k th + 1 frame respectively, , are the carrier velocities at the k th frame and the k th + 1 frame respectively, is the time difference between the two image frames k and k th + 1, , are the quaternions of the carrier attitude at the k th frame and the k th + 1 frame respectively, represents the vector part of the quaternion , which 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 parameterization methods of feature points in the visual reprojection error: the three-dimensional position parameterization of feature points in the navigation system and the inverse depth parameterization of feature points in the image frame; In the inverse depth parameterization method, the feature point is modeled as the reciprocal of the feature depth in a certain image frame. Assuming that the inverse depth of the feature point on the th frame image is , then the visual factor is shown in Equation (8): (8); In the formula, , are the coordinates of the feature point on the camera normalized image plane of the th frame and the j th frame respectively, is the component of the feature point on the th frame after coordinate transformation to the j th frame in the z axis of the camera coordinate system, , They are respectively the frame and the j carrier pose of the frame, and is the extrinsic parameter between the camera and the IMU.

[0015] Preferably, in S3, the state variables of the fusion positioning system optimized based on the factor graph include all carrier states, IMU biases, and feature point inverse depths within the sliding window, as shown in Equation (9): (9); In the formula, represents the k variables to be optimized for the frame, including the position , velocity , attitude quaternion , accelerometer bias and gyroscope bias of the carrier in the navigation coordinate system, and is the feature point inverse depth.

[0016] Preferably, in S4, the state variables in Equation (9) are optimized and solved by minimizing the sum of all residuals within the sliding window, and the objective function is defined as shown in Equation (10): (10); In the formula, is the marginalized prior information, is the pseudo-satellite position factor constructed by Equation (4), is the set of all pseudo-satellite position measurements corresponding to the state variable time within the sliding window, is the covariance matrix of the pseudo-satellite position factor; is the antenna array pseudo-satellite angle factor constructed by Equation (5), is the set of all antenna array pseudo-satellite angle measurements corresponding to the state variable time within the sliding window, is the covariance matrix of the antenna array pseudo-satellite angle factor; is the IMU pre-integration factor constructed by Equation (7), is the set of all IMU measurements within the sliding window, is the covariance matrix of the IMU pre-integration factor; is the vision factor constructed by Equation (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 vision factor; Equation (10) is solved by the nonlinear optimization library Ceres Solver to obtain the optimal estimated values of the system state variables.

[0017] 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: (1) Aiming at 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, the present invention effectively improves the positioning accuracy and robustness by fusing multi-modal pseudolite information, meeting the positioning requirements of relevant application scenarios.

[0018] (2) The present invention uses the factor graph optimization method to fuse various measurement information. Compared with the filtering-based method, it can perform multiple state variable linearizations and jointly estimate all states of the system using historical information, solve all state variables within the sliding window, improve the state estimation accuracy, and achieve high-precision and drift-free estimation of the carrier pose.

[0019] The technical solution of the present invention will be further described in detail below through the accompanying drawings and embodiments. Brief Description of the Drawings

[0020] Figure 1 is the overall architecture of an embodiment of the multi-modal pseudolite multi-source fusion positioning method based on factor graph optimization of the present invention; Figure 2 is the pseudolite uniform circular antenna array model of an embodiment of the multi-modal pseudolite multi-source fusion positioning method based on factor graph optimization of the present invention; Figure 3 is the schematic diagram of data fusion based on factor graph optimization of an embodiment of the multi-modal pseudolite multi-source fusion positioning method based on factor graph optimization of the present invention. Detailed Embodiments

[0021] The technical solution of the present invention will be further described below through the accompanying drawings and embodiments.

[0022] Unless otherwise defined, the technical terms or scientific terms used in the present invention shall have the ordinary meaning understood by those of ordinary skill in the field to which the present invention belongs. The "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. The terms such as "including" or "comprising" mean that the elements or objects appearing before this word cover the elements or objects listed after this word and their equivalents, without excluding other elements or objects. The terms such as "connected" or "coupled" are not limited to physical or mechanical connections, but may include electrical connections, whether direct or indirect. The terms such as "upper", "lower", "left", "right" are only used to represent relative positional relationships, and when the absolute position of the object being described changes, the relative positional relationship may also change accordingly.

[0023] As Figure 1 shown, a multi-modal pseudolite multi-source fusion positioning method based on factor graph optimization includes the following steps: S1. Predict the carrier pose using an improved visual inertial odometer system VIO; In the traditional visual inertial odometer system VIO, multi-modal pseudolite information is fused to obtain the improved VIO. The multi-modal pseudolites include antenna array pseudolites and traditional pseudolites. The visual camera is used to obtain environmental feature information, and the high-frequency characteristics output by the inertial measurement unit IMU are combined with the state estimation result of the previous moment to predict the carrier pose.

[0024] In the antenna array pseudolite system, the multi-channel pseudolite base stations synchronously transmit signals with the same carrier phase through the antenna array. Due to the position differences between the elements of the antenna array, the signals have different carrier phases when reaching the receiver. Using the carrier phase difference information and combining the signal wavelength and the geometric structure of the antenna array, the angle of arrival of the signal, including the azimuth angle and the elevation angle, can be estimated under the condition of single-point deployment.

[0025] As Figure 2 shown, the present invention adopts a uniform circular antenna array, and the elements are uniformly distributed on a circle with a radius of R , and the distance between adjacent elements is less than half of the pseudolite signal wavelength.

[0026] Let three different and non-collinear elements , , in the uniform circular array transmit pseudolite signals, and the carrier phases when reaching the satellite receiver are , , respectively. Then the carrier phase differences , between elements , , and the carrier phase differences , between elements can be calculated by formula (1): In the formula, , are the carrier phase difference ambiguities between elements , , and between elements , respectively. When the distance between different elements is greater than half of the signal wavelength, the ambiguity needs to be solved.

[0027] S2. Assist the antenna array pseudolite in angle calculation through the predicted value of the carrier pose, and at the same time calculate the position information through the traditional pseudolite positioning method.

[0028] Using the carrier phase difference between two sets of array elements, the azimuth angle of the satellite receiver in the coordinate system of the pseudolite array antenna can be solved through formula (2) and the elevation angle information: (2); where is the signal wavelength, is the total number of array elements of the pseudolite array antenna, , are respectively the array elements , , the distance between the array elements , .

[0029] In practical applications, the angle estimation of the antenna array pseudolite is easily affected by factors such as multipath signals and environmental noise, resulting in jumps in the angle measurement results and affecting the stability of azimuth angle and elevation angle measurements. In contrast, the IMU has a high output frequency and is not affected by the external environment, and can provide high-precision pose estimation results in a short time.

[0030] Using inertial information to assist in optimizing the angle calculation results of the antenna array pseudolite can effectively improve the accuracy and robustness of angle calculation. The state prediction process between two adjacent inertial measurements is shown in formula (3): (3); In the formula, , , are respectively the predicted values of the carrier position, velocity, and attitude at time, , , are respectively the predicted values of the carrier position, velocity, and attitude at time, , are respectively the median acceleration and median angular velocity of two frames of inertial measurements, is the time difference between two frames of inertial measurements, represents the multiplication operation of quaternions.

[0031] Adopt an iterative method to perform operations on all IMU data between the visual frame at k time and the pseudolite measurement frame at j time through formula (3) to obtain the predicted value of the carrier position at j time ; Calculate the predicted azimuth value of the satellite receiver in the pseudo-satellite array antenna coordinate system by combining the position and attitude of the pseudo-satellite array antenna in the navigation coordinate system. and the predicted elevation angle value ; And use the Kalman filtering method to fuse the angle prediction values , with the , solved in formula (2) to ensure the stability of the angle information.

[0032] S3. Construct a fusion positioning system optimized based on factor graph to solve all state variables within the sliding window.

[0033] The filtering-based method only performs linearization once during data fusion and only estimates the state variables at the current moment. Therefore, the errors generated during the state estimation process will accumulate in the system; while the optimization-based method can perform linearization of the state variables multiple times and jointly estimate all states of the system with historical information, with higher accuracy. Therefore, the present invention adopts the factor graph optimization method to fuse the traditional pseudo-satellite position measurement, antenna array pseudo-satellite angle measurement, IMU measurement and vision measurement to solve all state variables within the sliding window.

[0034] As Figure 3 shown, construct a fusion positioning system optimized based on factor graph, including constructing the state variables to be solved by the system and constructing the factors connecting the state variables. The factors connecting the state variables include pseudo-satellite position factor, antenna array pseudo-satellite angle factor, IMU pre-integration factor and vision factor. Add the constructed factors to the fusion positioning system optimized by factor graph to achieve the optimal estimation of the system state within the sliding window.

[0035] Pseudo-satellite position factor: The pseudo-satellite position factor is defined as the difference between the carrier position and the pseudo-satellite position. The pseudo-satellite position factor between the carrier position and the pseudo-satellite position in the k th frame within the sliding window is shown in formula (4): (4); In the formula, is the carrier position in the k th frame, is the pseudo-satellite position measurement corresponding to the carrier state variable at the moment of the k th frame, , are the rotation matrix and translation vector between the navigation coordinate system and the traditional pseudo-satellite positioning coordinate system.

[0036] When the number of visible pseudo-satellites is sufficient and the DOP value is lower than a certain threshold, add the pseudo-satellite position factor to the fusion positioning system optimized by factor graph.

[0037] Antenna array pseudolite angle factor: The antenna array pseudolite angle factor is defined as the difference between the predicted values of the carrier azimuth and pitch angles and the measured values of the antenna array pseudolite azimuth and pitch angles. The antenna array pseudolite angle factor between the carrier state of the k th frame within the sliding window and the antenna array pseudolite angle measurement is shown in Equation (5): (5); In the formula, is the position of the k th frame carrier in the antenna coordinate system of the pseudolite array, and the specific representation is shown in Equation (6): (6); represents the position of the kth frame carrier, represents the pose of the pseudolite array antenna in the navigation coordinate system; , are respectively the measured values of the antenna array pseudolite azimuth and pitch angles corresponding to the carrier state variables at the kth frame.

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

[0039] IMU pre-integration factor: The IMU pre-integration factor is defined as the difference between the pre-integration predicted value and the pre-integration estimated value. The IMU pre-integration factor between the k th frame and the k +1th frame images within the sliding window is shown in Equation (7): (7); In the formula, is the rotation matrix of the k th frame carrier attitude, , are respectively the positions of the k th frame and the k +1th frame carriers, , are respectively the velocities of the k th frame and the k +1th frame carriers, is the time difference between the two image frames k and k +1, , are respectively the quaternions of the k th frame and the k +1th frame carrier attitudes, Represents the extraction of quaternions The vector part of, which is used for the representation of three-dimensional rotation residuals; , and Are the estimated values of position pre-integration, velocity pre-integration, and rotation pre-integration respectively.

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

[0041] In the inverse depth parameterization method, the feature point is modeled as the reciprocal of the feature depth in a certain image frame. Assuming that the inverse depth of the feature point On the Frame image is , then the visual factor is as shown in formula (8): (8); In the formula, , Are the coordinates of the feature point On the Frame and the j Frame camera normalized image plane, Is the feature point From the Frame through coordinate transformation to the j Frame and then in the camera coordinate system z Axis component, , Are the carrier poses of the Frame and the j Frame respectively, Is the external parameter between the camera and the IMU.

[0042] System state variables and objective function: The state variables of the fusion positioning system based on factor graph optimization include all carrier states, IMU biases, and feature point inverse depths within the sliding window, as shown in formula (9): (9); In the formula, Represents the variable to be optimized in the k Frame, including the position , velocity , attitude quaternion , accelerometer bias And gyroscope bias , Is the feature point inverse depth.

[0043] S4. Optimize and solve the state variables by minimizing the sum of all residuals within the sliding window, and solve the objective function through the nonlinear optimization library Ceres Solver to obtain the optimal estimated value of the system state variables.

[0044] In the present invention, the state variables in formula (9) are optimized and solved by minimizing the sum of all residuals within the sliding window, and the objective function is defined as shown in formula (10): (10); In the formula, is the marginalized prior information, is the pseudolite position factor constructed through 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 the pseudolite position factor; is the antenna array pseudolite angle factor constructed through 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 antenna array pseudolite angle factor.

[0045] is the IMU pre-integration factor constructed through 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 vision factor constructed through 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 vision factor.

[0046] The pseudolite position factor, the antenna array pseudolite angle factor, the IMU pre-integration factor, and the vision factor need to be converted into Mahalanobis distance through the inverse of their respective covariance matrices for weighted summation. Solve formula (10) through the nonlinear optimization library Ceres Solver to obtain the optimal estimated value of the system state variables.

[0047] 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 calculation are improved, and information can be flexibly fused according to the number of pseudolites and DOP value, enhancing the applicability of the algorithm.

[0048] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit them. Although the present invention has been described in detail with reference to the preferred embodiments, those of ordinary skill in the art should understand that they can still modify the technical solutions of the present invention or make equivalent replacements, and these modifications or equivalent replacements cannot make the modified technical solutions 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 pseudo-satellite to perform angle calculation, and at the same time calculate the position information through the traditional pseudo-satellite positioning method; S3, build a fusion positioning system based on factor graph optimization to solve all state variables in the sliding window; S4. The state variables are optimized by minimizing the sum of all residuals in the sliding window, and the objective function is solved by the nonlinear optimization library CeresSolver to obtain the optimal estimate of the system state variables.

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 in the visual inertial odometry system VIO. 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 to predict the carrier's posture in combination with the state estimation result of the previous moment.

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, 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, 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 (1): （1）； In the formula, , Array element , , 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 of the satellite receiver in the pseudo-satellite array antenna coordinate system through formula (2): and pitch angle information: （2）； in is the signal wavelength, is the total number of array elements of pseudo-satellite array antenna, , Array element , , array element , The spacing between.

6. The multi-modal pseudolite multi-source fusion positioning method based on factor graph optimization according to claim 5 is 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 (3): （3）； In the formula, , , 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, respectively. is the time difference between two frames of inertial measurement, Represents quaternion multiplication; 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 predicted value of the carrier position at time ; Combined with the position and attitude of the pseudo-satellite array antenna in the navigation coordinate system, the azimuth prediction value of the satellite receiver in the pseudo-satellite array antenna coordinate system is calculated. , Pitch angle prediction value ; And the angle prediction value is converted into , The solution obtained in formula (2) , To merge.

7. The multi-modal pseudolite multi-source fusion positioning method based on factor graph optimization according to claim 6 is characterized in that: In S3, 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 pseudo-satellite position factors, antenna array pseudo-satellite angle factors, IMU pre-integration factors and vision factors.

8. The multi-modal pseudolite multi-source fusion positioning method based on factor graph optimization according to claim 7 is characterized in that: In S3, the pseudo-satellite position factor is defined as the difference between the carrier position and the pseudo-satellite position. k The pseudolite position factor between the frame carrier position and the pseudolite position is shown in formula (4): （4）； In the formula, For the k Frame carrier position, For the k The frame carrier state variable corresponds to the pseudo-satellite position measurement at the time, , It is the rotation matrix and translation vector between the navigation coordinate system and the traditional pseudo-satellite 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. k The antenna array pseudolite angle factor between the frame carrier state and the antenna array pseudolite angle measurement is shown in formula (5): （5）； In the formula, For the k The position of the frame carrier in the pseudo-satellite array antenna coordinate system is specifically expressed as shown in formula (6): （6）； represents the position of the carrier of the kth frame, 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 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 optimized by the factor graph; 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): （7）； In the formula, For the k The rotation matrix of the frame carrier attitude, , 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 attitude quaternion, Represents 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; 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 frame image The inverse depth is , then the visual factor is as shown in formula (8): （8）； In the formula, , The feature points In the Frame and j The frame camera coordinates on the normalized image plane, For feature points From The frame is transformed to j After the frame, in the camera coordinate system z The weight of the axis, , Respectively Frame and j The carrier pose of the frame, is the external reference between the camera and IMU.

9. The multi-modal pseudolite multi-source fusion positioning method based on factor graph optimization according to claim 8 is 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）； In the formula, Indicates k Frame variables to be optimized, including the position of the carrier in the navigation coordinate system ,speed , attitude quaternion , Accelerometer bias and gyro bias , is the inverse depth of the feature point.

10. The multi-modal pseudolite multi-source fusion positioning method based on factor graph optimization according to claim 9, 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）； In the formula, is the marginalized prior information, is the pseudo-satellite 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 pseudo-satellite 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 pseudo-satellite angle factor of the antenna array; 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 in the sliding window, is the covariance matrix of the visual factor; the nonlinear optimization library CeresSolver is used to solve formula (10) to obtain the optimal estimate of the system state variables.

Images