Multi-modal pseudolite multi-source fusion positioning method based on factor graph optimizationTechnical Field
The invention relates to the technical field of satellite navigation, in particular to a multi-modal pseudolite multi-source fusion positioning method based on factor graph optimization.
Background
The visual inertial odometer (VisualInertialOdometry, VIO) is widely applied to the fields of robot navigation, unmanned aerial vehicle autonomous flight, virtual reality, augmented reality and the like due to the advantages of high precision, low cost, miniaturization and the like. The VIO system obtains environmental characteristic information through a camera and combines the motion information of the inertial measurement unit (InertialMeasurementUnit, IMU) to realize local high-precision pose estimation. However, due to the influence of environmental interference and measurement noise, the VIO system can generate larger accumulated errors in the long-term operation process, and the requirements of high-precision positioning, long-term stable navigation and autonomous operation in a complex environment of the carrier are difficult to meet. In order to improve the positioning accuracy and robustness of the system, the method is an effective solution by fusing global constraint information provided by other global sensors.
The global navigation satellite system (GlobalNavigationSatelliteSystem, GNSS) can provide global coverage, drift-free absolute position information, and the fusion of GNSS information and VIO information is a common means for improving the positioning accuracy and long-term stability of the system. By fusing the position and pose information calculated by VIO with global position information provided by GNSS in position and pose map optimization, accumulated errors are eliminated, and position and pose estimation with local accuracy and global no drift is realized. However, the GNSS needs to receive at least four satellite signals to calculate the position, and in a complex environment, the positioning accuracy is seriously affected by insufficient available satellites, and in order to solve the problem, the GNSS original observed quantity and the visual and inertial information can be tightly coupled and fused, so as to improve the availability and robustness of the positioning system in the complex urban environment. However, GNSS does not provide effective global constraints in indoor and underground spaces where signals are rejected, so researchers have proposed methods to fuse other global information sources, such as magnetometers, ultra wideband (UltraWideBand, UWB), pseudolites, and the like.
By fusing vision, inertia measurement and magnetometer information, the heading drift of the navigation system is restrained by geomagnetic information, so that positioning is more accurate. Magnetometers can provide global attitude constraints, but in indoor environments, magnetic field information is subject to interference from metal objects and is thus not suitable. The single UWB ranging result is fused into the VIO system, so that positioning drift can be reduced, the position information is calculated by utilizing a plurality of UWB base stations, the UWB position information is fused with the visual odometer (VisualOdometry, VO) and the IMU information, and the positioning accuracy of the system is improved. UWB is not physically compatible with GNSS, however, such that positioning systems in indoor and outdoor environments need to rely on different hardware. In contrast, the pseudolite technology adopts the same signal system as the GNSS, can be directly compatible with the existing GNSS satellite receiver, avoids additional hardware transformation cost, and is outdoor, GNSS/vision/inertia fusion becomes a mainstream scheme of high-precision positioning, the pseudolite can provide stable global constraint information in an environment with limited GNSS signals, and the fusion pseudolite information can enable the whole system to maintain a consistent hardware architecture in indoor and outdoor environments, so that the indoor and outdoor seamless positioning effects of local high precision and global drift-free are achieved.
In the prior art, the relative pose information calculated by the VIO is fused with the global position and speed information calculated by the pseudolite, so that the accumulated error of the VIO is eliminated, and the accuracy and reliability of indoor positioning are effectively improved. However, the method cannot guarantee the positioning performance when the number of visible pseudolites is insufficient or the value of the precision attenuation factor (DilutionofPrecision, DOP) is high. The traditional pseudolite positioning method can not provide reliable measurement information when the number of visible satellites is insufficient or the geometric configuration is poor through solving carrier position information by a plurality of pseudolites, and has the advantages of high data acquisition and storage cost, limited generalization capability and difficulty in providing global drift-free positioning information.
Disclosure of Invention
The invention aims to provide a multi-mode pseudolite multi-source fusion positioning method based on factor graph optimization, designs a uniform circular antenna array structure, can realize the estimation of azimuth angle and pitch angle of a satellite receiver under the single-point deployment condition, improves the resolving precision and robustness of the azimuth angle and the pitch angle by using IMU information, and simultaneously estimates the position information of the satellite receiver by using a traditional pseudolite positioning method. And a multi-mode pseudolite/vision/inertia fusion positioning algorithm is designed based on a factor graph optimization technology, when the number of visible pseudolites is sufficient and the DOP value is low, the pseudolites position information and the vision and inertia information are fused, and when the number of the visible pseudolites is insufficient or the DOP value is high, the pseudolites angle measurement and the vision and inertia measurement are fused. The positioning capability of the algorithm in the indoor environment and the indoor and outdoor seamless positioning capability after the GNSS information is fused are verified through an actual test.
In order to achieve the above purpose, the invention provides a multi-modal pseudolite multi-source fusion positioning method based on factor graph optimization, which comprises the following steps:
s1, predicting the pose of a carrier by utilizing an improved visual inertial odometer system VIO;
s2, carrying out angle calculation by using the carrier pose predicted value to assist the antenna array pseudolite, and simultaneously calculating position information by using a traditional pseudolite positioning method;
S3, constructing a fusion positioning system based on factor graph optimization, and solving all state variables in the sliding window;
And S4, optimizing and solving the state variable by minimizing the sum of all residual errors in the sliding window, and solving the objective function by a nonlinear optimization library CeresSolver to obtain the optimal estimated value of the system state variable.
Preferably, in S1, the improved VIO is obtained by integrating the multimode pseudolite information in the visual inertial odometer system VIO, the multimode pseudolite includes an antenna array pseudolite and a conventional pseudolite, the environmental characteristic information is obtained by using a visual camera, and the carrier pose is predicted by using the high-frequency characteristic output by the inertial measurement unit IMU and combining the state estimation result of the last moment.
Preferably, in S1, in the antenna array pseudolite system, the multichannel pseudolite base station synchronously transmits signals with the same carrier phase through the antenna array, and estimates the angle of arrival of the signals under the single-point deployment condition, including azimuth angle and pitch angle, by using carrier phase difference information and combining the signal wavelength and the geometry of the antenna array.
Preferably, in S1, a uniform circular antenna array is adopted, the array elements are uniformly distributed on a circle with a radius R, and the distance between adjacent array elements is smaller than half wavelength of the pseudolite signal;
Setting 3 different non-collinear array elements in uniform circular array、、The carrier phases of the transmitted pseudolite signals when arriving at the satellite receiver are respectively、、Array element、Array element、Carrier phase difference between、Calculation is performed by the formula (1):
(1);
in the formula,、Respectively are array elements、Array element、Carrier phase difference ambiguity between them.
Preferably, in S2, the azimuth angle of the satellite receiver in the pseudo-satellite array antenna coordinate system is solved by using the carrier phase difference between the two groups of array elements through the formula (2)Pitch angleInformation:
(2);
Wherein the method comprises the steps ofAs a function of the wavelength of the signal,Is the total number of array elements of the pseudolite array antenna,、Respectively are array elements、Array element、The spacing between them.
Preferably, in S2, the inertial information is used to assist in optimizing the satellite angle calculation result of the antenna array, and the state prediction process between two adjacent frames of inertial measurement is shown in formula (3):
(3);
in the formula,、、Respectively isPredicted values of carrier position, speed and attitude at the moment,、、Respectively isPredicted values of carrier position, speed and attitude at the moment,、The acceleration median value and the angular velocity median value of two frames of inertial measurement are respectively,For the time difference between two frames of inertial measurements,Multiplication representing a quaternion;
all IMU data between the k moment visual frame and the j moment pseudolite measurement frame are calculated through a formula (3) in an iterative mode to obtain a carrier position predicted value at the j momentCombining the position and the gesture of the pseudo-satellite array antenna under the navigation coordinate system to calculate the azimuth angle predicted value of the satellite receiver under the pseudo-satellite array antenna coordinate systemPitch angle prediction valueAnd the angle predicted value is obtained through a Kalman filtering method、And the solution in the formula (2)、Fusion is performed.
Preferably, in S3, a fused positioning system based on factor graph optimization is constructed, including constructing a state variable to be solved by the system and constructing a factor of a connection state variable, where the factor of the connection state variable includes a pseudolite position factor, an antenna array pseudolite angle factor, an IMU pre-integration factor and a vision factor.
Preferably, in S3, the pseudolite position factor is defined as the difference between the carrier position and the pseudolite position, and the pseudolite position factor between the kth frame carrier position and the pseudolite position in the sliding window is shown in formula (4):
(4);
in the formula,For the carrier position of the kth frame,For pseudolite position measurements at times corresponding to the kth frame carrier state variables,、The rotation matrix and the 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 lower than a certain threshold value, adding a pseudolites position factor into the factor graph optimized fusion positioning system;
The antenna array pseudolite angle factor is defined as the difference between the carrier azimuth angle and pitch angle predicted value and the antenna array pseudolite azimuth angle and pitch angle measured value, and the antenna array pseudolite angle factor between the kth frame carrier state and the antenna array pseudolite angle measurement in the sliding window is shown as a formula (5):
(5);
in the formula,The specific expression of the position of the kth frame carrier under the pseudo satellite array antenna coordinate system is shown as the formula (6):
(6);
indicating the carrier position of the kth frame,Representing the pose of the pseudo satellite array antenna under a navigation coordinate system;、 measuring the azimuth angle and the pitch angle of the antenna array pseudolite at the moment corresponding to the state variable of the kth frame carrier;
when the number of the visible pseudolites is insufficient or the DOP value is higher than a certain threshold value, adding an antenna array pseudolites angle factor into the factor graph optimized fusion positioning system;
The IMU pre-integration factor is defined as the difference between the pre-integration predicted value and the pre-integration estimated value, and the IMU pre-integration factor between the kth frame and the k+1th frame of images in the sliding window is shown in formula (7):
(7);
in the formula,For a rotation matrix of the carrier pose of the kth frame,、Carrier positions for the kth frame and the k +1 frame respectively,、Carrier speeds for the kth frame and the k +1 frame respectively,Is the time difference between two image frames k and k +1,、Quaternion of the carrier pose for the kth frame and the k +1 frame respectively,Representing extraction quaternionFor representation of a three-dimensional rotation residual;、 AndThe estimated values of position pre-integration, speed pre-integration and rotation pre-integration are respectively obtained;
In the feature point method VO, the visual factor is defined as a reprojection error, and two parameterization modes of the feature point in the visual reprojection error are that the three-dimensional position parameterization of the feature point under a navigation system and the inverse depth parameterization of the feature point under an image frame are adopted;
in the inverse depth parameterization, the feature points are modeled as the inverse of the feature depth for a certain image frame, assuming that at the firstFeature points on frame imagesIs of inverse depth ofThe visual factor is as shown in equation (8):
(8);
in the formula,、Respectively as characteristic pointsIn the first placeThe frames and j-th frame cameras normalize coordinates on the image plane,Is the characteristic pointFrom the firstThe z-axis component in the camera coordinate system after the frame is transformed to the j-th frame,、Respectively the firstThe carrier pose of the frame and the j-th frame,Is an external parameter between the camera and the IMU.
Preferably, in S3, the state variables of the fused positioning system optimized based on the factor graph include all carrier states within the sliding window, IMU zero offset, and feature point inverse depth, as shown in formula (9):
(9);
in the formula,Representing the k frame of variables to be optimized, including the position of the carrier in the navigation coordinate systemSpeed and velocity ofGesture quaternionZero offset of accelerometerZero offset of gyroscope,Is the inverse depth of the feature point.
Preferably, in S4, the state variable in equation (9) is optimally 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,For the prior information to be marginalized,For the pseudolite position factor constructed by equation (4),Is the set of all pseudolite position measurements corresponding to the state variable time within the sliding window,Covariance matrix for pseudolite position factor; for the antenna array pseudolite angle factor constructed by equation (5),Is the set of all antenna array pseudolite angle measurements corresponding to the state variable time within the sliding window,Covariance matrix of angle factor of pseudo satellite for antenna array;
for IMU pre-integration factors constructed by equation (7),Is the set of all IMU measurements within the sliding window,Covariance matrix of the IMU pre-integral factor; For the vision factor constructed by formula (8),Is a set of feature points observed by at least two frames of images within the sliding window,And solving the formula (10) through a nonlinear optimization library CeresSolver to obtain the optimal estimated value of the system state variable.
Therefore, the multi-modal pseudolite multi-source fusion positioning method based on factor graph optimization has the following beneficial effects:
(1) Aiming at the problems that the VIO system generates larger accumulated errors due to environmental interference and measurement noise in long-term operation and is difficult to meet the requirements of high-precision positioning, long-term stable navigation and autonomous operation in complex environments, the positioning precision and robustness are effectively improved by fusing multi-mode pseudolite information, and the requirements of related application scenes on positioning are met.
(2) According to the invention, a factor graph optimization method is adopted to fuse various measurement information, compared with a filtering-based method, the method can be used for estimating all states of a system by carrying out multiple-time state quantity linearization and combining historical information, and can solve all state variables in a sliding window, so that the state estimation precision is improved, and the high-precision and drift-free estimation of the carrier pose is realized.
The technical scheme of the invention is further described in detail through the drawings and the embodiments.
Drawings
FIG. 1 is an overall architecture of an embodiment of a multi-modal pseudolite multi-source fusion positioning method based on factor graph optimization of the present invention;
FIG. 2 is a model of a uniform circular antenna array of pseudolites of an embodiment of a multi-modal pseudolite multi-source fusion positioning method based on factor graph optimization of the present invention;
FIG. 3 is a schematic diagram of factor graph optimization-based data fusion for an embodiment of a factor graph optimization-based multi-modal pseudolite multi-source fusion positioning method.
Detailed Description
The technical scheme of the invention is further described below through the attached drawings and the embodiments.
Unless defined otherwise, technical or scientific terms used herein should be given the ordinary meaning as understood by one of ordinary skill in the art to which this invention belongs. The terms "first," "second," and the like, as used herein, do not denote any order, quantity, or importance, but rather are used to distinguish one element from another. The word "comprising" or "comprises", and the like, means that elements or items preceding the word are included in the element or item listed after the word and equivalents thereof, but does not exclude other elements or items. The terms "connected" or "connected," and the like, are not limited to physical or mechanical connections, but may include electrical connections, whether direct or indirect. "upper", "lower", "left", "right", etc. are used merely to indicate relative positional relationships, which may also be changed when the absolute position of the object to be described is changed.
As shown in fig. 1, a multi-modal pseudolite multi-source fusion positioning method based on factor graph optimization comprises the following steps:
s1, predicting the pose of a carrier by utilizing an improved visual inertial odometer system VIO;
The VIO with improved multimode pseudolite information is fused with the VIO of the traditional visual inertial odometer system, the multimode pseudolite comprises an antenna array pseudolite and a traditional pseudolite, the environment characteristic information is obtained by utilizing a visual camera, and the carrier pose is predicted by utilizing the high-frequency characteristic output by an Inertial Measurement Unit (IMU) and combining the state estimation result of the last moment.
In an antenna array pseudolite system, a multichannel pseudolite base station synchronously transmits signals with the same carrier phase through an antenna array, and the signals have different carrier phases when reaching a receiver due to the position difference among array elements of the antenna array. By using carrier wave phase difference information and combining the signal wavelength and the geometric structure of the antenna array, the angle of arrival of the signal can be estimated under the single-point deployment condition, including azimuth angle and pitch angle.
As shown in fig. 2, the invention adopts a uniform circular antenna array, array elements are uniformly distributed on a circle with a radius of R, and the distance between adjacent array elements is smaller than half wavelength of pseudolite signals.
Setting 3 different non-collinear array elements in uniform circular array、、The carrier phases of the transmitted pseudolite signals when arriving at the satellite receiver are respectively、、Array element、Array element、Carrier phase difference between、The calculation can be performed by the formula (1):
(1);
in the formula,、Respectively are array elements、Array element、The ambiguity of the carrier phase difference between the two elements needs to be solved when the distance between the two elements is larger than the half wavelength of the signal.
S2, carrying out angle calculation by using the carrier pose predicted value to assist the antenna array pseudolite, and simultaneously calculating position information by using a traditional pseudolite positioning method.
By utilizing the carrier wave phase difference between the two groups of array elements, the azimuth angle of the satellite receiver under the coordinate system of the pseudo-satellite array antenna can be solved through the formula (2)Pitch angleInformation:
(2);
Wherein the method comprises the steps ofAs a function of the wavelength of the signal,Is the total number of array elements of the pseudolite array antenna,、Respectively are array elements、Array element、The spacing between them.
In practical application, the antenna array pseudolite angle estimation is easily influenced by multipath signals, environmental noise and other factors, so that the angle measurement result is jumped, and the stability of azimuth angle and pitch angle measurement is influenced. In contrast, the IMU has high output frequency, is not interfered by external environment, and can provide pose estimation results with higher precision in a short time.
And the inertial information is utilized to assist in optimizing the angle calculation result of the antenna array pseudolite, so that the accuracy and the robustness of the angle calculation can be effectively improved. The state prediction process between two adjacent frames of inertial measurement is shown in formula (3):
(3);
in the formula,、、Respectively isPredicted values of carrier position, speed and attitude at the moment,、、Respectively isPredicted values of carrier position, speed and attitude at the moment,、The acceleration median value and the angular velocity median value of two frames of inertial measurement are respectively,For the time difference between two frames of inertial measurements,Representing the multiplication of the quaternion.
All IMU data between the k moment visual frame and the j moment pseudolite measurement frame are calculated through a formula (3) in an iterative mode to obtain a carrier position predicted value at the j momentCombining the position and the gesture of the pseudo-satellite array antenna under the navigation coordinate system to calculate the azimuth angle predicted value of the satellite receiver under the pseudo-satellite array antenna coordinate systemPitch angle prediction valueAnd the angle predicted value is obtained through a Kalman filtering method、And the solution in the formula (2)、And fusing to ensure the stability of the angle information.
S3, constructing a fusion positioning system based on factor graph optimization, and solving all state variables in the sliding window.
The method based on filtering only carries out linearization processing once when data are fused, and only estimates state variables at the current moment, so that errors generated in the state estimation process are accumulated in the system, and the method based on optimization can carry out linearization of state quantities for a plurality of times, and estimates all states of the system by combining historical information, thereby having higher precision. Therefore, the invention adopts a factor graph optimization method to fuse the traditional pseudolite position measurement, the antenna array pseudolite angle measurement, the IMU measurement and the vision measurement and solve all state variables in the sliding window.
As shown in fig. 3, a factor graph optimization-based fusion positioning system is constructed, wherein the factor graph optimization-based fusion positioning system comprises a state variable to be solved by the construction system and a factor for constructing a connection state variable, the factor for constructing the connection state variable comprises a pseudolite position factor, an antenna array pseudolite angle factor, an IMU pre-integration factor and a vision factor, and the constructed factor is added into the factor graph optimization fusion positioning system to realize optimal estimation of the system state in a sliding window.
Pseudolite position factor:
The pseudolite position factor is defined as the difference between the carrier position and the pseudolite position, and the pseudolite position factor between the carrier position and the pseudolite position of the kth frame within the sliding window is shown in formula (4):
(4);
in the formula,For the carrier position of the kth frame,For pseudolite position measurements at times corresponding to the kth frame carrier state variables,、Is a rotation matrix and a translation vector between a navigation coordinate system and a traditional pseudo satellite positioning coordinate system.
And when the number of visible pseudolites is sufficient and the DOP value is lower than a certain threshold value, adding the pseudolites position factors into the factor graph optimized fusion positioning system.
Antenna array pseudolite angle factor:
The antenna array pseudolite angle factor is defined as the difference between the carrier azimuth angle and pitch angle predicted value and the antenna array pseudolite azimuth angle and pitch angle measured value, and the antenna array pseudolite angle factor between the kth frame carrier state and the antenna array pseudolite angle measurement in the sliding window is shown as a formula (5):
(5);
in the formula,The specific expression of the position of the kth frame carrier under the pseudo satellite array antenna coordinate system is shown as the formula (6):
(6);
indicating the carrier position of the kth frame,Representing the pose of the pseudo satellite array antenna under a navigation coordinate system;、 And respectively measuring the azimuth angle and the pitch angle of the antenna array pseudolite at the moment corresponding to the state variable of the k-th frame carrier.
And when the number of the visible pseudolites is insufficient or the DOP value is higher than a certain threshold value, adding the antenna array pseudolites angle factors into the factor graph optimized fusion positioning system.
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, and the IMU pre-integration factor between the kth frame and the k+1th frame of images in the sliding window is shown in formula (7):
(7);
in the formula,For a rotation matrix of the carrier pose of the kth frame,、Carrier positions for the kth frame and the k +1 frame respectively,、Carrier speeds for the kth frame and the k +1 frame respectively,Is the time difference between two image frames k and k +1,、Quaternion of the carrier pose for the kth frame and the k +1 frame respectively,Representing extraction quaternionFor representation of a three-dimensional rotation residual;、 AndThe estimated values of position pre-integration, speed pre-integration and rotation pre-integration are respectively.
Visual factor:
In the feature point method VO, the visual factor is defined as the reprojection error. The parameterization modes of the feature points in the visual re-projection error are two, namely, the three-dimensional position parameterization of the feature points under a navigation system and the inverse depth parameterization of the feature points under an image frame.
In the inverse depth parameterization, the feature points are modeled as the inverse of the feature depth for a certain image frame, assuming that at the firstFeature points on frame imagesIs of inverse depth ofThe visual factor is as shown in equation (8):
(8);
in the formula,、Respectively as characteristic pointsIn the first placeThe frames and j-th frame cameras normalize coordinates on the image plane,Is the characteristic pointFrom the firstThe z-axis component in the camera coordinate system after the frame is transformed to the j-th frame,、Respectively the firstThe carrier pose of the frame and the j-th frame,Is an external parameter between the camera and the IMU.
System state variables and objective functions:
the state variables of the fused positioning system based on factor graph optimization comprise all carrier states in a sliding window, IMU zero bias and feature point inverse depth, as shown in a formula (9):
(9);
in the formula,Representing the k frame of variables to be optimized, including the position of the carrier in the navigation coordinate systemSpeed and velocity ofGesture quaternionZero offset of accelerometerZero offset of gyroscope,Is the inverse depth of the feature point.
And S4, optimizing and solving the state variable by minimizing the sum of all residual errors in the sliding window, and solving the objective function by a nonlinear optimization library CeresSolver to obtain the optimal estimated value of the system state variable.
The invention optimizes and solves the state variable in the formula (9) by minimizing the sum of all residual errors in the sliding window, and the definition of the objective function is shown as the formula (10):
(10);
in the formula,For the prior information to be marginalized,For the pseudolite position factor constructed by equation (4),Is the set of all pseudolite position measurements corresponding to the state variable time within the sliding window,Covariance matrix for pseudolite position factor; for the antenna array pseudolite angle factor constructed by equation (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.
For IMU pre-integration factors constructed by equation (7),Is the set of all IMU measurements within the sliding window,Covariance matrix of the IMU pre-integral factor; For the vision factor constructed by formula (8),Is a set of feature points observed by at least two frames of images within the sliding window,Is the covariance matrix of the visual factor.
The pseudolite position factor, antenna array pseudolite angle factor, IMU pre-integration factor and vision factor need to be weighted summed by converting them to mahalanobis distances by the inverse of their respective covariance matrices. And solving the formula (10) through a nonlinear optimization library CeresSolver to obtain the optimal estimated value of the system state variable.
The multi-mode pseudolite multi-source fusion positioning method based on factor graph optimization is adopted, an indoor and outdoor positioning hardware architecture is unified, the limitation of the traditional method in a complex scene is overcome, the accuracy and the robustness of pseudolite angle calculation are improved through designing a uniform circular antenna array and using IMU to assist in optimization, information can be flexibly fused according to the number of the pseudolites and DOP values, and algorithm applicability is enhanced.
It should be noted that the above-mentioned embodiments are merely for illustrating the technical solution of the present invention and not for limiting the same, and although the present invention has been described in detail with reference to the preferred embodiments, it should be understood by those skilled in the art that the technical solution of the present invention may be modified or substituted by the same, and the modified or substituted technical solution may not deviate from the spirit and scope of the technical solution of the present invention.