A mobile Beidou high-precision positioning multi-source data fusion method and system

Abstract

The application discloses a mobile Beidou high-precision positioning multi-source data fusion method and system, and relates to the technical field of high-precision navigation and positioning.The application suppresses the problem of satellite observation value quality degradation in a complex environment by means of tight coupling layer explicit modeling and online estimation of multipath error.In the tight coupling deep fusion and solving process, a first factor graph model is constructed, the multipath error is explicitly modeled as a state parameter to be estimated, and the multipath error is included in Beidou original observation factors.In the joint optimization solving process, the first factor graph model performs online real-time estimation and update on the multipath error as an independent degree of freedom, actively distributes and absorbs the part of the observation residual that can be attributed to the multipath interference into the state parameter, and enables the tight coupling solving module to output a more reliable and stronger anti-interference tight coupling optimal pose sequence and provide a stable absolute position even when the satellite observation condition is seriously polluted by the multipath.

Description

Technical Field

[0001] This invention relates to the field of high-precision navigation and positioning technology, specifically to a multi-source data fusion method and system for mobile BeiDou high-precision positioning. Background Technology

[0002] With the rapid development of technologies such as autonomous driving and mobile robots, the demand for high-precision and high-reliability positioning of mobile platforms in complex urban environments is becoming increasingly urgent. Currently, this mainly relies on navigation satellite systems, inertial measurement units, and technologies such as visual / laser SLAM. However, in scenarios such as urban canyons and under overpasses, satellite signals are easily blocked and interfered with by reflections, leading to a sharp decline in GNSS positioning accuracy or even failure. While pure SLAM technology does not rely on external signals, its positioning results have inherent cumulative errors and lack absolute geographic coordinates, making it difficult to meet the requirements for accurate positioning over long periods and large areas.

[0003] In existing technologies, factor graph optimization methods have been applied to tightly coupled GNSS / INS integrated navigation systems to improve the flexibility and nonlinear solution capabilities of multi-source data fusion. For example:

[0004] Patent CN114545472B discloses a GNSS / INS integrated navigation method based on factor graphs. By constructing zero-velocity detection statistics and introducing zero-velocity correction factor nodes when the vehicle is stationary, it aims to suppress INS error accumulation and reduce the computational burden of factor graphs. While this method improves computational efficiency to some extent in urban scenarios with frequent start-stop operations, its core still uniformly attributes GNSS observation residuals to receiver clock errors and noise, failing to explicitly model and parameterize multipath effects. In environments with severe multipath interference such as satellite signal reflection and obstruction, systematic biases in the observation residuals still directly contaminate position and velocity state variables, leading to a significant decrease in the absolute positioning accuracy of the tightly coupled solution module. Furthermore, this scheme only integrates IMU and raw GNSS observation data, failing to correct for the inherent cumulative drift of the SLAM system, making it difficult to meet the global consistency requirements for long-term, large-area autonomous navigation of mobile platforms.

[0005] Patent CN112946711A proposes a joint weight matrix correction method based on the internal signal quality index of a GNSS receiver. It constructs weight coefficients using pseudo-code lock-on detection values, carrier lock-on detection values, and message verification flags to adaptively adjust the pseudorange measurement covariance matrix, thereby improving the robustness of the factor map in scenarios with degraded signal quality. However, this method is essentially a passive weighting strategy, only reducing the confidence level of inferior satellite observations to mitigate their impact, without fundamentally estimating and absorbing multipath errors as independent state parameters online. When multipath interference causes systematic biases in observations from multiple satellites simultaneously, the weighting strategy struggles to effectively suppress its overall contamination of the positioning solution. Furthermore, this scheme is also limited to a tightly coupled GNSS / INS architecture, lacking a loosely coupled fusion mechanism with visual / laser SLAM pose information, and cannot utilize environmental perception information for absolute coordinate calibration of trajectory drift during long-term operation.

[0006] In summary, existing factor graph-based GNSS / INS tight combination schemes generally lack explicit modeling and real-time estimation capabilities when dealing with multipath effects, and none of them have formed a tightly coupled anti-multipath and loosely coupled drift correction two-layer fusion architecture, making it difficult to achieve both highly reliable absolute positioning and high-precision relative pose consistency in complex urban environments. Summary of the Invention

[0007] The purpose of this invention is to overcome the shortcomings of existing technologies and provide a multi-source data fusion method and system for mobile BeiDou high-precision positioning. This method can suppress the degradation of satellite observation quality in complex environments by explicitly modeling and estimating multipath errors in a tightly coupled layer. During the tightly coupled deep fusion and solution process, the constructed first-factor graphical model explicitly models the multipath error as a state parameter to be estimated and incorporates it into the original BeiDou observation factors. In the joint optimization solution process, the first-factor graphical model estimates and updates the multipath error as an independent degree of freedom online in real time, actively allocating and absorbing the portion of the observation residual attributable to multipath interference into the state parameters. This separates and weakens the impact of multipath effects on positioning solutions from the source, enabling the tightly coupled solution module to output a more reliable and anti-interference-resistant tightly coupled optimal pose sequence even when satellite observation conditions are severely contaminated by multipath interference, providing a stable absolute position for the entire system.

[0008] To solve the above-mentioned technical problems, the present invention provides the following technical solution: On one hand, a multi-source data fusion method for mobile BeiDou high-precision positioning, the specific steps of which are as follows:

[0009] S100: Simultaneously acquire raw observation data from the BeiDou receiver on the mobile platform, raw data from the inertial measurement unit, and raw data from the environmental perception sensor.

[0010] S200. Based on the raw data of the inertial measurement unit and the raw observation data of the Beidou receiver, construct and solve the first factor graph model to obtain the tightly coupled optimal pose sequence;

[0011] S300: Based on the raw data from the environmental perception sensor acquired in S100, feature extraction and tracking are performed, and a SLAM pose map is generated through pose map optimization.

[0012] S400, Loosely Coupled Adaptive Fusion Step: Based on the tightly coupled optimal pose sequence output by S200 and the SLAM pose graph output by S300, construct and solve the second factor graph model to obtain the global optimal pose of the system;

[0013] The system's globally optimal pose obtained by S500 and S400 is used as the final positioning result of the mobile platform.

[0014] Furthermore, the raw observation data of the Beidou receiver includes pseudorange observations and carrier phase observations, the raw data of the inertial measurement unit includes three-axis angular velocity data and three-axis acceleration data, and the environmental perception sensor is a visual camera or a lidar.

[0015] Furthermore, in S200, the first factor graph model is a tightly coupled fusion factor graph model, which uses the error state of the inertial measurement unit as the variable node and the IMU pre-integration factor generated by pre-integrating the inertial measurement unit data and the BeiDou original observation factor generated by modeling the original BeiDou observation values ​​as the constraint edges. This model is used to explicitly model the multipath error parameters in the state node, so as to suppress the influence of multipath effects on positioning in real time during the solution process.

[0016] Furthermore, in step S200, the step of constructing and solving the first factor graph model includes:

[0017] Pre-integration is performed on the raw data of the inertial measurement unit between consecutive time stamps to generate IMU pre-integration factors that describe the relative motion of the carrier between two time points, which serve as time prediction constraints for the first factor graph model.

[0018] After differential processing and error correction of the raw observations of the BeiDou receiver, a BeiDou raw observation factor directly related to the error state of the inertial measurement unit is constructed as the absolute measurement constraint of the first factor graph model.

[0019] The first factor graph model is solved by a nonlinear optimization algorithm to minimize the sum of squared residuals of all IMU pre-integration factors and BeiDou original observation factors, while optimizing the parameters in all state nodes to obtain the tightly coupled optimal pose sequence and multipath error parameters.

[0020] Furthermore, the original BeiDou observation factors are used to model the multipath effect as a time-varying parameter through observation equations for explicit estimation, thereby suppressing the impact of the multipath effect on positioning, including:

[0021] For in The first time received pseudorange observations of a Beidou satellite Construct a system containing multipath error parameters The observation equation: ,in, express Time for the first The actual pseudorange measurement values ​​of the satellites, Indicates based on Receiver position estimation in time-state node Satellite positions calculated from known satellite ephemeris Geometric distance between them At the speed of light, express Receiver clock bias in the time-state node For satellite clock bias derived from satellite ephemeris, and These are the ionospheric delay and the tropospheric delay, respectively. That is, the part to be optimized, specifically for the first... A satellite in Multipath error parameters at time points This is noise in pseudorange measurements;

[0022] Multipath error parameters As an expanded state, add In the state node vector at time t, based on the observation equation, the original BeiDou observation factors are created, and the j-th satellite is located in... pseudorange observations at time With inclusion of The time-state nodes are connected;

[0023] In the process of solving the first factor graph model using a nonlinear optimization algorithm, the residuals generated by the original BeiDou observation factors are minimized, where the residuals are the observed values. The difference between the calculated value and the state-based prediction is due to the explicit inclusion of multipath error parameters in the observation equation. As a degree of freedom, the nonlinear optimization algorithm distributes and absorbs the portion of the error caused by multipath effects in the observation residuals. In the estimation update, instead of passing all of it to navigation state parameters such as position and attitude, it can achieve real-time estimation and suppression of multipath effects.

[0024] Furthermore, in step S300, the step of generating the SLAM pose map includes:

[0025] The raw data frames acquired by the environmental perception sensor in S100 are processed to extract feature points and feature surfaces in the environment. Through feature matching and data association, the movement of feature points and feature surfaces between consecutive frames is tracked.

[0026] Based on the feature tracking results, the relative pose transformation of the mobile platform between adjacent data frames is calculated to form edge constraints between nodes in the pose graph, and the pose of the key frame is used as the node of the SLAM pose graph.

[0027] The SLAM pose graph, which is composed of pose nodes of keyframes and relative pose constraint edges, is optimized in the back end. By minimizing the error of all relative pose constraint edges, a globally consistent set of keyframe pose estimations in the coordinate system of the environmental perception sensor is obtained, which is the SLAM pose graph.

[0028] Furthermore, in S400, the second factor graph model is a loosely coupled adaptive fusion factor graph model, which uses the tightly coupled optimal pose sequence as a priori constraint factor to perform absolute coordinate calibration and drift correction on the SLAM pose graph. The construction process of the second factor graph model is as follows:

[0029] Align the keyframe pose nodes in the SLAM pose graph output by S300 with the tightly coupled optimal pose sequence output by S200 in time.

[0030] A tightly coupled pose prior factor is added to each aligned keyframe pose node. The tightly coupled pose prior factor uses the aligned tightly coupled optimal pose as the observation value and retains the relative pose constraints between the original keyframe nodes in the SLAM pose graph as the SLAM pose factor.

[0031] Construct a second factor graph model with the keyframe pose nodes as optimization variables. The second factor graph model includes the tightly coupled pose prior factor and the SLAM pose factor.

[0032] By solving the second factor graph model, the optimized keyframe pose simultaneously satisfies the absolute position constraints from the tightly coupled optimal pose sequence and the geometric consistency constraints from the SLAM pose graph, and the optimized keyframe pose is output as the global optimal pose of the system.

[0033] The steps for drift correction of the SLAM pose map are as follows:

[0034] For the second factor graph model, the first For each keyframe pose node, define the error of its tightly coupled pose prior factor: ,in, Let be the pose transformation matrix to be optimized for this node. These are time-aligned pose transformation matrix observations from tightly coupled optimal pose sequences. The operation maps the differences in the pose matrix to the Lie algebra space, and for the connection of the first Lie algebra in the SLAM pose graph... The relative pose constraints between the node and the j-th keyframe node are used to define the error of its SLAM pose factor: ,in The relative pose transformation is obtained from the SLAM front-end measurement. and These are the poses to be optimized for two related nodes;

[0035] The overall objective function F of the second factor graphical model is defined as the weighted sum of squares of the errors of all factors: ,in, It is the covariance matrix of the tightly coupled pose prior factors, used to reflect the confidence level of the tightly coupled optimal pose. It is the covariance matrix of the SLAM pose factors, used to reflect the confidence level of the SLAM relative pose measurement;

[0036] All keyframe pose nodes were adjusted using a nonlinear optimization algorithm. To minimize the overall objective function F, in this process, high confidence (i.e., small covariance) is required. Tightly coupled pose prior factors By imposing stronger constraints on the optimization process, the keyframe poses of the SLAM pose graph are pulled toward a tightly coupled pose trajectory with absolute geographic coordinates, thereby achieving continuous correction of SLAM cumulative drift.

[0037] Furthermore, S400 also includes introducing external elevation constraints, accessing a pre-stored elevation digital map, and in the second factor graph model, attaching elevation map constraint factors to the keyframe pose nodes. These elevation map constraint factors optimize the planar projected coordinates obtained from the keyframe pose nodes. Elevation values ​​in the corresponding digital elevation map The height value calculated from the pose of the keyframe pose nodes. Comparisons are made to form an altitude observation error term. and the height observation error term This is incorporated into the overall optimization objective of the second factor graph model to further constrain and correct the estimation accuracy of the height channel.

[0038] On the other hand, a mobile BeiDou high-precision positioning multi-source data fusion system includes: a data acquisition and synchronization module, a tightly coupled solution module, a SLAM processing module, a loosely coupled fusion module, and a map management module;

[0039] The data acquisition and synchronization module is connected to the BeiDou receiver, the inertial measurement unit, and the environmental perception sensor. It is used to receive and time-synchronize the raw observation data from the BeiDou receiver, the raw data from the inertial measurement unit, and the raw data from the environmental perception sensor.

[0040] The tightly coupled solution module is used to construct and optimize the embedded first factor graph model based on the original data of the synchronized inertial measurement module and the original observation data of the Beidou receiver, and output the tightly coupled optimal pose sequence.

[0041] The SLAM processing module is used to perform feature tracking and pose map optimization based on the raw data from the synchronized environmental perception sensor, and to generate and output a SLAM pose map.

[0042] The loosely coupled fusion module is used to receive the tightly coupled optimal pose sequence and SLAM pose graph, construct and optimize the embedded second factor graph model, perform fusion calculation and output the global optimal pose of the system.

[0043] The map management module is used to store and provide elevation digital maps. The loosely coupled fusion module queries the elevation information of the elevation digital map from the map management module and incorporates the elevation information as a constraint into the optimization process of the second factor graph model.

[0044] Compared with existing technologies, this multi-source data fusion method for mobile BeiDou high-precision positioning has the following advantages:

[0045] I. This invention suppresses the degradation of satellite observation quality under complex environments by explicitly modeling and estimating multipath errors in a tightly coupled layer. In the process of tightly coupled deep fusion and solution, the constructed first factor graph model explicitly models the multipath error as a state parameter to be estimated and incorporates it into the original BeiDou observation factors. In the joint optimization solution process, the first factor graph model estimates and updates the multipath error as an independent degree of freedom online in real time, actively allocating and absorbing the part of the observation residual that can be attributed to multipath interference into the state parameters. This separates and weakens the impact of multipath effects on positioning solution from the source, so that even when satellite observation conditions are severely polluted by multipath, the tightly coupled solution module can still output a more reliable and more interference-resistant tightly coupled optimal pose sequence, providing a stable absolute position for the entire system.

[0046] Second, this invention introduces tightly coupled results as prior constraints through a loosely coupled layer, achieving continuous calibration of SLAM cumulative drift and reliable introduction of absolute geographic coordinates. The tightly coupled optimal pose sequence output from the bottom layer is used as a tightly coupled pose prior factor and introduced into the second factor graphical model. The error term of this factor directly expresses the difference between the SLAM keyframe pose and the tightly coupled pose with absolute geographic coordinates. When optimizing the overall objective function, the high-confidence tightly coupled pose prior factor exerts strong constraints on the optimization process, injecting absolute geographic reference into the entire SLAM. It corrects the drift in real time and effectively in each fusion optimization, thereby solving the drift problem that cannot be overcome in pure SLAM and ensuring the long-term global consistency of the system's globally optimal pose.

[0047] Other advantages, objectives and features of the invention will be set forth in part in the description which follows, and in part will be apparent to those skilled in the art from the following examination or study, or may be learned from the practice of the invention. Attached Figure Description

[0048] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the accompanying drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are merely some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without any creative effort.

[0049] Figure 1 A flowchart illustrating the steps of a multi-source data fusion method for mobile BeiDou high-precision positioning;

[0050] Figure 2 This is a flowchart illustrating the steps involved in constructing and solving the first factor graph model in an embodiment of the present invention.

[0051] Figure 3 This is a diagram showing the modular composition of a mobile BeiDou high-precision positioning multi-source data fusion system. Detailed Implementation

[0052] To address the shortcomings of existing high-precision positioning technologies, such as the insufficient suppression of multipath effects in the tightly coupled GNSS / INS method and the inability to correct SLAM cumulative drift due to GNSS gross errors in the loosely coupled GNSS / SLAM combination, this invention provides a multi-source data fusion method for mobile BeiDou high-precision positioning. This method utilizes a two-layer factor graph optimization architecture—tight coupling to suppress multipath effects and loose coupling to correct SLAM drift—to deeply fuse raw BeiDou observations, inertial measurement unit data, and environmental perception information. This enables continuous, stable, and high-precision absolute and relative positioning of mobile platforms in complex urban environments. This invention is applicable to various mobile platforms, including autonomous vehicles, mobile robots, and drones, in scenarios with significant multipath effects and unstable satellite signals, such as high-rise buildings, canyons, under overpasses, and tunnel entrances / exits.

[0053] Specifically, such as Figure 1 As shown, the specific steps of a multi-source data fusion method for mobile BeiDou high-precision positioning are as follows:

[0054] S100, Multi-source data synchronous acquisition: Synchronously acquire raw observation data from the Beidou receiver on the mobile platform, raw data from the inertial measurement unit, and raw data from the environmental perception sensor;

[0055] S200, Tightly Coupled Fusion Solution: Based on the raw data of the inertial measurement unit and the raw observation data of the Beidou receiver, the first factor graph model is constructed and solved to obtain the tightly coupled optimal pose sequence;

[0056] S300 and SLAM pose graph construction: Based on the raw data of the environmental perception sensor obtained by S100, feature extraction and tracking are performed, and a SLAM pose graph is generated through pose graph optimization.

[0057] S400, Loosely Coupled Adaptive Fusion: Based on the tightly coupled optimal pose sequence output by S200 and the SLAM pose graph output by S300, a second factor graph model is constructed and solved to obtain the global optimal pose of the system;

[0058] S500, Positioning Result Output: Output the system's globally optimal pose obtained in S400 as the final positioning result of the mobile platform.

[0059] In practical implementation, the mobile platform integrates a BeiDou high-precision receiver, an inertial measurement unit (IMU), and a visual camera and / or lidar. Data acquisition and synchronization modules are time-stamp aligned to ensure strict time synchronization of data streams from different sensors, guaranteeing the effectiveness of subsequent fusion. The system receives raw observation data streams from the BeiDou receiver in real time, including pseudorange observations at multiple frequencies such as L1 / L2, carrier phase observations, and navigation messages. Simultaneously, it receives three-axis angular velocity (gyroscope) and three-axis acceleration (accelerometer) data streams from the IMU, and receives data streams from environmental perception sensors (camera image frames, lidar point cloud frames) in parallel.

[0060] Each data packet is stamped with a precise hardware timestamp or a synchronization timestamp based on a high-precision clock. A time-aligned data buffer is established, and BeiDou observation data, IMU data, and nearest neighbor image / point cloud frames within the same time window are associated and packaged to form a multi-source data packet with a time stamp.

[0061] Step S200 involves receiving synchronized IMU raw data and BeiDou raw observation data, constructing a first factor graph model to deeply fuse these two types of data, and explicitly estimating and suppressing multipath errors, such as... Figure 2 As shown, the steps for constructing and solving the first factor graph model include:

[0062] Pre-integration is performed on the raw data of the inertial measurement unit between consecutive time stamps to generate IMU pre-integration factors that describe the relative motion of the carrier between two time points, which serve as time prediction constraints for the first factor graph model.

[0063] After differential processing and error correction of the raw observations of the BeiDou receiver, a BeiDou raw observation factor directly related to the error state of the inertial measurement unit is constructed as the absolute measurement constraint of the first factor graph model.

[0064] The first factor graph model is solved by nonlinear optimization algorithm, minimizing the sum of squared residuals of all IMU pre-integration factors and BeiDou original observation factors, while optimizing the parameters in all state nodes to obtain the tightly coupled optimal pose sequence and multipath error parameters.

[0065] In this embodiment, the steps for solving the first factor graph model are as follows:

[0066] For consecutive timestamps and The high-frequency IMU raw angular velocity and acceleration data are pre-integrated. In this embodiment, the pre-integration calculation is performed in a local coordinate system, calculating the relative rotation increment, velocity increment, and position increment of the carrier between two moments, generating an independent IMU pre-integration factor. This IMU pre-integration factor serves as the time prediction constraint edge in the first factor graph model, connecting... and The two state nodes at time t are based on the IMU kinematic equations, and the residuals reflect the cumulative error of the IMU measurements.

[0067] Define the state nodes in the first factor graph model ,Include Location of the carrier ,attitude ,speed IMU zero bias (including accelerometer bias and gyroscope bias), receiver clock bias And for each one in BeiDou satellites that are always visible Introduce an independent multipath error parameter The state node vector is expanded to , This indicates the transpose operation.

[0068] for Received at time pseudorange observations of a Beidou satellite Construct observation equations that explicitly include multipath parameters: ,in, express The actual pseudorange measurement value for the j-th satellite at time j. This represents the receiver position estimation in the state node at time k. Satellite positions calculated from known satellite ephemeris The geometric distance between them, where c is the speed of light. This represents the receiver clock bias in the state node at time k. For satellite clock bias derived from satellite ephemeris, and These are the ionospheric delay and the tropospheric delay, respectively. This refers to the multipath error parameters to be optimized for the j-th satellite at time k. To mitigate pseudorange measurement noise, based on this observation equation, a BeiDou raw observation factor is created, and the observed values ​​are... With state nodes Connecting them, the residual of the original BeiDou observation factor is the sum of the observed value and the state-based predicted value (including...). The difference between the calculated values ​​of ).

[0069] The first factor graph model uses time-series state nodes. For each variable node, the constraint edges include: IMU pre-integration factors connecting adjacent nodes, and multiple raw BeiDou observation factors connecting a single node to the corresponding BeiDou observation value. A sliding window optimization strategy is employed to manage a fixed number of latest-state nodes. A nonlinear optimization algorithm is used to solve this first-factor graph model, minimizing the weighted sum of squares of all factor residuals. ,in, To connect state nodes and The residuals of the IMU pre-integration factor To connect state nodes and pseudorange observations The residuals of the original BeiDou observation factors and These are the covariance matrices of the IMU pre-integration factor and the BeiDou raw observation factor, respectively, used to characterize the uncertainty (reciprocal of the confidence level) of their respective measurements, and used as weights for the residuals in optimization. and This represents all time-series indices within the sliding window. The optimization involves iterating and summing over all states within the entire time window. Let represent the set of all visible satellites at time k. During the optimization process, all state parameters are automatically adjusted, including... The systematic bias in the observation residuals caused by multipath effects will be allocated and absorbed into the corresponding [databases / residues]. The estimated values ​​are not fully transmitted to core navigation states such as position and attitude, thereby suppressing the multipath effect.

[0070] After optimization, in all state nodes within the sliding window and The optimization results are output as a tightly coupled optimal pose sequence with absolute geographic coordinates and minimal impact from multipath, which has reliability and accuracy in complex urban environments.

[0071] Step S300 is executed in parallel by the SLAM processing module, receiving synchronized camera images or LiDAR point cloud data. For visual SLAM, feature points, lines, or surfaces are extracted from each frame. For LiDAR SLAM, geometric features such as planes and corners are extracted from the point cloud, and the correspondence between features in consecutive frames is tracked through descriptor matching or geometric association. Based on the feature matching relationship, epipolar geometry is used to estimate the relative pose transformation of the mobile platform between adjacent frames. Meanwhile, key frames are selected based on criteria such as motion amplitude and number of features tracked to reduce computational load and accumulated error.

[0072] pose of each keyframe (Relative to the SLAM starting coordinate system) is used as a node in the pose graph, representing the relative pose transformation between adjacent keyframes calculated by the front end. As a connection between two nodes and The edge constraints (SLAM pose factors) are used to construct a pose graph containing only relative constraints. Then, through backend optimization, the error of all relative pose constraint edges is minimized. ,in, Keyframes to be optimized and The position, The relative pose transformation from frame i to frame j is obtained from the SLAM front-end measurement. To map the differences in the pose matrix to its Lie algebra space, facilitating error calculation and differentiation in Euclidean space, This is the covariance matrix of SLAM relative pose measurement, reflecting the confidence level of the front-end measurement. Let be the set of all edges in the pose graph. To sum all connected edges in the pose graph, after optimization, a globally consistent SLAM pose graph is obtained in the coordinate system of the environment perception sensor itself. This graph has high local geometric accuracy, but suffers from cumulative drift over time and lacks absolute geographic coordinates.

[0073] The S400 receives the tightly coupled optimal pose sequence from the S200 and the SLAM pose map from the S300, and performs adaptive fusion by constructing a second factor graph model. It utilizes the absolute pose anchor points provided by the tight coupling to calibrate and correct the accumulated drift of the SLAM. Using a unified timestamp, each keyframe pose node in the SLAM pose map is... The pose that is closest in time to the tightly coupled optimal pose sequence Perform alignment association.

[0074] The second factor graph model uses the pose nodes of SLAM keyframes. For the variable node to be optimized, the constraint edges include:

[0075] Tightly Coupled Pose Prior Factor: Add a unary factor to each keyframe node i, whose observation value is the aligned tightly coupled pose. The error of this factor is defined as: , directly Pull towards locations with absolute geographic coordinates ;

[0076] SLAM pose factor: The relative pose constraint edges inside the SLAM pose graph constructed in S300 are retained and used as binary factors connecting nodes i and j, and their error is defined as: It is used to maintain high-precision geometric consistency within SLAM.

[0077] Introducing elevation constraints: To improve the accuracy of the elevation channel, pre-stored high-precision DEM data is queried from the map management module. For each keyframe node i, the elevation constraint is determined based on its planar position during optimization. Obtain the corresponding elevation values ​​from the DEM. The elevation map constraint factors are constructed, and their error is: ,in It is the node pose The height value in the graph is added as the third constraint edge to the second factor graph model.

[0078] The overall objective function of the second factor graph model is: ,in, , , These are the covariance matrices for each factor, reflecting the confidence level of the corresponding observation. The covariance is set online or offline based on factors such as sensor noise characteristics, posterior variance of tightly coupled solutions, and DEM accuracy. During the optimization process, the weights of different constraints are adaptively balanced according to the magnitude of the covariance. When the quality of tightly coupled positioning is high ( When the prior factor constraint is small, it effectively corrects SLAM drift; when the tight coupling is temporarily disabled due to occlusion... When the elevation increases, the SLAM pose factor and elevation constraint play a dominant role. Positioning continuity is maintained by SLAM and elevation maps, and is minimized through nonlinear optimization. This yields a set of globally optimal poses for the system that simultaneously satisfy absolute coordinate constraints and local geometric consistency. The optimized keyframe pose sequence is the final output. This keyframe pose sequence has absolute geographic coordinates and high local accuracy, and can effectively control long-term drift.

[0079] The S500 outputs the system's globally optimal pose sequence obtained in the S400 to the upper-layer application of the mobile platform in real time in a standardized message format, serving as a reliable positioning result for the platform.

[0080] In practical implementation, this method is applicable to a mobile BeiDou high-precision positioning multi-source data fusion system. The various modules of this system work collaboratively to achieve the above-described method flow, such as... Figure 3 As shown, the system includes:

[0081] Data acquisition and synchronization module: Implemented by hardware synchronization triggers or software synchronization middleware, responsible for aggregating and synchronizing data streams from Beidou receiver, IMU, and camera / LiDAR;

[0082] Tightly Coupled Solving Module: Built-in first factor graph model builder and optimizer, receives synchronous data packets in real time, performs IMU pre-integration, multi-path modeling, nonlinear optimization, and outputs the tightly coupled optimal pose sequence and its confidence.

[0083] SLAM processing module: Runs visual or laser SLAM algorithms, performs front-end feature processing, tracking and back-end pose graph optimization, and outputs SLAM pose graph;

[0084] Map management module: Stores and provides pre-loaded local or global high-precision digital elevation model data, and responds to elevation query requests from the loosely coupled fusion module;

[0085] Loosely Coupled Fusion Module: It has a built-in second-factor graph model builder and optimizer. It receives pose information from the tightly coupled solution module and the SLAM processing module, performs time alignment, and obtains elevation constraints from the map management module as needed. By solving the second-factor graph model, it adaptively fuses multi-source information and finally outputs the global optimal pose of the system.

[0086] Each module can be deployed on the vehicle-mounted high-performance computing unit of the mobile platform and run in parallel in a multi-threaded or inter-process communication manner to ensure the real-time performance of the system.

[0087] To further verify the technical effectiveness and engineering applicability of the method of this invention, systematic real-vehicle testing was conducted in several typical and complex scenarios in a central city. The test platform was equipped with a Beidou high-precision positioning terminal, a MEMS inertial measurement unit, a forward-looking binocular industrial camera, and a 32-line mechanical lidar. The Beidou high-precision positioning terminal supports BDS-3 full-frequency signals and can output raw pseudorange and carrier phase observations of B1I / B2I / B3I, B1C / B2a, with a sampling frequency of 20Hz. The MEMS inertial measurement unit has a three-axis gyroscope with a range of ±450° / s, a zero-bias stability of 0.8° / h, an accelerometer with a range of ±8g, a zero-bias stability of 0.03mg, and a sampling frequency of 200Hz. The forward-looking binocular industrial camera has a resolution of 1280×1024 and a frame rate of 20Hz. The 32-line mechanical lidar has a horizontal field of view of 360°, a vertical field of view of 40°, and a scanning frequency of 10Hz. All sensors in the computing platform achieve microsecond-level hard-triggered synchronization through a hardware synchronization pulse generator, and the data acquisition and processing module runs in a multi-threaded parallel manner within the embedded real-time operating system.

[0088] The test route traversed four typical multipath environments: urban canyons, under viaducts, tree-lined roads, and tunnel entrances / exits, accumulating a test mileage of over 120km. It covered positioning conditions under different time periods and satellite geometries. In the urban canyons, buildings on both sides were over 80m high, and the road was approximately 20m wide, resulting in frequent satellite signal reflection and diffraction. Under the viaducts, the concrete structure had a 25m wide bridge deck and an 8m clearance height, obstructing the view of only 4-6 visible satellites, leading to a mixture of multipath and non-line-of-sight signals. The tree-lined roads were two-lane roads with dense tree canopy coverage, causing signal attenuation of 3-8dB. The tunnels at the entrances / exits were approximately 800m long, with signal loss and rapid reacquisition occurring immediately upon entry / exit. The trajectory output by the high-precision integrated navigation system was used as the true reference.

[0089] To highlight the performance advantages of the method of the present invention, two sets of control schemes are set up:

[0090] Option A (traditional GNSS / INS tightly coupled): The standard extended Kalman filter framework is adopted to deeply fuse the original pseudorange / carrier phase observations of BeiDou with the IMU pre-integration results. The multipath effect is only handled in the observation model by increasing the observation noise covariance, without explicit parameterization modeling.

[0091] Scheme B (traditional GNSS / SLAM loose coupling): First, the single-point positioning result is output by GNSS / INS tight combination (same as Scheme A), and then loosely coupled fusion based on the pose map optimization is performed with the visual / laser SLAM pose map. When GNSS observations are abnormal, gross errors are only removed by setting a large robustness threshold. No tight coupling prior factor and adaptive confidence adjustment mechanism are introduced.

[0092] Scheme C (method of the present invention): Following the process described in S100~S500, the first factor graph model estimates the multipath error of each visible satellite as a time-varying state variable online, and the second factor graph model uses the tightly coupled optimal pose as a priori factor and adds digital elevation model constraints to achieve adaptive calibration of SLAM drift.

[0093] Positioning accuracy comparison: Using the root mean square error of three-dimensional position as the evaluation index, the continuous one-hour operation data of each scheme in various scenarios were statistically analyzed. As shown in Table 1, the method of this invention achieved the best positioning accuracy in all test scenarios:

[0094] Table 1

[0095] Scene Option A Option B Option C City Canyon 1.87 2.34 0.38 Under the overpass 1.52 1.96 0.31 tree-lined road 0.96 1.13 0.27 Tunnel entrance and exit 2.15 2.61 0.45

[0096] The above data shows that the method of the present invention improves the positioning accuracy by about 80% compared with the traditional tightly coupled scheme and by more than 83% compared with the traditional loosely coupled scheme in high multipath environments such as urban canyons and under viaducts. It also shows an improvement of more than 70% in scenarios with slight signal attenuation, such as tree-lined roads. In particular, at the moment of signal recapture at the tunnel entrance and exit, scheme C can converge to an accuracy of 0.45 meters within 3 seconds after exiting the tunnel, while schemes A and B require 12 seconds and 15 seconds respectively and are accompanied by a position jump of more than 2 meters. This proves that the method of the present invention has stronger transient adaptability and convergence stability.

[0097] Quantitative Analysis of Multipath Suppression Effect: To isolate the multipath effect suppression capability of this invention, the explicitly estimated multipath error parameter sequence in the first factor graph model was extracted and compared with the traditional residual analysis method based on signal-to-noise ratio weighting and elevation angle modeling. In urban canyon scenarios, the time series of multipath error parameters estimated by this invention for each satellite exhibits stationary random characteristics, and its standard deviation is reduced by 62.4% (from 0.51m to 0.19m) compared with the unmodeled residuals attributed to observation noise in the traditional method. Further analysis of the pseudorange residual distribution before and after optimization of the original BeiDou observation factors: After optimization, the mean of the pseudorange residuals in scheme C approaches zero (-0.03m), and the standard deviation is 0.21m. The low-frequency systematic deviations in the residuals are completely absorbed into the multipath state parameters; while the mean of the pseudorange residuals in scheme A without explicit modeling is 0.47m, and the standard deviation is 0.68m, and the residual sequence shows a significant correlation with the satellite elevation angle. This invention verifies that by incorporating multipath error as an independent degree of freedom into factor graph optimization, it fundamentally achieves the separation and suppression of multipath interference, thus avoiding the transmission of observation errors to navigation state parameters.

[0098] Computational complexity and real-time performance: Benchmark tests were conducted on the time consumption of each module of the method of this invention. During the tests, the CPU load was controlled within the range of 60% to 70%, and the peak memory usage was stabilized within 1.5GB after optimization by the object pool. The average time for a single nonlinear optimization of the tightly coupled solution module (first factor map, sliding window size 15) was 23.7ms. The average time for the SLAM front-end feature extraction and tracking vision thread was 18.5ms (20Hz image input), and the average time for the laser thread was 15.2ms (10Hz point cloud input). The average time for the SLAM back-end pose map optimization was 9.3ms (keyframe size 50), and the average time for a single optimization of the loosely coupled fusion module (second factor map, keyframe size 50) was 8.1ms. The end-to-end latency of the entire system (from the entry of raw sensor data into the buffer to the output of the system's global optimal pose) was stabilized within 65ms after pipeline optimization, and the overall processing frequency reached 15Hz, which fully meets the real-time positioning requirements of autonomous driving and mobile robots in complex urban environments.

[0099] This embodiment, through multi-scenario, long-term real-vehicle testing, systematically verifies the significant advantages of the method of this invention under extreme conditions such as strong multipath and partial degradation of satellite signals: the positioning accuracy is improved by 70%~85% compared with existing mainstream solutions, multipath error is effectively modeled and suppressed to less than 40% of traditional methods, and the system's real-time performance and resource overhead both demonstrate clear engineering feasibility. The two-layer factor graph fusion architecture proposed in this invention, through source suppression of multipath in a tightly coupled layer and adaptive calibration of SLAM drift in a loosely coupled layer, provides a highly reliable and high-precision positioning solution for challenging satellite positioning scenarios such as urban canyons and under viaducts.

[0100] In summary, this invention improves the reliability of BeiDou positioning from the source by explicitly modeling multipath errors through a tightly coupled layer, and then uses an loosely coupled layer with adaptive weights to use absolute pose as a priori constraint to calibrate the SLAM pose map, forming a progressive fusion positioning scheme. This method effectively solves the problems of multipath effects and SLAM cumulative drift in complex urban environments, providing continuous, stable, and high-precision positioning capabilities for mobile platforms such as autonomous driving, and significantly improving navigation robustness and safety in extreme scenarios.

[0101] The above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention in any way. Although the present invention has been disclosed above with reference to preferred embodiments, it is not intended to limit the present invention. Any person skilled in the art can make some modifications or alterations to the above-disclosed technical content to create equivalent embodiments without departing from the scope of the present invention. Any simple modifications, equivalent changes and alterations made to the above embodiments based on the technical essence of the present invention without departing from the scope of the present invention shall still fall within the scope of the present invention.

Claims

1. A multi-source data fusion method for mobile BeiDou high-precision positioning, characterized in that, The specific steps of this method are as follows: S100: Simultaneously acquire raw observation data from the BeiDou receiver on the mobile platform, raw data from the inertial measurement unit, and raw data from the environmental perception sensor. S200. Based on the raw data of the inertial measurement unit and the raw observation data of the Beidou receiver, construct and solve the first factor graph model to obtain the tightly coupled optimal pose sequence; The first factor graph model is a tightly coupled fusion factor graph model. It uses the error state of the inertial measurement unit as the variable node and the IMU pre-integration factor generated by pre-integrating the inertial measurement unit data and the Beidou original observation factor generated by modeling the original Beidou observation values ​​as the constraint edges. It is used to explicitly model the multipath error parameters in the state node so as to suppress the impact of multipath effects on positioning in real time during the solution process. S300: Based on the raw data from the environmental perception sensor acquired in S100, feature extraction and tracking are performed, and a SLAM pose map is generated through pose map optimization. S400, Loosely Coupled Adaptive Fusion Step: Based on the tightly coupled optimal pose sequence output by S200 and the SLAM pose graph output by S300, construct and solve the second factor graph model to obtain the global optimal pose of the system; The second factor graph model is a loosely coupled adaptive fusion factor graph model, which uses the tightly coupled optimal pose sequence as a priori constraint factor to perform absolute coordinate calibration and drift correction on the SLAM pose graph. The system's globally optimal pose obtained by S500 and S400 is used as the final positioning result of the mobile platform.

2. The multi-source data fusion method for mobile BeiDou high-precision positioning according to claim 1, characterized in that, The raw observation data of the Beidou receiver includes pseudorange observations and carrier phase observations; the raw data of the inertial measurement unit includes three-axis angular velocity data and three-axis acceleration data; and the environmental perception sensor is a visual camera or a lidar.

3. The multi-source data fusion method for mobile BeiDou high-precision positioning according to claim 1, characterized in that, In step S200, the step of constructing and solving the first factor graph model includes: Pre-integration is performed on the raw data of the inertial measurement unit between consecutive time stamps to generate IMU pre-integration factors that describe the relative motion of the carrier between two time points, which serve as time prediction constraints for the first factor graph model. After differential processing and error correction of the raw observations of the BeiDou receiver, a BeiDou raw observation factor directly related to the error state of the inertial measurement unit is constructed as the absolute measurement constraint of the first factor graph model. The first factor graph model is solved by a nonlinear optimization algorithm to minimize the sum of squared residuals of all IMU pre-integration factors and BeiDou original observation factors, while optimizing the parameters in all state nodes to obtain the tightly coupled optimal pose sequence and multipath error parameters.

4. The multi-source data fusion method for mobile BeiDou high-precision positioning according to claim 3, characterized in that, The original BeiDou observation factors are used to model the multipath effect as a time-varying parameter through observation equations for explicit estimation, thereby suppressing the impact of the multipath effect on positioning, including: For in The first time received pseudorange observations of a Beidou satellite Construct a system containing multipath error parameters The observation equation: ,in, express Time for the first The actual pseudorange measurement values ​​of the satellites, Indicates based on Receiver position estimation in time-state node Satellite positions calculated from known satellite ephemeris Geometric distance between them At the speed of light, express Receiver clock bias in the time-state node For satellite clock bias derived from satellite ephemeris, and These are the ionospheric delay and the tropospheric delay, respectively. That is, the part to be optimized, specifically for the first... A satellite in Multipath error parameters at time points This is noise in pseudorange measurements; Multipath error parameters As an expanded state, add In the state node vector at time t, based on the observation equation, the original BeiDou observation factors are created, and the j-th satellite is located in... pseudorange observations at time With inclusion of The time-state nodes are connected; In the process of solving the first factor graph model using a nonlinear optimization algorithm, the residuals generated by the original BeiDou observation factors are minimized, where the residuals are the observed values. The nonlinear optimization algorithm, based on the difference between the calculated values ​​and the state-based predictions, distributes and absorbs the errors caused by multipath effects in the observed residuals. In the estimation update, real-time estimation and suppression of multipath effects are achieved.

5. The multi-source data fusion method for mobile BeiDou high-precision positioning according to claim 1, characterized in that, In step S300, the step of generating the SLAM pose map includes: The raw data frames acquired by the environmental perception sensor in S100 are processed to extract feature points and feature surfaces in the environment. Through feature matching and data association, the movement of feature points and feature surfaces between consecutive frames is tracked. Based on the feature tracking results, the relative pose transformation of the mobile platform between adjacent data frames is calculated to form edge constraints between nodes in the pose graph, and the pose of the key frame is used as the node of the SLAM pose graph. The SLAM pose graph, which is composed of pose nodes of keyframes and relative pose constraint edges, is optimized in the back end. By minimizing the error of all relative pose constraint edges, a globally consistent set of keyframe pose estimations in the coordinate system of the environmental perception sensor is obtained, which is the SLAM pose graph.

6. The multi-source data fusion method for mobile BeiDou high-precision positioning according to claim 1, characterized in that, In S400, the construction process of the second factor graph model is as follows: The keyframe pose nodes in the SLAM pose graph output by S300 are aligned in time with the tightly coupled optimal pose sequence output by S200. A tightly coupled pose prior factor is added to each aligned keyframe pose node. The tightly coupled pose prior factor uses the aligned tightly coupled optimal pose as the observation value and retains the relative pose constraints between the original keyframe nodes in the SLAM pose graph as the SLAM pose factor. Construct a second factor graph model with the keyframe pose nodes as optimization variables. The second factor graph model includes the tightly coupled pose prior factor and the SLAM pose factor. By solving the second factor graph model, the optimized keyframe pose simultaneously satisfies the absolute position constraints from the tightly coupled optimal pose sequence and the geometric consistency constraints from the SLAM pose graph, and the optimized keyframe pose is output as the global optimal pose of the system.

7. The multi-source data fusion method for mobile BeiDou high-precision positioning according to claim 6, characterized in that, The S400 process further includes introducing external elevation constraints, accessing a pre-stored digital elevation map, and in the second factor graph model, attaching elevation map constraint factors to the keyframe pose nodes. These elevation map constraint factors optimize the planar projection coordinates obtained from the keyframe pose nodes. Elevation values ​​in the corresponding digital elevation map The height value calculated from the pose of the keyframe pose nodes. Comparisons are made to form an altitude observation error term. and the height observation error term This is incorporated into the overall optimization objective of the second factor graph model.

8. A multi-source data fusion system for mobile BeiDou high-precision positioning, applicable to the multi-source data fusion method for mobile BeiDou high-precision positioning as described in any one of claims 1-7, characterized in that, The system includes: a data acquisition and synchronization module, a tightly coupled solution module, a SLAM processing module, a loosely coupled fusion module, and a map management module; The data acquisition and synchronization module is connected to the BeiDou receiver, the inertial measurement unit, and the environmental perception sensor. It is used to receive and time-synchronize the raw observation data from the BeiDou receiver, the raw data from the inertial measurement unit, and the raw data from the environmental perception sensor. The tightly coupled solution module is used to construct and optimize the embedded first factor graph model based on the original data of the synchronized inertial measurement unit and the original observation data of the Beidou receiver, and output the tightly coupled optimal pose sequence. The SLAM processing module is used to perform feature tracking and pose map optimization based on the raw data from the synchronized environmental perception sensor, and to generate and output a SLAM pose map. The loosely coupled fusion module is used to receive the tightly coupled optimal pose sequence and SLAM pose graph, construct and optimize the embedded second factor graph model, perform fusion calculation and output the global optimal pose of the system. The map management module is used to store and provide elevation digital maps. The loosely coupled fusion module queries the elevation information of the elevation digital map from the map management module and incorporates the elevation information as a constraint into the optimization process of the second factor graph model.

Images