An elastic sliding window factor graph optimization method for asynchronous multi-source measurement fusion positioning

Abstract

The application discloses a kind of flexible sliding window factor graph optimization methods for asynchronous multi-source measurement fusion positioning, belong to multi-source measurement fusion positioning and factor graph optimization technical field, for flexible sliding window factor graph optimization, including obtaining multi-source asynchronous observation information and instantiating state node, measurement factor in factor graph is built;The number of effective measurement constraints of the latest state is counted, and is divided into single-constraint state or multi-constraint state;Single-constraint state is only solved latest state and window is elastically expanded;Multi-constraint state triggers current flexible window full-window factor graph optimization;After optimization, the preset window size is recovered according to the difference between the actual length of window and the size of base window to batch marginalization;Output fusion positioning result and execute cyclically.The application switches optimization strategy adaptively, avoids redundant full-window optimization and frequent marginalization in single-constraint stage, while maintaining positioning accuracy, significantly reduces the computing burden.

Description

Technical Field

[0001] This invention discloses an elastic sliding window factor graph optimization method for asynchronous multi-source measurement fusion positioning, belonging to the technical field of multi-source measurement fusion positioning and factor graph optimization. Background Technology

[0002] In practical multi-source asynchronous measurement fusion positioning, the frequencies of different measurement sources often differ significantly. For example, visual inertial odometry can continuously output relative pose measurements at a high frequency, while global measurements such as GNSS, magnetometers, and barometers have lower output frequencies and may be intermittently missing due to obstruction, interference, or equipment limitations. If the traditional fixed sliding window factor graph optimization method is still used, the system will typically trigger a full-window nonlinear optimization every time a new state is added to the sliding window, and then perform marginalization after optimization to maintain a fixed window size.

[0003] This traditional approach leads to significant computational resource waste under multi-source asynchronous measurement conditions. When a state is instantiated based on a high-frequency measurement source, but a low-frequency measurement has not yet arrived, the newly added state may only be constrained by a single measurement factor. For this single-constraint case, this invention, through optimization characteristic analysis, shows that the optimal estimate of the older state within the window remains unchanged from the previous round's optimal estimate, and the latest state can be solved independently by the corresponding single-measurement constraint equation. Therefore, continuing to perform full-window nonlinear optimization on the entire sliding window is unnecessary. Operations such as full-window linearization, iterative solving, marginalization, and prior factor reconstruction can all be avoided, thereby reducing the computational burden in multi-source asynchronous measurement fusion localization.

[0004] Furthermore, when multiple consecutive latest states are constrained by only a single measurement factor, according to the analysis conclusion of the single-constraint state optimization characteristics of this invention, the latest state can be solved independently by the corresponding measurement constraint while maintaining the existing window state as the optimal estimate of the previous round, thus eliminating the need to trigger full-window optimization. To reduce unnecessary window reconstruction and ensure that multiple types of measurement constraints are still retained within the sliding window when full-window optimization is triggered again, this invention does not perform marginalization during single-constraint state updates and keeps the left boundary of the sliding window unchanged, allowing newly added single-constraint states to fill the window sequentially from the right side; when the latest state is again constrained by two or more measurement factors, full-window optimization and batch marginalization are then performed on the current elastic window. Summary of the Invention

[0005] The purpose of this invention is to provide an elastic sliding window factor graph optimization method for asynchronous multi-source measurement fusion positioning, so as to solve the problems of full-window optimization redundancy and waste of computational resources in the existing fixed sliding window factor graph optimization method in multi-source asynchronous measurement fusion positioning.

[0006] A flexible sliding window factor graph optimization method for asynchronous multi-source measurement fusion localization includes: S1. Obtain asynchronous observation information from different measurement sources, generate state nodes using the preset output frequency of the high-frequency local measurement source, associate the asynchronous observation information from different measurement sources with the corresponding state nodes, and construct a state set within an elastic sliding window; construct the measurement factors in the factor graph from the asynchronous observation information associated with the state nodes. S2. Count the number of valid measurement factors directly associated with the latest state. Based on the number of valid measurement factors in the latest state, divide it into single-constraint state and multi-constraint state. If it is a single-constraint state, execute S3; if it is a multi-constraint state, execute S4. S3. For the latest state of a single-constraint state, construct the latest state optimization objective function based on the effective measurement factors and solve it to obtain the optimal estimate of the latest state. During the solution of the single-constraint state, keep the other states of the elastic sliding window unchanged and retain the latest state at the right end of the elastic sliding window. The right boundary of the elastic sliding window expands as the latest state is added. S4. For the latest state of the multi-constraint state, construct the full-window optimization objective function based on the set of all effective measurement factors in the current elastic sliding window and solve it to obtain the optimal estimate of all states in the current window. S5. Set a base sliding window. If the current elastic sliding window is larger than the base sliding window, perform batch edge-trimming on the oldest state that exceeds the limit. S6. After step S3 is completed, output the optimal estimate of the latest state and keep the estimated values ​​of the remaining states in the sliding window unchanged. After steps S4 and S5 are completed, the optimal estimate of the latest state is output, and the optimal estimates of the remaining states within the current elastic sliding window are updated synchronously. After obtaining the optimal estimate, return to step S1 to continue execution.

[0007] S1 includes S1.1, where the high-frequency local measurement source is the measurement source with the highest output frequency specified during initialization; Let the state of the latest state node be... The set of states within the current elastic sliding window for: ; In the formula, This is the time step index for the current left boundary state of the elastic sliding window. The time step index for the right boundary state; Each state includes at least the position and orientation of the mobile platform: ; In the formula, For mobile platforms in the world coordinate system The lower position, For mobile platform slave system To the world coordinate system The posture.

[0008] S1 includes S1.2, and each measurement factor corresponds to a residual term: ; In the formula, In order to be with the first The time step associated with the first One measurement information, This is the residual function corresponding to the measurement information.

[0009] S2 includes statistics. Number of directly related effective measurement factors If multiple measurement factors originate from the same measurement source and have the same type of observation information, they are merged into one category of valid measurement factors; if the timestamp of a measurement factor exceeds the maximum allowable time difference with the state node, it is determined to be an abnormal observation quality and is not included in the number of valid measurement factors. when At that time, the judgment It is a single-constraint state; when At that time, the judgment This is a multi-constraint state.

[0010] S3 includes, and The only effective measurement factor that is directly related is The corresponding measurement is Construct the latest state optimization objective function: ; In the formula, This is the optimal estimate for the latest state. This is an estimate of the existing states in the current window, excluding the latest state.

[0011] S4 includes a full-window optimization objective function: ; In the formula, To optimize for the entire window, The a priori residuals formed by historically marginalized information, It is the set of all valid measurement factors within the current elastic sliding window. This is the information matrix corresponding to the measurement factors. The Mahalanobis distance; The optimal state estimate is obtained by solving the full-window optimization objective using the nonlinear least squares method. .

[0012] S5 includes, S5.1, setting the basic sliding window size as... The number of states in the current elastic sliding window is , ; when At that time, the oldest prior art was constructed using the Schul complement or equivalent prior art method. Batch edge-factoring of each state; when At this time, edgeware is not performed, and the current window remains unchanged.

[0013] S5 includes S5.2, in factor graph optimization. In the process, the variable block corresponding to the marginalized state is eliminated, and the constraint information is passed to the prior factors of the retained state through the Schur complement; After completing batch edge detection, update the left boundary of the elastic sliding window, retaining the most recent edge detection. Each state and its corresponding measurement factor will be used as a historical constraint for the next round of elastic sliding window optimization, and the window size will be restored to the basic window size. .

[0014] Compared with existing technologies, this invention has the following advantages: This invention determines whether full-window optimization is needed based on the number of effective measurement constraints on the latest state. When the latest state is only constrained by a single effective measurement factor, only the latest state is solved without repeatedly optimizing the entire window, thus avoiding the large amount of redundant computation generated by the traditional fixed sliding window method under high-frequency single-constraint updates; the latest state can still be quickly calculated and output under single-constraint conditions without reducing the output frequency of positioning results; at the same time, it avoids full-window nonlinear optimization, matrix solving, and frequent marginalization operations, significantly reducing the time consumption and processor utilization of a single state update; marginalization is temporarily suspended during continuous single-constraint updates, and batch marginalization is performed after the multi-source constraints are reformed. Compared with the traditional method of performing marginalization after each state is added, this reduces the number of prior factor reconstructions, reduces window maintenance overhead, and ensures... This invention supports diverse measurements within a sliding window; it does not require synchronous output from all measurement sources, nor does it require multiple measurement factor constraints for each state. Instead, it dynamically adjusts the optimization strategy based on the actual factor connection relationships, making it suitable for practical fusion positioning scenarios with varying measurement frequencies, intermittent missing low-frequency measurements, and continuous high-frequency output of local measurements. It replaces the full-window nonlinear optimization of numerous single-constraint moments with solving the latest state individually, significantly reducing peak and average computational burden. Therefore, it is more suitable for deployment in mobile robots, intelligent vehicles, unmanned platforms, underground space inspection equipment, and other embedded positioning terminals. The core criterion is the number of effective measurement constraints in the latest state, independent of any specific measurement source or residual form. Therefore, GNSS position factors, visual-inertial relative pose factors, magnetometer orientation factors, barometer altitude factors, wheel speed factors, map matching factors, and other factorizable measurements can all be incorporated into this invention's method. Attached Figure Description

[0015] Figure 1 This is a flowchart of the method of the present invention. Detailed Implementation

[0016] To make the objectives, technical solutions, and advantages of this invention clearer, the technical solutions of this invention are described clearly and completely below. Obviously, the described embodiments are only some, not all, of the embodiments of this invention. All other embodiments obtained by those skilled in the art based on the embodiments of this invention without creative effort are within the scope of protection of this invention.

[0017] A flexible sliding window factor graph optimization method for asynchronous multi-source measurement fusion localization includes: S1. Obtain asynchronous observation information from different measurement sources, generate state nodes using the preset output frequency of the high-frequency local measurement source, associate the asynchronous observation information from different measurement sources with the corresponding state nodes, and construct a state set within an elastic sliding window; construct the measurement factors in the factor graph from the asynchronous observation information associated with the state nodes. S2. Count the number of valid measurement factors directly associated with the latest state. Based on the number of valid measurement factors in the latest state, divide it into single-constraint state and multi-constraint state. If it is a single-constraint state, execute S3; if it is a multi-constraint state, execute S4. S3. For the latest state of a single-constraint state, construct the latest state optimization objective function based on the effective measurement factors and solve it to obtain the optimal estimate of the latest state. During the solution of the single-constraint state, keep the other states of the elastic sliding window unchanged and retain the latest state at the right end of the elastic sliding window. The right boundary of the elastic sliding window expands as the latest state is added. S4. For the latest state of the multi-constraint state, construct the full-window optimization objective function based on the set of all effective measurement factors in the current elastic sliding window and solve it to obtain the optimal estimate of all states in the current window. S5. Set a base sliding window. If the current elastic sliding window is larger than the base sliding window, perform batch edge-trimming on the oldest state that exceeds the limit. S6. After step S3 is completed, output the optimal estimate of the latest state and keep the estimated values ​​of the remaining states in the sliding window unchanged. After steps S4 and S5 are completed, the optimal estimate of the latest state is output, and the optimal estimates of the remaining states within the current elastic sliding window are updated synchronously. After obtaining the optimal estimate, return to step S1 to continue execution.

[0018] S1 includes S1.1, where the high-frequency local measurement source is the measurement source with the highest output frequency specified during initialization; Let the state of the latest state node be... The set of states within the current elastic sliding window for: ; In the formula, This is the time step index for the current left boundary state of the elastic sliding window. The time step index for the right boundary state; Each state includes at least the position and orientation of the mobile platform: ; In the formula, For mobile platforms in the world coordinate system The lower position, For mobile platform slave system To the world coordinate system The posture.

[0019] S1 includes S1.2, and each measurement factor corresponds to a residual term: ; In the formula, In order to be with the first The time step associated with the first One measurement information, This is the residual function corresponding to the measurement information.

[0020] S2 includes statistics. Number of directly related effective measurement factors If multiple measurement factors originate from the same measurement source and have the same type of observation information, they are merged into one category of valid measurement factors; if the timestamp of a measurement factor exceeds the maximum allowable time difference with the state node, it is determined to be an abnormal observation quality and is not included in the number of valid measurement factors. when At that time, the judgment It is a single-constraint state; when At that time, the judgment This is a multi-constraint state.

[0021] S3 includes, and The only effective measurement factor that is directly related is The corresponding measurement is Construct the latest state optimization objective function: ; In the formula, This is the optimal estimate for the latest state. This is an estimate of the existing states in the current window, excluding the latest state.

[0022] S4 includes a full-window optimization objective function: ; In the formula, To optimize for the entire window, The a priori residuals formed by historically marginalized information, It is the set of all valid measurement factors within the current elastic sliding window. This is the information matrix corresponding to the measurement factors. The Mahalanobis distance; The optimal state estimate is obtained by solving the full-window optimization objective using the nonlinear least squares method. .

[0023] S5 includes, S5.1, setting the basic sliding window size as... The number of states in the current elastic sliding window is , ; when At that time, the oldest prior art was constructed using the Schul complement or equivalent prior art method. Batch edge-factoring of each state; when At this time, edgeware is not performed, and the current window remains unchanged.

[0024] S5 includes S5.2, in factor graph optimization. In the process, the variable block corresponding to the marginalized state is eliminated, and the constraint information is passed to the prior factors of the retained state through the Schur complement; After completing batch edge detection, update the left boundary of the elastic sliding window, retaining the most recent edge detection. Each state and its corresponding measurement factor will be used as a historical constraint for the next round of elastic sliding window optimization, and the window size will be restored to the basic window size. .

[0025] The states of the present invention may also include speed, inertial device bias, wheel speed scaling factor, time offset, or other parameters to be estimated.

[0026] The number of effective measurement constraints in this invention can be counted according to the number of measurement factors or according to the effective measurement category. If multiple factors originate from the same measurement source and only provide duplicate information, they can be merged into one type of effective constraint according to a preset strategy; if a measurement factor is masked due to abnormal observation quality, timestamp mismatch, excessive residual, or unsatisfactory health status, it will not be included in the number of effective measurement constraints.

[0027] The maximum allowable time difference between the present invention and the state node is 1 second under the condition of high-frequency measurement of 10Hz.

[0028] The "elastic sliding window" in this invention refers to the fact that the actual length of the sliding window is not forcibly kept constant after each state update, but rather adaptively changes according to the constraints of the latest state. Specifically, during continuous single-constraint state updates, the left boundary of the window remains unchanged, while the right boundary gradually expands with the addition of new states, temporarily increasing the window length. After full-window optimization is triggered by multiple constraints, the window is restored to the preset basic sliding window size through batch edge-mapping. Thus, the sliding window expands elastically in the single-constraint stage and shrinks back after multi-constraint optimization, thereby avoiding unnecessary full-window optimization and edge-mapping operations in the single-constraint stage.

[0029] In this invention, a "single-constraint state" refers to a newly added state node that is directly associated with only one valid measurement factor at the current moment. This measurement factor can be a relative pose factor connecting the previous state and the latest state, or an absolute measurement factor constraining some components of the latest state. This invention does not limit the specific type of the single measurement factor; as long as it can form a valid constraint on the latest state or its related state components, it can be used for the individual update of the latest state in this invention.

[0030] In this invention, "multi-constraint state" refers to a newly added state node that is directly associated with two or more valid measurement factors at the current moment. For example, when the latest state is simultaneously constrained by at least two of the following factors: visual inertial odometry relative pose factor, GNSS position factor, magnetometer orientation factor, or barometer altitude factor, it is considered to satisfy the multi-constraint condition and can trigger full-window optimization.

[0031] The latest state solution under a single constraint state can be implemented in different forms depending on the type of measurement factor, including closed recursion, direct calculation, single-state local optimization, or small-scale numerical solution. This invention does not limit the single measurement constraint equation to a non-iterative solution. Its key difference from traditional fixed sliding window factor graph optimization lies in the fact that this step only solves for the latest state, without re-optimizing all states within the window or performing full window marginalization.

[0032] During the single-constraint state solution process, this invention does not trigger full-window optimization, does not resolve older states within the current window, and does not perform marginalization operations. At this time, the left boundary of the elastic sliding window is maintained. No change, keep the latest state Retain it at the right edge of the window so that the right border of the window expands as the latest state is added.

[0033] Furthermore, if multiple consecutive latest states satisfy the single constraint condition, this step is repeated. The left boundary of the window remains unchanged, while the right boundary of the window gradually expands with the addition of consecutive single constraint states, allowing the current window length to temporarily exceed the preset base sliding window size. This avoids frequent full-window optimization and marginalization during the single-constraint stage, while preserving recent states and their measurement factors so that they can participate in full-window optimization when subsequent multi-source constraints are reformed.

[0034] During the full-window optimization process, the state retained from continuous single-constraint updates, local relative measurement factors, low-frequency global measurement factors, auxiliary measurement factors, and historical prior factors all participate in the optimization within the current elastic window. Therefore, when GNSS, magnetometer, barometer, or other low-frequency measurements arrive and form a multi-constraint state, the system can fully utilize the various previously retained local or global measurement sources to achieve joint estimation under multi-source constraints.

[0035] Batch marginalization can be achieved through Schur complement or equivalent prior construction. The constraint information on the retained states in the marginalized states and their related measurement factors is transformed into new prior factors, thus enabling the transfer of historical information to subsequent windows. Batch marginalization differs from the traditional fixed sliding window method, where each new state marginalizes an older state. This invention only triggers full window optimization under the multi-constraint condition of the most desired state within the sliding window, uniformly performing marginalization on the oldest states, thereby reducing the number of marginalizations and prior factor reconstructions, and lowering window maintenance overhead. After batch marginalization is completed, the left boundary of the elastic sliding window is updated, retaining the most recent states. Each state and its associated factors are considered, and the new prior factors are used as historical constraints for the next round of elastic sliding window optimization.

[0036] After the batch marginalization of this invention is completed, the following two types of information will be retained in the current elastic window: the original measurement factors of the retained state (i.e., the 'related factors') for the next round of motion constraints; and the prior factors generated by the transformation of the marginalized state. These prior factors will replace the discarded old state and apply historical information to the state in the current retained state in the form of prior constraints.

[0037] After solving the latest state individually under a single constraint state, or after completing full-window optimization and batch edge detection under multiple constraints, the fused localization result corresponding to the latest state is output. The output result includes at least position and attitude, and may also include velocity, the number of valid measurement constraints for the latest state, the current window length, the optimization trigger type, and computation time. After outputting the localization result for the current moment, the system returns to step S1 to continue acquiring multi-source asynchronous measurements for the next moment and instantiating a new latest state. Thus, steps S1 to S6 constitute an online cyclic execution process. To avoid the continuous expansion of the elastic window due to prolonged absence of low-frequency measurements, this invention can also set a maximum window length threshold or a maximum consecutive single constraint count threshold. When the current window length exceeds the preset maximum window length, or the number of consecutive single constraint states exceeds the preset threshold, protective full-window optimization and other processing can be triggered to ensure that the system's computational burden remains within a controllable range.

[0038] The present invention will be further described below with reference to the accompanying drawings and specific embodiments. The present invention first acquires multi-source asynchronous measurements (visual odometry / GNSS / magnetism / barometer / wheel speedometer), then instantiates the state within the sliding window and constructs relevant measurement factors (updating the elastic sliding window state and factor graph), counts the number of effective measurement constraints in the latest state, and determines whether the latest state is constrained by only one effective measurement factor. If it is constrained by only one factor, the latest state is solved separately under single-constraint state, keeping the left boundary of the window unchanged and elastically expanding the right side, outputting the current positioning result. If it is not constrained by one factor (i.e., there are two or more constraints), the current elastic window full-window factor graph is optimized under multi-constraint state, then the optimal state is batch marginalized, restored to the preset basic sliding window size, and the current positioning result is output. Then, the process continues in the next moment, returning to the step of acquiring multi-source asynchronous measurements.

[0039] In this embodiment of the invention, the platform is equipped with a camera and an inertial measurement unit (IMU), and operates a visual inertial odometry (VIO) to provide high-frequency local relative pose measurement. Simultaneously, the platform can also receive GNSS position measurements, magnetometer orientation measurements, and barometer altitude measurements. The output frequency of the VIOMA is higher than that of measurement sources such as GNSS, magnetometer, and barometer, thus different measurement factors exhibit significant temporal asynchrony. Multi-source asynchronous measurement refers to different measurement sources providing observation information according to their respective output frequencies, timestamps, and availability states, without requiring all measurement sources to synchronously output observation values ​​at the same time. The system can associate the observation information of different measurement sources with corresponding state nodes based on measurement timestamps, interpolation relationships, nearest neighbor relationships, or preset synchronization strategies. Measurement sources may include visual odometry, visual inertial odometry, GNSS, magnetometer, barometer, wheel speedometer, inertial measurement unit, map matching module, or other measurement sources that can be used for positioning.

[0040] During online operation, the system continuously receives observation information from different measurement sources. The visual inertial odometry outputs local or relative pose measurements at a higher frequency, the GNSS outputs position measurements at a lower frequency, the magnetometer outputs direction-correlation measurements, and the barometer outputs altitude-correlation measurements. The measurement sources do not require synchronous arrival; instead, they are associated with corresponding status nodes based on their respective timestamps and availability.

[0041] In this embodiment, the system uses the visual inertial odometry (VIO) output frequency as the state instantiation reference. Whenever the VIO outputs a new local pose measurement, the system adds a new state node to the right end of the elastic sliding window. Each state node includes at least the position and attitude of the mobile platform. For continuous local poses output by the VIO, the system constructs a relative pose factor between adjacent states. This factor connects adjacent states and provides high-frequency, continuous local motion constraints. When a GNSS measurement arrives, the system converts the GNSS position to the world coordinate system or the local navigation coordinate system and constructs a GNSS position factor. This factor is used to constrain the position components of the corresponding state. When a magnetometer measurement arrives, the system compares the magnetic field direction measured by the magnetometer with the reference magnetic field direction and constructs a magnetometer orientation factor. This factor is used to assist in constraining the orientation information in the state attitude. When a barometer measurement arrives, the system converts the barometer output into an altitude observation and constructs a barometer altitude factor. This factor is used to constrain the vertical position components of the corresponding state. Whenever a new state is added to the elastic sliding window, the system counts the number of effective measurement factors directly associated with the latest state. If the state is constrained only by the visual inertial odometry relative pose factor, it is determined to be under a single constraint condition. If, in addition to the visual inertial odometry relative pose factor, the state is also constrained by at least one of the GNSS position factor, magnetometer orientation factor, barometer altitude factor, or other measurement factors, it is determined to be under a multi-constraint condition.

[0042] For example, at a certain moment, the visual inertial odometry (VIO) outputs local relative pose at a frequency of 10 Hz, while the GNSS, magnetometer, and barometer all output measurements at a frequency of 1 Hz. If the system instantiates states using the VIOF frequency, then between two adjacent low-frequency measurement arrival times, multiple latest states constrained only by the VIOF factor will typically appear consecutively. For these states, if a traditional fixed sliding window method is used, each state arrival may trigger full-window optimization; however, this invention identifies them as single-constraint states and employs a separate update strategy for the latest state.

[0043] When the latest state is constrained only by the visual inertial odometry relative pose factor, the system keeps the older state within the window unchanged and directly recursively derives the latest state using the relative motion of the visual inertial odometry.

[0044] Specifically, let the previous state be... The fusion estimate is known, and its pose is The continuous pose output by the visual inertial odometry in the local coordinate system is and The latest status can be updated in the following format: ; This process does not require full-window nonlinear optimization over the entire sliding window, nor does it require edge detection. The system can output the current positioning result after completing the latest state recursion, thus maintaining high-frequency output.

[0045] When multiple consecutive states are under a single constraint, the system does not marginalize the oldest state, but keeps the left boundary of the window unchanged, and allows the right boundary to expand as a new state is added.

[0046] For example, let the base sliding window size be... After a full window optimization and edge optimization is completed, the window retains... The process involves solving for nine consecutive states, each constrained only by the visual-inertial odometry factor. The present invention then solves for each of these nine latest states individually, while maintaining the left boundary. At this point, the window size can be adjusted from... Temporarily extended to .

[0047] This flexible expansion process preserves multiple types of low-frequency measurements within the sliding window and avoids unnecessary window reconstruction in the single-constraint stage.

[0048] When the next low-frequency measurement, such as a GNSS, magnetometer, or barometer measurement, arrives and forms a measurement constraint with the latest state, the number of effective measurement constraints in the latest state becomes two or more. At this point, the system triggers full-window factor graph optimization of the current elastic window.

[0049] In full-window optimization, all states retained within the current elastic window, local relative pose factors, GNSS position factors, magnetometer orientation factors, barometer altitude factors, and historical prior factors participate in the optimization. The optimizer obtains the optimal estimate of each state within the current window by minimizing the weighted sum of squares of all residual terms. The full-window optimization can employ the Gauss-Newton method, the Levenberg-Marquardt method, the Dogeg method, or other nonlinear least squares solution methods.

[0050] Since these states were not marginalized in advance during the previous continuous single-constraint phase, when low-frequency measurements arrive, the system can simultaneously utilize local continuous motion constraints and low-frequency global or auxiliary constraints within the same window, thereby forming a complete multi-source fusion optimization.

[0051] After the full window optimization is completed, the system performs batch edge-mapping on the oldest states based on the difference between the current window size and the base window size.

[0052] For example, if the base window size is The current elastic window size is Then, after full-window optimization, the nine oldest states are marginalized all at once, and new prior factors are generated using Shure complement. After marginalization, the states within the window are retained again. There are several states.

[0053] By employing the above methods, the system avoids frequent marginalization during the single-constraint phase and concentrates the marginalization operations on execution after the multi-source full-window optimization is truly completed, thereby reducing computational redundancy and prior reconstruction overhead.

[0054] After each single-constraint latest state update, the system outputs the pose estimate corresponding to the latest state; after each multi-constraint full-window optimization, the system outputs the optimized latest state estimate and can simultaneously update the historical estimates of other states within the window.

[0055] The output information may include the current position, attitude, timestamp, current window length, number of valid measurement constraints in the latest state, optimization type flag, and computation time. The optimization type flag distinguishes whether the current result was obtained by updating the latest state under a single constraint or by full-window optimization triggered by multiple constraint states.

[0056] After completing the processing of the latest state, the system continues to receive multi-source asynchronous measurements at the next moment and repeatedly executes the processes of latest state instantiation, constraint number determination, single constraint update or multi-constraint optimization, marginalization, and result output.

[0057] In some scenarios, GNSS, magnetometers, barometers, or other low-frequency auxiliary measurements may be unavailable for extended periods, resulting in a large number of consecutive single-constraint states. To prevent the flexible window from becoming too large, this embodiment allows setting a maximum window length threshold. When the current window length exceeds this threshold, the system can trigger protective full-window optimization processing.

[0058] Protective full-window optimization can be performed when computational resources allow, reducing computational peaks during the next multi-constraint optimization. This protective mechanism keeps the system's computational burden manageable even when the latest state is re-constrained by multiple measurements and full-window optimization is performed, even with long-term low-frequency measurement gaps.

[0059] The core of this invention lies in an elastic sliding window optimization mechanism based on the number of effective measurement constraints in the latest state, without limiting the specific type of measurement source, the specific front-end odometer calculation method, or the specific optimizer implementation. Visual inertial odometry, visual odometry, laser odometry, wheel speed odometry, GNSS, magnetometers, barometers, map matching results, and other measurements that can be represented as factor graph residuals can all be applied to this invention.

[0060] The above embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit it. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some or all of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A flexible sliding window factor graph optimization method for asynchronous multi-source measurement fusion positioning, characterized in that, include: S1. Obtain asynchronous observation information from different measurement sources, generate state nodes using the preset output frequency of the high-frequency local measurement source, associate the asynchronous observation information from different measurement sources with the corresponding state nodes, and construct a state set within an elastic sliding window; construct the measurement factors in the factor graph from the asynchronous observation information associated with the state nodes. S2. Count the number of valid measurement factors directly associated with the latest state. Based on the number of valid measurement factors in the latest state, divide it into single-constraint state and multi-constraint state. If it is a single-constraint state, execute S3; if it is a multi-constraint state, execute S4. S3. For the latest state of a single-constraint state, construct the latest state optimization objective function based on the effective measurement factors and solve it to obtain the optimal estimate of the latest state. During the solution of the single-constraint state, keep the other states of the elastic sliding window unchanged and retain the latest state at the right end of the elastic sliding window. The right boundary of the elastic sliding window expands as the latest state is added. S4. For the latest state of the multi-constraint state, construct the full-window optimization objective function based on the set of all effective measurement factors in the current elastic sliding window and solve it to obtain the optimal estimate of all states in the current window. S5. Set a base sliding window. If the current elastic sliding window is larger than the base sliding window, perform batch edge-trimming on the oldest state that exceeds the limit. S6. After step S3 is completed, output the optimal estimate of the latest state and keep the estimated values ​​of the remaining states in the sliding window unchanged. After steps S4 and S5 are completed, the optimal estimate of the latest state is output, and the optimal estimates of the remaining states within the current elastic sliding window are updated synchronously. After obtaining the optimal estimate, return to step S1 to continue execution.

2. The elastic sliding window factor graph optimization method for asynchronous multi-source measurement fusion positioning according to claim 1, characterized in that, S1 includes S1.1, where the high-frequency local measurement source is the measurement source with the highest output frequency specified during initialization; Let the state of the latest state node be... The set of states within the current elastic sliding window for: ； In the formula, This is the time step index for the current left boundary state of the elastic sliding window. The time step index for the right boundary state; Each state includes at least the position and orientation of the mobile platform: ； In the formula, For mobile platforms in the world coordinate system The lower position, For mobile platform slave system To the world coordinate system The posture.

3. The elastic sliding window factor graph optimization method for asynchronous multi-source measurement fusion positioning according to claim 2, characterized in that, S1 includes S1.2, and each measurement factor corresponds to a residual term: ； In the formula, In order to be with the first The time step associated with the first One measurement information, This is the residual function corresponding to the measurement information.

4. The elastic sliding window factor graph optimization method for asynchronous multi-source measurement fusion positioning according to claim 3, characterized in that, S2 includes statistics. Number of directly related effective measurement factors If multiple measurement factors originate from the same measurement source and have the same type of observation information, they are merged into one category of valid measurement factors; if the timestamp of a measurement factor exceeds the maximum allowable time difference with the state node, it is determined to be an abnormal observation quality and is not included in the number of valid measurement factors. when At that time, the judgment It is a single-constraint state; when At that time, the judgment This is a multi-constraint state.

5. The elastic sliding window factor graph optimization method for asynchronous multi-source measurement fusion positioning according to claim 4, characterized in that, S3 includes, and The only effective measurement factor that is directly related is The corresponding measurement is Construct the latest state optimization objective function: ； In the formula, This is the optimal estimate for the latest state. This is an estimate of the existing states in the current window, excluding the latest state.

6. The elastic sliding window factor graph optimization method for asynchronous multi-source measurement fusion positioning according to claim 5, characterized in that, S4 includes a full-window optimization objective function: ； In the formula, To optimize for the entire window, The a priori residuals formed by historically marginalized information, It is the set of all valid measurement factors within the current elastic sliding window. This is the information matrix corresponding to the measurement factors. The Mahalanobis distance; The optimal state estimate is obtained by solving the full-window optimization objective using the nonlinear least squares method. .

7. The elastic sliding window factor graph optimization method for asynchronous multi-source measurement fusion positioning according to claim 6, characterized in that, S5 includes, S5.1, setting the basic sliding window size as... The number of states in the current elastic sliding window is , ; when At that time, the oldest prior art was constructed using the Schul complement or equivalent prior art method. Batch edge-divide each state; when At this time, edgeware is not performed, and the current window remains unchanged.

8. The elastic sliding window factor graph optimization method for asynchronous multi-source measurement fusion positioning according to claim 7, characterized in that, S5 includes S5.2, in factor graph optimization. In the process, the variable block corresponding to the marginalized state is eliminated, and the constraint information is passed to the prior factors of the retained state through the Schur complement; After completing batch edge detection, update the left boundary of the elastic sliding window, retaining the most recent edge detection. Each state and its corresponding measurement factor will be used as a historical constraint for the next round of elastic sliding window optimization, and the window size will be restored to the basic window size. .

Images