Multi-source fusion SLAM method and system based on mixed scale sliding window factor graph
By using hybrid-scale sliding window factor maps and a GPU-accelerated multi-threaded online real-time system, the problem of real-time mapping failure of single-scale sliding window factor maps is solved, achieving high real-time pose estimation and high-consistency mapping.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2025-12-16
- Publication Date
- 2026-03-31
Smart Images
Figure CN121761896A_ABST
Abstract
Description
Technical Field
[0001] This invention belongs to the field of multi-source fusion navigation and mapping technology, and relates to Simultaneous Localization and Mapping (SLAM) based on sliding window factor graphs. Specifically, it relates to a multi-source fusion SLAM method and system based on mixed-scale sliding window factor graphs. Background Technology
[0002] The factor graph-based multi-source fusion SLAM method establishes measurement models of navigation system state variables and landmark coordinates from different sensors, constructs residual factors between measurement and prediction, and then uses these residual factors to form a probabilistic graph of the navigation problem. Finally, it employs nonlinear least squares iteration to minimize the residuals, obtaining batch estimates of the navigation system state and landmark coordinates at each time step that are globally or locally optimal. This SLAM method has the advantages of flexible structure and good consistency, and is widely used for optimizing global or local mapping results in SLAM problems. However, the large number of factor nodes results in a high computational cost due to the need to iteratively solve a large-scale nonlinear least squares problem. Therefore, to obtain real-time navigation solution output, a sliding window factor graph based on Schur complement is needed to control the scale of the optimization problem. Current single-scale sliding window factor graph methods cannot simultaneously optimize historical states for mapping while performing real-time pose estimation for fused navigation, thus failing to complete the online mapping task.
[0003] In this context, the technical problem that needs to be solved is how to perform real-time pose estimation for fusion navigation, optimize historical states online for mapping, and control the time complexity so that the method can run on edge devices. Summary of the Invention
[0004] To address the aforementioned technical problems, this invention provides a multi-source fusion SLAM method and system based on hybrid-scale sliding window factor graphs. The method employs a small-scale sliding window for real-time pose estimation, providing high real-time navigation output; a large-scale sliding window is then used to iteratively optimize the locally optimal state optimized by the small-scale sliding window over a large scale range, obtaining more consistent mapping state estimates, thereby completing the online mapping task. The system is designed for a hybrid edge CPU-GPU architecture platform, using the GPU to accelerate the multi-factor batch edge-based Schur complement calculation in large-scale sliding window factor graph multi-source fusion mapping.
[0005] To achieve the above objectives, the present invention adopts the following technical solution:
[0006] In a first aspect, the present invention provides a multi-source fusion SLAM method based on a hybrid-scale sliding window factor map, comprising:
[0007] Once the carrier begins to move, the measurement data from the inertial measurement unit (IMU) is acquired in real time, and inertial navigation calculations are performed based on the measurement data from the inertial measurement unit.
[0008] When new measurement data is received, a time stamp is inserted at the measurement time, and the inertial navigation solution from the previous time stamp to the current time stamp is pre-integrated, which is regarded as one measurement; the new measurement data is the measurement data of other sensors besides the inertial measurement unit.
[0009] Treating all measurements as factors of the least squares problem, a nonlinear least squares problem is constructed for small-scale sliding window optimization. The inertial navigation solution is recalculated based on the navigation system state variables at the last time point of the current optimization, and the state at the current time is updated. The state at the current time is used as the real-time pose estimation output, and the state estimation results of the small-scale sliding window optimization are saved for large-scale sliding window optimization mapping.
[0010] When the number of factors reaches the preset small-scale sliding window limit, the Schul complement method is used to marginalize the factors related to the navigation system state variables at the oldest time scale.
[0011] When the number of factors reaches the preset large-scale sliding window limit, nonlinear least squares solution is performed in the large-scale sliding window based on the state estimation result optimized by the small-scale sliding window to obtain a state estimation result that is closer to the global optimum. This result is saved and output as the online mapping result.
[0012] After the large-scale sliding window optimization mapping is completed, GPU-accelerated batch Shure complement edgeification is performed on factors that exceed the preset large-scale sliding window limit.
[0013] Secondly, this invention provides a multi-source fusion SLAM multi-threaded online real-time running system based on a hybrid-scale sliding window factor graph, used to implement the above method, the system comprising:
[0014] The data acquisition and real-time pose output thread is used for:
[0015] The system receives raw measurement data from each sensor in real time and aligns the raw measurement data to a unified time system.
[0016] Upon receiving measurement data from the inertial measurement unit, inertial navigation calculations are performed based on the measurement data from the inertial measurement unit, and a real-time pose estimate is output.
[0017] When new measurement data is received, a time stamp is inserted at the measurement time. The inertial navigation solution from the previous time stamp to the current time stamp is pre-integrated and regarded as a single measurement. All measurements are used as factors of the least squares problem.
[0018] The small-scale sliding window optimization thread is used to construct nonlinear least squares problems for small-scale sliding window optimization and save the state estimation results of the small-scale sliding window optimization for use in the large-scale sliding window optimization thread.
[0019] The data acquisition and real-time pose output thread is also used to recalculate the inertial navigation based on the navigation system state variables at the last time point of the current small-scale sliding window optimization, update the state to the current time, and output the current state as the real-time pose estimation.
[0020] The large-scale sliding window optimization thread is used to perform nonlinear least squares calculation within the large-scale sliding window based on the state estimation results of the small-scale sliding window optimization when the number of factors reaches the preset large-scale sliding window limit, so as to obtain a state estimation result that is closer to the global optimum, which is then saved and output as the online mapping result.
[0021] The oldest factor marginalization thread is used to perform marginalization on factors related to the navigation system state variables at the oldest time scale when the number of factors reaches a preset small-scale sliding window limit;
[0022] The batch edge-mapping thread is used to perform GPU-accelerated batch Shure complement edge-mapping on factors that exceed the preset large-scale sliding window limit after the large-scale sliding window optimization mapping is completed.
[0023] Thirdly, the present invention provides an electronic device comprising: one or more processors; and a memory for storing one or more programs; wherein, when the one or more programs are executed by the one or more processors, the one or more processors cause the one or more processors to implement the above-described method.
[0024] Fourthly, the present invention provides a computer-readable storage medium having executable instructions stored thereon, which, when executed by a processor, enable the processor to implement the above-described method.
[0025] Fifthly, the present invention provides a non-transitory computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the above-described method.
[0026] The beneficial effects of this invention are as follows:
[0027] To address the limitation of single-scale sliding window factor graph methods in simultaneously optimizing historical states for mapping during real-time pose estimation in fusion navigation, this invention provides a multi-source fusion SLAM method based on hybrid-scale sliding window factor graphs. This method can output highly consistent mapping results online while providing high-real-time pose estimation. Furthermore, to address the high computational complexity and time overhead of edge-mapping in large-scale sliding window factor graph optimization, this invention provides a GPU-accelerated method for multi-factor batch edge-mapping in large-scale sliding window factor graph multi-source fusion mapping. This method enables rapid batch edge-mapping computation on edge CPU-GPU heterogeneous computing platforms. Finally, this invention employs multi-threading technology and designs an online real-time system for the proposed hybrid-scale sliding window factor graph multi-source fusion SLAM method. Attached Figure Description
[0028] Figure 1 This is a flowchart of the multi-source fusion SLAM method based on a hybrid scale sliding window factor map according to the present invention.
[0029] Figure 2 This is a flowchart illustrating a multi-source fusion SLAM method based on a hybrid scale sliding window factor map according to an embodiment of the present invention.
[0030] Figure 3 This is a schematic diagram of the multi-source fusion SLAM multi-threaded online real-time running system based on a hybrid scale sliding window factor graph according to the present invention. Detailed Implementation
[0031] The present invention will be further described below with reference to the accompanying drawings and embodiments.
[0032] The term “exemplary” as used herein means “serving as an example, embodiment, or illustration.” Any embodiment illustrated herein as “exemplary” is not necessarily to be construed as superior to or better than other embodiments.
[0033] The terms “first”, “second”, etc., are used to distinguish similar objects, not to describe or indicate a specific order or sequence.
[0034] The term "comprising" or any other similar term is intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus / device that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent in such process, method, article, or apparatus / device.
[0035] Furthermore, to better illustrate the present invention, numerous specific details are provided in the following detailed embodiments. Those skilled in the art should understand that the present invention can be practiced without certain specific details. In some instances, methods, means, elements, and circuits well known to those skilled in the art have not been described in detail in order to highlight the spirit of the invention.
[0036] Figure 1 The flowchart of the multi-source fusion SLAM method based on hybrid-scale sliding window factor graphs of the present invention is shown, as follows: Figure 1 As shown, the method includes:
[0037] S101. After the carrier starts moving, the measurement data of the inertial measurement unit is acquired in real time, and inertial navigation calculation is performed based on the measurement data of the inertial measurement unit.
[0038] S102. When new measurement data is received, a time stamp is inserted at the measurement time, and the inertial navigation calculation from the previous time stamp to the current time stamp is pre-integrated and regarded as a measurement; the new measurement data is the measurement data of other sensors besides the inertial measurement unit.
[0039] S103. Small-scale sliding window optimization and saving of optimization results: Treat all measurements as factors of the least squares problem, construct a nonlinear least squares problem for small-scale sliding window optimization; recalculate the inertial navigation based on the navigation system state variables at the last time point of the current optimization, update the state to the current time, use the current state as the real-time pose estimation output, and save the state estimation results of the small-scale sliding window optimization for large-scale sliding window optimization mapping.
[0040] For example, the state estimation results of small-scale sliding window optimization include the navigation system state variables and landmark coordinates at each time point after small-scale sliding window optimization.
[0041] For example, the method further includes: during the small-scale sliding window optimization process, saving the original measurement data at each time scale for use in calculating the residuals during the large-scale sliding window optimization process.
[0042] S104. Small-scale sliding window marginalization: When the number of factors reaches the preset small-scale sliding window limit, the Schur complement method is used to marginalize the factors related to the navigation system state variables at the oldest time scale in order to control the amount of computation.
[0043] For details on the implementation process of edgedification, please refer to relevant technologies.
[0044] S105. Large-scale sliding window optimization: When the number of factors reaches the preset large-scale sliding window limit, nonlinear least squares solution is performed in the large-scale sliding window based on the state estimation result of the small-scale sliding window optimization to obtain a state estimation result that is closer to the global optimum. This result is saved and output as the online mapping result.
[0045] For example, large-scale sliding window optimization specifically includes:
[0046] (1) Recalculation of residuals: Calculate the residuals between the original measurement data and the navigation system state variables and landmark coordinates at each time point after optimization by the small-scale sliding window under the large-scale sliding window.
[0047] (2) Construction of the large-scale sliding window optimization problem: Based on the measurement equations corresponding to each sensor, the state estimation results of the small-scale sliding window optimization are used as the linearization points to calculate the Jacobian matrix of the large-scale sliding window optimization problem. The Jacobian matrix is used to construct the large-scale sliding window optimization solver.
[0048] (3) Iterative solution: The state estimation result based on the small-scale sliding window optimization is iteratively solved in the large-scale sliding window until the residual converges to below the preset value, so as to obtain a state estimation result that is closer to the global optimum.
[0049] S106, GPU-accelerated large-scale sliding window edge detection: After the large-scale sliding window optimization mapping is completed, GPU-accelerated batch Shure complement edge detection is performed on factors that exceed the preset large-scale sliding window limit.
[0050] For example, a GPU-accelerated batch Shure edge detection method may include the following steps:
[0051] (1) Perform block rearrangement of the information matrix, separate the factors to be marginalized and their related factors from the information matrix, and rearrange the information matrix according to the dimension m of the factors to be marginalized and the dimension n of their related factors. The equation to be solved in the optimization problem becomes:
[0052] ;
[0053] in, The dimension is m×m, which is the information matrix corresponding to the factor to be marginalized (i.e. the information matrix to be marginalized). The dimension is n×n, which is the information matrix of factors related to the factor to be marginalized; The upper right corner of the information matrix after block rearrangement represents the correlation information of the factor to be marginalized and its related factors. The dimension is m×1, which represents the state to be estimated that is only associated with the factor to be marginalized; The dimension is n×1, which represents the estimated state that is associated with the factor to be marginalized but needs to be retained; , They represent , The minute increment is obtained by solving the above equation; The dimension is m×1, which is the information vector of the factor to be marginalized; The dimension is n×1, which is the information vector of the factors related to the factor to be marginalized; the superscript T indicates the transpose of the matrix.
[0054] (2) Memory allocation: according to The size is allocated corresponding memory in the GPU, and It is copied from CPU memory to GPU memory.
[0055] (3) Choleski decomposition of the information matrix to be marginalized: using The positive definite symmetry of the matrix allows us to obtain the lower triangular matrix using the Choreski decomposition operator. Store the decomposition results in a dedicated file. The lower triangular portion of the allocated GPU memory does not occupy new GPU memory space.
[0056] (4) Inverting a lower triangular matrix: Using the lower triangular matrix inversion operator, we obtain... inverse matrix Stored in The lower triangular portion of the allocated GPU memory.
[0057] (5) Clear the upper triangular part: Clear The upper triangular part should be avoided to prevent contamination of subsequent calculations.
[0058] (6) Solving the lower triangular part of the inverse of the information matrix to be marginalized: using the rank k update operator to calculate ,because It is symmetric, only calculating the lower triangular part of the matrix, and the calculation result is stored in [location missing]. The lower triangular portion of the allocated GPU memory.
[0059] (7) Recovering the upper triangular part of the inverted information matrix to be marginalized: copying The lower triangular portion excluding the diagonal elements to the upper triangular portion is restored. The upper triangular part.
[0060] (8) Calculate matrix multiplication: Use matrix multiplication operators for parallel computation. The parallel solving capability of GPUs can be used to quickly transform the equations to be solved into:
[0061] ;
[0062] in, Represents the identity matrix. Information matrix of prior factors. Only with The associated factors are removed from the optimizer, but their constraints remain in the prior factors. The equation to be solved for the prior factors becomes:
[0063] ;
[0064] (9) GPU-accelerated Jacobian calculation of prior factors: Jacobian matrix of prior factors is calculated using the Cholesky decomposition operator. The calculation formula is:
[0065] ;
[0066] The above steps can be performed on servers, desktop computers, or edge heterogeneous computing platforms with GPUs. Through parallel computing, rapid batch edge computing can be achieved, especially accelerating large-scale matrix inversion and matrix multiplication in Shure complement computation.
[0067] The multi-source fusion SLAM method based on hybrid-scale sliding window factor graphs proposed in this invention can output highly consistent mapping results online while providing high real-time pose estimation. The multi-factor batch edge-mapping GPU-accelerated method for large-scale sliding window factor graph multi-source fusion mapping (i.e., GPU-accelerated batch Shure complement edge-mapping method) proposed in this invention can achieve fast batch edge-mapping computation on edge CPU-GPU heterogeneous computing platforms.
[0068] The multi-source fusion SLAM method based on hybrid-scale sliding window factor graphs of the present invention will be described in detail below with reference to the accompanying drawings and embodiments.
[0069] Figure 2 This diagram illustrates a flowchart of a multi-source fusion SLAM method based on a hybrid-scale sliding window factor graph according to an embodiment of the present invention. Figure 2 As shown, it includes the following steps:
[0070] Step 1: Based on the measurements of each sensor With the state variables to be estimated A measurement model is established between (including navigation system state variables and landmark coordinates) to create nonlinear measurement equations for each sensor. The residual between the sensor measurement and the state estimate is: , This is an estimate of the current state. A cost function for the weighted least squares estimation is established. :
[0071] ;
[0072] in, The covariance matrix of the sensor measurements is given by the inverse matrix of the Chollisky decomposition, which yields the following result. ,but:
[0073] ;
[0074] ;
[0075] in, This is the measurement error function.
[0076] Step 2: Weighted measurement error function After performing first-order linearization, we obtain the calculation models for the measurement Jacobian matrix and information matrix:
[0077] ;
[0078] ;
[0079] in, For the linearization point, For relative to a tiny increment, For the measurement Jacobian matrix, the information matrix .
[0080] Step 3: Once the carrier begins to move, if there are no new measurements, the system performs pure inertial navigation (hereinafter referred to as inertial navigation) calculations based on the output of the inertial measurement unit, and outputs navigation parameters based on the inertial navigation calculation results.
[0081] Step 4: When a new measurement is made, insert a timescale at the measurement time and pre-integrate the inertial navigation solution from the previous timescale to the current timescale as a single measurement. All measurements are considered as factors of the least squares problem.
[0082] Step 5: Construct a weighted nonlinear least squares problem based on the preset measurement equations of each sensor, the variance of each measurement, and the residuals between each measurement and the state estimate.
[0083] Step 6: Calculate the Jacobian matrix based on the Jacobian calculation model for each sensor. The linearization point is selected from the current prior estimate. The prior estimate of the state at the old time node (i.e., the scale) is the posterior estimate obtained from the previous solution (i.e., the posterior obtained by using measurement information and factor graph optimization); the prior estimate of the state at the newly added time node is the prediction derived from the state equation (i.e., the prior of the state quantity obtained by recursion based on the system state equation).
[0084] Step 7: Perform weighted linear least squares at the current linearization point, and use the solution obtained in Step 2. Substituting the first-order linear model into the cost function in step 1 yields:
[0085] ;
[0086] State residual at the current linearization point for:
[0087] ;
[0088] The optimization problem can be transformed into finding the extreme points of the cost function:
[0089] ;
[0090] Right now:
[0091] ;
[0092] in, Let be the information vector. Iteratively update the linearization point as... Repeat the solution until... If the convergence falls below the preset value, then it is considered that the value at this point is... This is the optimal state estimation result under small-scale sliding window optimization.
[0093] Step 8: Save the state estimation results obtained in Step 7 for large-scale sliding window optimization mapping.
[0094] Step 9: Recalculate the inertial navigation state from the last time point of the current optimization, update it to the current state, and output it in real time.
[0095] Step 10: Repeat steps 3-9 until the number of factors reaches the preset small-scale sliding window limit. When the number of factors exceeds the preset small-scale sliding window limit, update the Jacobian matrix using the Schur complement method. This eliminates the oldest factor (i.e., the factor related to the navigation system state at the oldest timescale) and its associated state variables to be estimated in the least squares problem. The constraint information of the oldest factor is incorporated into the factors related to it through the Shure complement operation, forming a dense block of prior information in the information matrix. The factors in this block are called prior factors. The linearization points of the prior factor measurements are no longer updated with changes in state estimation. The size of the window is always kept within a small scale to control the amount of computation.
[0096] Step 11: Repeat steps 3-10 until the number of factors reaches the preset large-scale sliding window limit. Perform nonlinear least squares calculations on the previously saved small-scale sliding window optimization state estimation results within the large-scale sliding window to obtain historical estimated state variables that are closer to the global optimum, which are then used as online mapping results and output.
[0097] Step 12: Continue to perform small-scale sliding window optimization real-time solution until the large-scale sliding window optimization mapping solution is completed. Perform GPU-accelerated batch Shure complement marginalization on factors that exceed the large-scale window.
[0098] By repeating the above steps, multi-source fusion SLAM based on mixed-scale sliding window factor maps can be performed, outputting high-frequency pose estimation in real time and providing highly consistent mapping results online.
[0099] This invention also proposes a multi-source fusion SLAM multi-threaded online real-time running system based on a hybrid-scale sliding window factor graph, which is used to implement the above-mentioned multi-source fusion SLAM method based on a hybrid-scale sliding window factor graph. Figure 3 This diagram illustrates the multi-source fusion SLAM multi-threaded online real-time running system based on a hybrid-scale sliding window factor graph according to the present invention. Figure 3 As shown, the system includes:
[0100] The data acquisition and real-time pose output thread (i.e., thread 1) performs real-time processing of raw sensor measurement data, inertial navigation calculation, factor creation, time scale management, and real-time pose estimation output.
[0101] The small-scale sliding window optimization thread (i.e., thread 2) calculates and saves the state to be estimated within the small-scale window, which is used for real-time pose estimation updates and large-scale sliding window factor map construction.
[0102] The large-scale sliding window optimization thread (i.e., thread 3) continues to iterate and solve based on the small-scale optimization results, calculates and saves the state to be estimated within the large-scale window as the online mapping result.
[0103] The oldest factor marginalization thread (i.e., thread 4) performs marginalization on factors related to the navigation system state variables at the oldest time scale.
[0104] The batch marginalization thread (i.e., thread 5) performs batch marginalization on all factors that exceed the preset large-scale sliding window limit.
[0105] The data acquisition and real-time pose output thread is triggered after the inertial navigation system (INS) completes initial alignment. In this thread, raw measurement data from each sensor is received in real time and aligned to a unified time system. INS calculations are performed when IMU data arrives, outputting a real-time pose estimate. When other measurement data arrives, factors for the state variables to be estimated are created at the corresponding time scale based on the measurement model, including pre-integration factors from the previous time scale to the current time scale. A small-scale sliding window optimization thread is triggered when a preset key measurement arrives; a large-scale sliding window optimization thread is triggered when the number of preset key measurements (i.e., the number of key measurement factors) reaches a preset large-scale sliding window limit. When small-scale sliding window optimization is complete, the INS is recalculated to the current state using the optimized state and the updated zero bias, achieving real-time pose estimation. The preset key measurement can be designed as a visual keyframe or a satellite navigation signal, etc. The arrival of a preset key measurement means the system detects a new preset key measurement input and adds a time node (i.e., a time scale) to it.
[0106] The small-scale sliding window optimization thread is triggered when a preset key measurement arrives in the data acquisition and real-time pose output thread. In this thread, the state variables to be estimated within the small-scale window are calculated according to the aforementioned small-scale sliding window optimization method. Upon completion of optimization, the state estimation results obtained from the small-scale sliding window optimization are saved for use in the large-scale sliding window optimization thread. When the number of key measurement factors within the window equals the preset small-scale sliding window limit, the oldest factor marginalization thread is triggered.
[0107] The large-scale sliding window optimization thread is triggered when the number of key measurements in the data acquisition and real-time pose output threads reaches a preset large-scale sliding window limit. In this thread, following the aforementioned large-scale sliding window optimization method, based on the state estimation results obtained from the small-scale sliding window optimization, the state variables to be estimated within the large-scale window are calculated. When optimization is complete, the state estimation results obtained from the large-scale sliding window optimization are saved and output as the online mapping result. When the number of key measurement factors within the window exceeds the preset large-scale sliding window limit, a batch edge detection thread is triggered.
[0108] The oldest factor marginalization thread is triggered when the number of key measurement factors within the window in the small-scale sliding window optimization thread equals the preset small-scale sliding window limit. In the oldest factor marginalization thread, the factors related to the navigation system state variables at the oldest time scale are marginalized according to the aforementioned Schur complement-based marginalization method.
[0109] The batch marginalization thread is triggered when the number of key measurement factors within the window exceeds a preset large-scale sliding window limit in the large-scale sliding window optimization thread. In the batch marginalization thread, batch marginalization is performed on all factors exceeding the preset large-scale sliding window limit, following the aforementioned GPU-accelerated batch Shure complement marginalization method.
[0110] The aforementioned multi-threaded online real-time running system can implement the aforementioned multi-source fusion SLAM method based on hybrid-scale sliding window factor graphs in servers, desktop computers, or edge heterogeneous computing platforms equipped with CPUs and GPUs.
[0111] The present invention also provides an electronic device, comprising: one or more processors; and a memory for storing one or more programs; wherein, when the one or more programs are executed by the one or more processors, the one or more processors implement the above-described multi-source fusion SLAM method based on a hybrid scale sliding window factor map.
[0112] The present invention also provides a computer-readable storage medium storing executable instructions thereon, which, when executed by a processor, enable the processor to implement the above-described multi-source fusion SLAM method based on a hybrid scale sliding window factor map.
[0113] The present invention also provides a non-transitory computer-readable storage medium storing a computer program thereon, which, when executed by a processor, implements the above-described multi-source fusion SLAM method based on a hybrid-scale sliding window factor map.
[0114] Those skilled in the art will understand that embodiments of the present invention can be provided as methods, systems, or computer program products. Therefore, the present invention can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention can take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code. The embodiments of the present invention can be implemented using various computer languages, for example, C++.
[0115] This invention is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart illustrations and / or block diagrams. Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.
[0116] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.
[0117] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.
[0118] The contents not described in detail in this specification are existing technologies known to those skilled in the art.
[0119] The specific embodiments described above further illustrate the purpose, technical solution, and beneficial effects of the present invention. It should be understood that the above descriptions are merely specific embodiments of the present invention and are not intended to limit the present invention. The above descriptions are exemplary and not exhaustive. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.
Claims
1. A multi-source fusion SLAM method based on mixed-scale sliding window factor graph, characterized in that, The method comprises: After the carrier starts to move, real-time measurement data of an inertial measurement unit is acquired, and inertial navigation is calculated according to the measurement data of the inertial measurement unit; When new measurement data is received, a time mark is inserted at a measurement time, pre-integration of inertial navigation calculation from a previous time mark to a current time mark is performed, and the new measurement data is measurement data of other sensors except the inertial measurement unit; All measurements are regarded as factors of a least square problem, a nonlinear least square problem is constructed for small-scale sliding window optimization, inertial navigation is recalculated according to a navigation system state quantity at a last time mark of current optimization, a state at the current time is updated, the state at the current time is output as real-time pose estimation, and state estimation results of the small-scale sliding window optimization are saved for large-scale sliding window optimization mapping; When the number of factors reaches a preset small-scale sliding window limit, a Schur complement method is used to perform marginalization on factors related to the navigation system state quantity at the oldest time mark; When the number of factors reaches a preset large-scale sliding window limit, nonlinear least square calculation is performed in a large-scale sliding window according to the state estimation results of the small-scale sliding window optimization, state estimation results closer to global optimization are obtained, saved as online mapping results, and output; After the large-scale sliding window optimization mapping is completed, GPU-accelerated batch Schur complement marginalization is performed on factors exceeding the preset large-scale sliding window limit.
2. The method of claim 1, wherein, The state estimation results of the small-scale sliding window optimization include navigation system state quantities at each time mark after small-scale sliding window optimization and landmark point coordinates.
3. The method of claim 2, wherein, The method further comprises: During the small-scale sliding window optimization process, original measurement data at each time mark is saved for calculating residuals during large-scale sliding window optimization.
4. The method of claim 3, wherein, The nonlinear least square calculation in the large-scale sliding window according to the state estimation results of the small-scale sliding window optimization includes: Calculating residuals between the original measurement data and the navigation system state quantities at each time mark after small-scale sliding window optimization and the landmark point coordinates in the large-scale sliding window; Based on measurement equations corresponding to each sensor, taking the state estimation results of the small-scale sliding window optimization as a linearization point, a Jacobian matrix of a large-scale sliding window optimization problem is calculated; Iterative solving in the large-scale sliding window based on the state estimation results of the small-scale sliding window optimization is performed until the residuals converge to below a preset value, and the state estimation results closer to global optimization are obtained.
5. The method of claim 1, wherein, The GPU-accelerated batch Schur complement marginalization on factors exceeding the preset large-scale sliding window limit includes: Block rearrangement of an information matrix is performed, the factors to be marginalized and factors related to the factors to be marginalized are separated from the information matrix, and block rearrangement of the information matrix is performed according to dimensions m of the factors to be marginalized and dimensions n of the factors related to the factors to be marginalized, and a to-be-solved equation of the optimization problem becomes: ； wherein, is an information matrix corresponding to the factor to be marginalized, with dimension m x m; is an information matrix corresponding to the factor related to the factor to be marginalized, with dimension n x n; is the upper right corner part of the rearranged information matrix, representing the associated information of the factor to be marginalized and the factor related to it; is the state to be estimated associated with the factor to be marginalized, with dimension m x 1; is the state to be estimated associated with the factor to be marginalized, but needs to be reserved, with dimension n x 1; , respectively represent , a small increment of is an information vector of the factor to be marginalized, with dimension m x 1; is an information vector of the factor related to the factor to be marginalized, with dimension n x 1; the superscript T represents the transpose of the matrix; According to the size of the corresponding memory in the GPU, copy from the CPU memory to the GPU memory; GPU-accelerated Schur complement solving based on Cholesky decomposition is performed, and the to-be-solved equation is quickly converted into by using GPU parallel solving capability: ； wherein is an information matrix of the prior factors; denotes the identity matrix; only the associated factors are removed from the optimizer, but their constraints remain in the prior factors; The Jacobian matrix of the prior factor is calculated using a Cholesky decomposition operator The formula is: 。 6. The method of claim 5, wherein, The GPU-accelerated Schur complement solving based on Cholesky decomposition includes: Utilizing the positive definite symmetry, the lower triangular matrix is obtained using Cholesky decomposition operator, and the decomposition result is stored in the lower triangular part of the GPU memory allocated for , without occupying new GPU memory space; Using the lower triangular matrix inversion operator gives , stored in the lower triangular part of the GPU memory allocated for ; clearing the upper triangular part of the matrix, avoiding contaminating subsequent computations; Using the rank-k update operator computation Since is symmetric, only the lower triangular part of the matrix is computed, and the result is stored in the lower triangular part of the GPU memory allocated for copy lower triangular part to upper triangular part excluding diagonal elements, restore upper triangular part Using matrix multiplication operators, parallel computation ; information matrix of the prior factor The equation to be solved for the prior factor part becomes 。 7. A multi-source fusion SLAM multi-threaded online real-time running system based on mixed-scale sliding window factor graph, for implementing the method of any one of claims 1-6, characterized in that, The system comprises: A data acquisition and real-time pose output thread is configured to: Real-time receive raw measurement data of each sensor, and align the raw measurement data to a unified time system; When receiving the measurement data of the inertial measurement unit, perform inertial navigation calculation according to the measurement data of the inertial measurement unit, and output real-time pose estimation; When receiving new measurement data, insert a time mark at the measurement time, pre-integrate the inertial navigation calculation from the last time mark to the current time mark, regard it as a measurement, and take all measurements as factors of a least square problem; Small-scale sliding window optimization thread, used for constructing a nonlinear least square problem for small-scale sliding window optimization, and saving the state estimation result of small-scale sliding window optimization for large-scale sliding window optimization thread; The data acquisition and real-time pose output thread is also used for re-performing inertial navigation calculation according to the navigation system state quantity at the last time mark of the current small-scale sliding window optimization, updating the state at the current time, and outputting the state at the current time as real-time pose estimation; Large-scale sliding window optimization thread, used for performing nonlinear least square calculation in a large-scale sliding window according to the state estimation result of the small-scale sliding window optimization when the number of factors reaches a preset large-scale sliding window limit, obtaining a state estimation result closer to a global optimal result, saving and outputting the state estimation result as an online mapping result; Oldest factor marginalization thread, used for performing marginalization on factors related to the navigation system state quantity at the oldest time mark by using a Schur complement method when the number of factors reaches a preset small-scale sliding window limit; Batch marginalization thread, used for performing GPU-accelerated batch Schur complement marginalization on factors exceeding the preset large-scale sliding window limit after the large-scale sliding window optimization mapping ends.
8. An electronic device, comprising: comprise: one or more processors; a memory for storing one or more programs; wherein when the one or more programs are executed by the one or more processors, the one or more processors implement the method of any one of claims 1-6.
9. A computer-readable storage medium, characterized in that, having stored thereon executable instructions that, when executed by a processor, enable the processor to implement the method of any one of claims 1-6.
10. A non-transitory computer-readable storage medium, comprising: having stored thereon a computer program which, when executed by a processor, implements the method of any one of claims 1-6.