Multi-sensor fusion positioning method and system based on factor graph optimization in low-light environment

Abstract

The invention discloses a multi-sensor fusion positioning method and system based on factor graph optimization in a low-light environment, and relates to the field of automatic driving and robot navigation positioning. The method solves the problem of how to realize efficient and accurate fusion of laser radar and visual data under a dark light condition in the prior art, i.e., improve the depth estimation precision of a system, and comprises the following steps: constructing a visual inertial odometer and a laser radar inertial odometer, extracting key feature points and corresponding descriptors from a continuous image stream, and constructing a visual inertial odometer and a laser radar inertial odometer; combining the high-frequency attitude prior information with visual feature matching and laser radar point cloud matching to realize high-precision state estimation of a visual inertial odometer and a laser radar inertial odometer; the laser radar point cloud is projected to a visual key frame, motion distortion is corrected, and robust depth correlation is carried out by adopting a plane inspection method; a joint optimization strategy based on a factor graph is adopted, data of the speedometers are modeled in a unified mode, and global state optimization is achieved through a nonlinear optimization method.

Description

Technical Field

[0001] This invention relates to the fields of autonomous driving and robot navigation and positioning technology, specifically to a multi-sensor fusion positioning method and system based on factor graph optimization in low-light environments. Background Technology

[0002] With the rapid development of autonomous driving and robot navigation technologies, multi-sensor fusion navigation systems have been widely used, with combinations of LiDAR, cameras, and inertial measurement units being particularly common. However, in low-light environments, the image quality acquired by the camera deteriorates significantly, leading to difficulties in visual feature extraction, inaccurate matching, and increased depth estimation errors, which in turn significantly reduces the positioning accuracy and stability of the entire navigation system. To address this issue, this paper proposes a high-precision, highly robust multi-sensor fusion navigation method suitable for low-light environments.

[0003] Autonomous driving technology, as a cutting-edge field deeply integrating artificial intelligence and the automotive industry, is leading a transformation in future mobility and has attracted significant attention from global industry and academia. In recent years, international companies such as Tesla and Waymo have achieved remarkable technological breakthroughs in autonomous driving, while domestic technology companies like Huawei, Xiaomi, and Baidu have also actively entered the field, launching commercially viable autonomous vehicles and services. However, achieving a truly safe, efficient, and reliable autonomous driving system still faces numerous technological bottlenecks, among which accurate positioning and stable navigation capabilities are particularly crucial.

[0004] While mainstream Global Navigation Satellite Systems (GNSS) offer high-precision positioning capabilities in open areas, their application is limited in complex terrains such as urban canyons, tunnels, and underground parking lots. Severe multipath effects and signal blockage significantly reduce their positioning performance, making it difficult to meet the high-precision requirements of autonomous driving systems. Therefore, effectively fusing information from multiple sensors, including cameras, inertial measurement units (IMUs), and lidar, to achieve stable and reliable multi-source fusion navigation and positioning has become a crucial technical challenge urgently needing to be addressed in the field of autonomous driving. In low-light or dim-light conditions such as at night, in tunnels, and underground parking garages, the quality of images acquired by visual sensors deteriorates significantly. Image details are severely lost, noise increases significantly, and image blurring intensifies. This greatly increases the difficulty of visual feature extraction and matching, directly reduces the accuracy and stability of multi-source fusion navigation systems, and severely restricts the practical application capability of autonomous driving technology in complex dim-light environments.

[0005] To improve the performance of visual sensors in low-light environments, academia and industry have conducted extensive research, developing several typical image enhancement algorithms such as histogram equalization, gamma correction, and multi-scale Retinex. While these methods can improve image brightness and contrast to some extent, they also introduce significant noise amplification and detail loss. Achieving an ideal balance between image sharpness and realism is difficult, and their adaptability to complex dynamic scenes is poor, limiting their practicality.

[0006] In recent years, deep learning-based image enhancement techniques have achieved good results, but they still have significant shortcomings in low-light conditions. Convolutional Neural Networks (CNNs) face difficulties in balancing capturing global information and enhancing details in images. While existing Transformer-based algorithms can capture the global context of images well, their computational cost is high, making it difficult to meet the real-time requirements of autonomous driving. Therefore, developing computationally efficient image enhancement algorithms suitable for low-light conditions has become a major research hotspot in the field of autonomous driving.

[0007] In low-light conditions, visual sensors struggle to accurately estimate environmental depth information, while lidar, with its high accuracy and immunity to lighting conditions, serves as an effective auxiliary tool for visual sensors. However, when visual image quality degrades significantly, the accurate fusion of visual and lidar data remains a major challenge. Existing fusion techniques often suffer from low fusion efficiency, high computational complexity, and high mismatch rates when dealing with environments with sparse features and limited texture. Therefore, achieving efficient and accurate fusion of lidar and visual data to significantly improve the system's depth estimation accuracy is a pressing technical challenge that needs to be addressed.

[0008] Despite significant progress in multi-source fusion technology, numerous challenges remain. Tightly coupled methods rely on high-frequency joint optimization, resulting in high computational complexity and making real-time operation difficult in low-computing-power embedded devices. Loosely coupled schemes, while simple in structure, struggle to handle extreme scenarios involving sensor depth failure. Furthermore, existing technologies are largely based on high-cost solid-state LiDAR and high-resolution cameras, limiting their application in consumer robots. Summary of the Invention

[0009] To overcome the problems in the prior art of how to achieve efficient and accurate fusion of lidar and visual data under low light conditions, thereby significantly improving the depth estimation accuracy of the system and the shortcomings of single sensors in terms of positioning accuracy and robustness, this invention proposes a multi-sensor fusion positioning method and system based on factor graph optimization in low light environment.

[0010] To solve the above-mentioned technical problems, the present invention is achieved through the following technical solution: Option 1: This invention proposes a multi-sensor fusion localization method based on factor map optimization under low-light conditions. The method includes the following steps: Step 1: Construct visual inertial odometry and lidar inertial odometry, and extract key feature points and their corresponding descriptors from continuous image streams. Utilize the high-frequency attitude prior information provided by the inertial navigation system, and combine it with visual feature matching and lidar point cloud matching respectively to achieve high-precision state estimation of visual inertial odometry and lidar inertial odometry. Step 2: Project the LiDAR point cloud onto the visual keyframe and correct motion distortion. Use the planar inspection method to perform robust depth association. Step 3: Adopt a joint optimization strategy based on factor graphs to unify the data from inertial odometry, visual odometry and lidar odometry into a unified model, and achieve global state optimization through nonlinear optimization algorithms.

[0011] Furthermore, a preferred embodiment is provided, in which step 1 further includes constructing an IMU pre-integration model and an error propagation model to calculate the relative IMU measurements between two historical keyframes and convert them into constraint edges, i.e., processing noise and zero bias in the IMU measurements.

[0012] Furthermore, a preferred embodiment is provided, wherein the method for extracting key feature points and their corresponding descriptors from a continuous image stream in step 1 is as follows: corner points in the continuous image are detected using the FAST algorithm, and inter-frame tracking of the continuous image is achieved using the Lucas-Kanade algorithm in the sparse optical flow method.

[0013] Furthermore, a preferred embodiment is provided in which the RANSAC algorithm is introduced into the matching results of inter-frame tracking of continuous images in the Lucas-Kanade algorithm to remove erroneous matches.

[0014] Furthermore, a preferred embodiment is provided, wherein the method for robust depth association using the planar inspection method in step 2 is as follows:

[0015] Where n is the normal vector of the plane.

[0016] Furthermore, a preferred embodiment is provided, wherein the factor graph structure in step 3 includes an IMU pre-integration factor, used to constrain motion changes caused by IMU integration; a laser odometry factor, used to constrain feature matching between laser frames and map keyframes; a visual odometry factor, used to constrain relative motion between visual image frames; and a loop closure detection factor, used to construct closed-loop constraints to ensure spatial consistency between the current frame and previous candidate keyframes.

[0017] Furthermore, a preferred embodiment is provided, wherein the method for achieving global state optimization through a nonlinear optimization algorithm in step 3 is as follows:

[0018] in, and Let these represent the edge sets of IMU, visual odometry, LiDAR, and closed-loop constraints, respectively. It is a residual; ,in, It is based on the relative pose measured by sensors. and These are the poses of two keyframes; the poses are: ,in: yes The rotation matrix represents the rotation from the world coordinate system to the sensor coordinate system. yes The translation vector is the system's position coordinates.

[0019] Option 2: A multi-sensor fusion positioning system based on factor map optimization under low-light conditions, the system comprising: The state estimation module is used to construct visual inertial odometry and lidar inertial odometry, and extract key feature points and their corresponding descriptors from continuous image streams. It utilizes the high-frequency attitude prior information provided by inertial navigation system, and combines it with visual feature matching and lidar point cloud matching to achieve high-precision state estimation of visual inertial odometry and lidar inertial odometry. The correction module is used to project the LiDAR point cloud onto the visual keyframe and correct motion distortion, and uses a planar inspection method for robust depth association. The optimization module is used to adopt a joint optimization strategy based on factor graphs to unify the modeling of data from inertial odometry, visual odometry, and lidar odometry, and to achieve global state optimization through nonlinear optimization algorithms.

[0020] Option 3: A computer-readable storage medium storing a computer program that, when executed by a processor, implements the steps of the method described in Option 1.

[0021] Option 4: A computer device, including a memory and a processor, wherein the memory stores a computer program, and when the processor runs the computer program stored in the memory, the processor executes the method described in Option 1.

[0022] The advantages of this invention are: The multi-sensor fusion localization method based on factor graph optimization in low-light environments described in this invention constructs a multi-sensor fusion framework based on factor graph optimization. Through collaborative optimization of IMU, vision, and LiDAR data, it significantly improves the localization accuracy and robustness of the system under low-light conditions. Effective collaboration between modules not only reduces accumulated errors but also improves the accuracy of depth estimation. Overall, the method described in this invention provides practical technical support for the application of autonomous driving and robot navigation systems in complex environments. Attached Figure Description

[0023] Figure 1 This is a schematic diagram of FAST feature points as described in Implementation Method 1.

[0024] Figure 2 This is a schematic diagram of the new tracking feature points described in Implementation Method 1.

[0025] Figure 3 This is a schematic diagram of the feature points for stable tracking as described in Implementation Method 1.

[0026] Figure 4 This is a schematic diagram of LK optical flow tracing as described in Implementation Method 1.

[0027] Figure 5 This is a schematic diagram illustrating the estimation of motion at six different moments using the LK optical flow method described in Implementation Method 1.

[0028] Wherein, (a) is a schematic diagram of the first optical flow tracing moment, (b) is a schematic diagram of the second optical flow tracing moment, (c) is a schematic diagram of the third optical flow tracing moment, (d) is a schematic diagram of the fourth optical flow tracing moment, (e) is a schematic diagram of the fifth optical flow tracing moment, and (f) is a schematic diagram of the sixth optical flow tracing moment.

[0029] Figure 6 This is a schematic diagram of mismatched points that do not conform to the actual motion model as described in Implementation Method 1.

[0030] Figure 7 The diagram illustrates an error matching problem in the conventional method described in Implementation Method 1.

[0031] Figure 8 This is a schematic diagram illustrating the RANSAC algorithm for removing incorrect matches as described in Implementation Method 1.

[0032] Figure 9 This is a schematic diagram illustrating the constraint relationships between different factor nodes as described in Implementation Method 1.

[0033] (a) is a schematic diagram of the map prior factor structure, (b) is a schematic diagram of the odometry factor structure, and (c) is a schematic diagram of the IM factor structure.

[0034] Figure 10This is a schematic diagram of the edge-setting process described in Implementation Method 1.

[0035] Figure 11 This is a schematic diagram of the LiDAR described in Implementation Method 1, showing a stationary state without distortion.

[0036] Figure 12 This is a schematic diagram illustrating the motion distortion of the lidar as described in Embodiment 1.

[0037] Figure 13 This is a schematic diagram of point cloud distortion removal as described in Implementation Method 1.

[0038] Figure 14 This is a schematic diagram of the edge points and planar points described in Embodiment 1.

[0039] Figure 15 This is a schematic diagram of the point cloud feature matching principle described in Implementation Method 1.

[0040] (a) is a schematic diagram of edge point matching, and (b) is a schematic diagram of planar point matching.

[0041] Figure 16 This is a schematic diagram of the sampling time of the visual keyframes of the IMU radar point cloud as described in Implementation Method 1.

[0042] Figure 17 This is a schematic diagram of the depth association of visual features as described in Implementation Method 1.

[0043] Figure 18 This is the factor diagram of the navigation system described in Implementation Method 1.

[0044] Figure 19 The satellite map of the experimental trajectory described in Implementation Method 1.

[0045] Figure 20 The image shows the multi-source fusion normal illumination positioning trajectory diagram and a magnified view as described in Implementation Method 1.

[0046] Among them, (a) is the positioning trajectory diagram, and (b) is a magnified view of a part.

[0047] Figure 21 This is the APE index chart described in Implementation Method 1. Detailed Implementation

[0048] To make the objectives, technical solutions, and advantages of the embodiments of this application clearer, the technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of this application, and not all of them.

[0049] Implementation Method 1: This implementation method proposes a multi-sensor fusion localization method based on factor graph optimization in low-light environments. First, it introduces the establishment of IMU pre-integration and error propagation models, and then elaborates on key feature extraction, optical flow tracing, and nonlinear optimization methods in visual inertial odometry. Simultaneously, for LiDAR data, point cloud distortion correction, feature extraction, and scan matching strategies are proposed, and how to utilize LiDAR data to enhance visual depth estimation is discussed. The factor graph-based backend optimization method is systematically applied in this chapter, thereby achieving efficient fusion of multi-source information and improving overall system performance.

[0050] I. IMU Pre-integration The IMU output frequency is higher than that of LiDAR and cameras. To integrate its data into the graph optimization framework, the relative IMU measurements between two keyframes need to be calculated and converted into constraint edges. This is achieved through IMU pre-integration, which can handle noise and zero bias in the IMU measurements. In local scenes, the navigation system can treat the northeast-sky coordinate system as an inertial frame, where the gravity vector is constant.

[0051] 1. The IMU pre-integration model utilizes Euler integrals to obtain the IMU's motion equations, which are then discretized in the following manner:

[0052] In the derivation process, to ensure the simplicity of the symbols, we can simplify it as follows:

[0053] Assuming the sampling time is fixed, substituting this observation equation into the discrete equation of motion, the equation can be expressed as:

[0054] By accumulating the IMU observations for keyframes at time i and j, assuming that the gyroscope and accelerometer biases remain unchanged during this period, we can obtain:

[0055] in So you don't have to start every time. Each update requires recalculating the integral. The pre-integration term between two keyframes is defined as follows:

[0056] 2. Error Propagation Model Under ideal conditions, the pre-integration process yields a noise-free theoretical value; however, in actual sensor measurements, the obtained pre-integration results inevitably contain various noise components. These noises not only originate from the Gaussian white noise of the current time step but also include errors propagated from the state of the previous time step. Therefore, it is necessary to model and analyze the noise propagation mechanism in the pre-integration process in order to effectively separate it from the observation values.

[0057] First, let's analyze the rotation increment. :

[0058] The measured value is

[0059] make Then the noise term is:

[0060] In this case, the rotational pre-integral can be expressed as a combination of the ideal rotational increment and the error term, i.e., the measured value equals the true value plus the disturbance term:

[0061] Analysis speed increment ,Right now

[0062] Similarly, the velocity increment can also be decomposed into a form that includes measurement error, with its observations consisting of the ideal value and superimposed noise:

[0063] Finally, the position increment is analyzed. We can obtain:

[0064] Finally, in order to accurately describe the cumulative propagation behavior of noise over time, the recursive relationship of the corresponding pre-integrated noise covariance can be derived, and its expression is shown below:

[0065] II. Visual Inertial Odometry 1. Feature point extraction In the front-end processing of a visual odometry system, the primary task is to extract key feature points and their corresponding descriptors from a continuous image stream, and then estimate the relative motion between adjacent frames through feature matching between images.

[0066] The Lucas-Kanade (LK) method within sparse optical flow implements feature extraction and tracking. Specifically, it utilizes the FAST algorithm to detect corner points in the image and then employs sparse optical flow for inter-frame tracking. Compared to indirect methods, optical flow-based tracking avoids descriptor computation and matching operations, significantly improving processing speed.

[0067] The FAST algorithm detects corner points in images by comparing brightness. Its basic principle is: select a pixel in the image to be detected. Take 16 pixels on a circle with a radius of 3 centered on the original pixel, and compare the grayscale values ​​of these pixels. If the 16 pixels are consecutive... Each pixel has a brightness value greater than [number]. or both are less than When this pixel is a feature point, it is commonly set to... Every pixel in the entire image is judged according to this criterion to complete the corner detection.

[0068] To prevent feature points from becoming too concentrated in space, a non-maximum suppression strategy is employed during detection, retaining only the feature points with the strongest response within a certain neighborhood to achieve a balanced distribution. During feature point tracking, to further ensure spatial uniformity, a mask region with a radius of 20 pixels is set around each successfully tracked corner point to conceal weaker features in its neighboring region, preventing excessive density of local feature points.

[0069] As image frames are continuously updated, some feature points may be lost due to exceeding boundaries or tracking failure. To address this, the system checks whether the number of successfully tracked corner points in the current frame meets a threshold after each optical flow tracking step. If the threshold is not met, corner points are re-extracted in the current frame and added to the tracking queue, while ensuring that feature points are evenly distributed. Figure 2 and Figure 3 The tracking effect is shown, where the red marks are the newly added corner points in the current frame, and the green marks are the feature points that are stably tracked across multiple frames.

[0070] 2. LK optical flow tracing After matching feature points between two adjacent frames, the system can estimate the essential matrix using multiple matching point pairs, thereby further deriving the relative pose change between the two frames. To improve computational efficiency while maintaining tracking accuracy, this paper employs the LK method for inter-frame tracking of the extracted feature points, such as... Figure 4 As shown.

[0071] The LK optical flow method is a gray-level invariant method that estimates the motion of a photograph by analyzing local changes in continuous images.

[0072] Under this assumption, the pixel brightness values ​​in the image satisfy the following expression:

[0073] Based on this, performing a Taylor expansion on the left side of the equation and linearizing it once, we obtain the following:

[0074] Based on the assumption of gray-level invariance, we can obtain:

[0075] Relating the above equation to time Differentiating, we get:

[0076] in, These represent the points at... Velocity in the direction; The images are respectively in Place The gradient in direction. It can be represented in matrix form:

[0077] At any given time, assume that the pixels in the neighborhood of this feature point have consistent motion parameters. Within the neighborhood There are 100 pixels. Therefore:

[0078] The above equation can be expressed as an overdetermined equation. By using least squares calculation, the estimation between two frames of a pixel block can be obtained, and the motion state of the camera can be calculated. Figure 5 The image shows the pixel state estimation using the L-K optical flow method, with six different time points represented.

[0079] In practical feature matching, factors such as noise, lighting variations, occlusion, and mismatches often introduce mismatches that do not conform to the actual motion model. To obtain more reliable matching results, these mismatches need to be removed. Random Sample Consensus (RANSAC) is a model estimation method that can accurately estimate the model parameters from massive datasets containing outliers while simultaneously removing inconsistencies.

[0080] The RANSAC algorithm is based on the following assumptions: a large proportion of inliers exist in the dataset, satisfying a correct model, while outliers, due to noise or mismatches, do not conform to the model. The algorithm estimates model parameters using a minimum sample set (MSS) through random sampling, then calculates the residuals between all data points and the model to determine which points are inliers. This sampling and evaluation process is repeated multiple times, ultimately selecting the model with the most inliers as the final estimate, and using these inliers to refine the model re-estimation. Assume the current set of matching points is:

[0081] in and These represent the coordinates of the matching points in the first and second frames, respectively. Our goal is to estimate a geometric transformation model. This satisfies the following for the vast majority of interior points.

[0082] The specific steps of the RANSAC algorithm in LK optical flow error removal are as follows: From the set of matching points A minimum sample size (MSS) is randomly selected from the dataset, depending on the chosen model. Model parameters are then calculated using this randomly selected minimum sample set. For example, for a two-dimensional translation model, the translation vector can be calculated directly; for an affine model or homography matrix, the corresponding linear algorithm is used to solve it.

[0083] For each matching point in the matching point set Using the model Calculate the predicted location:

[0084] The reprojection error is defined as:

[0085] if (in If the set of interior points is a preset error threshold, then the matched point is considered an interior point. The set of interior points is denoted as […].

[0086] Statistical analysis of the current model Corresponding interior point number And record the current model as the best model (if its number of interior points exceeds the previous record).

[0087] Repeat the above steps until the predetermined maximum number of iterations is reached, or until a pre-defined termination condition is met. This process continues until the maximum set of interior points is finally determined. Then, use all interior points to model A reassessment was performed to obtain a more accurate motion model. .

[0088] The effectiveness of the RANSAC algorithm depends on a reasonable threshold. and number of iterations The choice. Let the proportion of points in the data that conform to the model, i.e., the proportion of interior points, be . The minimum sample set size is In one iteration, the probability of selecting all interior points is... To ensure probability To obtain a sample set of all interior points at least once, the required number of iterations should satisfy the following:

[0089] Therefore, we can conclude that:

[0090] In practical applications, based on matching quality estimation Value, and set appropriate This determines the number of sufficient iterations. .

[0091] By introducing the RANSAC algorithm into the matching results of LK optical flow tracking, erroneous matches caused by noise, local distortion, or occlusion can be effectively eliminated. The inlier set filtered by RANSAC not only improves the estimation accuracy of the motion model but also provides a more reliable data foundation for subsequent visual motion estimation. Combining the local tracking advantages of LK optical flow with the robustness of RANSAC, their synergistic effect significantly enhances the system's robustness and accuracy in complex scenes.

[0092] III. Nonlinear Optimization Extending the reprojection error form of an image to three-dimensional space allows for a clearer characterization of the geometric errors of feature points in the camera coordinate system.

[0093] The transformation matrix satisfies Then the above formula simplifies to:

[0094] Based on the above formula, the observation error residual term between each pair of matched feature points can be obtained. This residual reflects the difference between the actual observed position of the feature point in the image and the predicted position estimated by the current state.

[0095]

[0096] in, Represents the back projection function; and It is an orthogonal basis for the tangent plane; This represents the reprojection estimation model. After simplifying the above observation error expression, it can be summarized into a unified constraint form for the visual residual term:

[0097] Based on this, the visual error term can be incorporated into the overall optimization objective function, thereby constructing a visual-inertial tightly coupled optimization model, which provides constraint support for subsequent state estimation.

[0098]

[0099] IV. Marginalization During the operation of a multi-source navigation system, as the external environment continuously generates new measurement information, the optimization graph grows continuously, and the number of state variables to be estimated and constraints gradually increases, leading to a significant increase in the overall optimization computational cost. To address this issue, a sliding window mechanism is typically introduced in practical systems, which retains and optimizes only the state variables within the recent time frame, thereby controlling the scale of the optimization problem.

[0100] When a new frame of data enters the system, the corresponding factors are added to the graphical model, while the oldest frame needs to be removed from the window. Since these older frames are associated with the state variables in the current window, directly discarding them would result in the loss of existing information. To avoid this problem, the system needs to use a marginalization method to transform the impact of the removed variables on the current window state into a priori form and retain it, thereby conveying the influence of historical information on the current estimate without increasing the size of the graphical model.

[0101] The sliding window graph optimization model integrates multiple information sources, and its corresponding factor nodes and residual term structures are as follows: Map prior factors: represent map matching errors and impose constraints on pose variables for only one frame; Laser and visual odometry factors: reflect the relative pose error between adjacent frames, with each factor connecting the poses of two adjacent frames; IMU pre-integration factors: used to model the inertial constraints between adjacent frames, connecting not only the poses at two times but also containing the corresponding IMU zero bias information at the two times. Figure 9 The connection structure of the different types of factors in the graph model is shown, and the roles and constraints of various types of observation information in the graph optimization system are clarified.

[0102] During the optimization process, the solution of the state variables can be expressed in the general form of the following linear equation: ; To simplify the solution, the state variables to be marginalized can be removed, and the matrix H and vector b can be partitioned into blocks as follows:

[0103] In the formula, This indicates the amount to be marginalized. Indicates the amount to be retained. , , , Indicates from The decomposition of the matrix yields the following results: and The corresponding parts, and Indicates from The disassembly obtained from the middle and the respectively and The corresponding part.

[0104] To triangulate the above equations, matrix transformations can be performed using the Schul complement formula:

[0105] An equivalent form can be obtained further:

[0106] Taking the equation in the second row above, we get:

[0107] It can be divided into two parts when in use:

[0108] As can be seen from the above derivation, in the optimization process, we do not need to explicitly solve for the estimates of the marginalized variables. In other words, we can directly... A new marginalization factor is generated by removing elements from the graphical model and introducing equivalent prior information through a marginalization operation. This factor is constructed during the first marginalization operation and persists thereafter, continuously updating in subsequent marginalization steps to reflect changes in the current model state.

[0109] After constructing the marginalization factors, old frames and their associated information can be safely deleted from the graph. Simultaneously, the system adds newly arriving frames and their measurement information to the graph as factor nodes, thus achieving dynamic updates to the graph structure. This process is repeatedly performed along with the sliding window, ensuring that the optimization window length remains constant. The entire marginalization process is as follows: Figure 10 As shown.

[0110] V. LiDAR Inertial Odometry 1. Point cloud distortion removal Point cloud distortion is caused by motion during data acquisition by the lidar. When the carrier moves at high speed, multiple points in the same frame of the point cloud are not acquired synchronously, resulting in geometric distortion. In static scenes, such as when the lidar is fixed and there are no moving objects around, the acquired point cloud data will not exhibit distortion.

[0111] When the system is in a dynamic environment, such as when the radar platform is moving or when there are moving targets in the scene, the resulting point cloud will be distorted. For example... Figure 12 As shown, when the lidar moves forward, objects in the scene appear to recede relative to the lidar. The same color is used in the figure to represent the beams formed by the same laser line during scanning. Because the start and end positions of a lidar scan frame are not the same, this causes stretching and distortion of the point cloud in three-dimensional space.

[0112] This distortion causes points in the same frame of point cloud data to be scattered across different lidar coordinate systems. To restore the true geometric shape, spatiotemporal correction of the point cloud is necessary. The method is to back-project each point onto the lidar coordinate system at the time of the first scan point in that frame, unifying all points to the same coordinate reference system, thereby eliminating motion distortion.

[0113] To correct distortion in points acquired at different times in a point cloud, the pose change of each point in a unified time reference frame can be estimated using interpolation, and then back-projected onto the coordinate system corresponding to the starting point of the frame. The formula provides a method for calculating pose transformation based on linear interpolation: ; in, This indicates the first point in the point cloud of that frame. The data collection time for each point; This represents the pose transformation of the point relative to the first point in the current frame. and These are the IMU pose changes corresponding to the two most recent moments; and These are the IMU measurement timestamps at the two specific moments. Using the above formula, the pose transformation of each point can be estimated to a unified starting reference time, and back-projection can be performed, thereby correcting the motion distortion of the point cloud.

[0114] 2. Point cloud feature extraction LiDAR possesses high ranging accuracy and a wide detection field of view, thus extracting key feature points from point clouds helps achieve high-precision matching and positioning. During point cloud feature extraction, feature points can be broadly categorized into two types: edge feature points and planar feature points. Edge features typically appear at the structural edges of object surfaces, while planar features are distributed in areas with continuity and flatness.

[0115] To effectively classify point clouds, the magnitude of local curvature can be used to determine whether a point belongs to an edge or a planar feature. The curvature is calculated based on the distribution of other points in the point's neighborhood, and its expression is as follows:

[0116] in, Indicating the first point cloud The spatial coordinates of a point in the radar coordinate system; set It is a set of points that are spatially adjacent to that point; This represents the local curvature value at that point. If the curvature... A smaller value indicates that the distribution of points in the neighborhood of that point is close to a plane, and that the point is more likely to belong to a planar feature; conversely, if... A large threshold indicates a significant geometric abrupt change around the point, which may be an edge feature. By setting an appropriate threshold, edge points and planar points in the point cloud can be distinguished.

[0117] Edge feature points and planar feature points are shown in the figure as follows Figure 14 As shown, this illustrates the distribution of different types of features in the point cloud structure.

[0118] 3. Scan Matching After extracting point cloud features for each frame, the next step is to estimate the pose of the current frame in the local map based on the spatial geometric constraints between the point clouds of each frame. This process is called feature matching, which essentially establishes a spatial correspondence between the point cloud features of the current frame and the prior map. The matching method adopted in this paper is a scan-to-map strategy, which matches the point cloud of the current frame to the local map constructed from historical keyframes.

[0119] Let the extracted feature point clouds of the current frame be the edge features. and planar features The corresponding historical features in the local map constitute two types of map point clouds. and The initial pose estimate for the current frame is set to... The goal of feature matching is to continuously optimize through iteration. This allows the features of the current frame to be aligned with the features in the map after transformation, thereby achieving high-precision registration.

[0120] For edge feature points, it is necessary to extract each edge point in the current frame. Mapped to the map coordinate system and on the edge map Find the point in the interval that has the closest Euclidean distance to it, and denote it as . Next, within the laser beam at that point, select two adjacent points before and after it. and A line segment is fitted using these three points, serving as the geometric constraint for that edge point. In the diagram, blue and orange lines represent the corresponding laser line segments. The perpendicular distance to its corresponding line can be expressed as:

[0121] Accordingly, in planar feature point matching, for each planar point First, transform it to a map coordinate system, and then apply it to the point cloud on the map plane. Find the nearest point Subsequently, two adjacent points were selected from the laser line segment at that point. and Three points are coplanar and form a reference plane. The perpendicular distance to this plane is calculated as follows:

[0122] After calculating the distances from points to lines and points to surfaces, all residual terms can be unified into a single objective function for a nonlinear optimization problem, which can then be used to solve and optimize the pose of the current frame. The overall optimization objective function is as follows:

[0123] The objective function integrates the geometric errors of edge points and planar points into the same optimization framework. By iteratively calculating through a nonlinear solver, the optimal pose estimate for the current frame can be obtained.

[0124] 5. LiDAR enhances visual depth 1. Point cloud projection Because of the non-repeating mode of LiDAR, dense point clouds can be obtained. However, for LiDAR, each LiDAR point is sampled at different times, which may cause motion distortion and reduce the accuracy of depth association. Furthermore, to correlate visual landmarks with LiDAR depth, the point cloud needs to be accumulated. Therefore, the sequentially sampled point cloud should be projected onto the camera frames corresponding to the visual keyframes.

[0125] First, the extrinsic parameters of the LiDAR-camera system need to be obtained to project the point cloud onto the camera frame. Utilizing the non-repeating mode of the LiDAR, the LiDAR-camera extrinsic parameters can be accurately estimated. Specifically, the LiDAR point cloud and the image are sampled simultaneously for several seconds in a static state. Then, the 3D points in the accumulated point cloud and the corresponding 2D pixels in the image can be obtained simultaneously. The extrinsic parameters are estimated by nonlinear optimization that minimizes the reprojection error. The estimated extrinsic parameters can be expressed as... ,in and These represent camera frames (c-frames) and lidar frames (l-frames), respectively.

[0126] like Figure 16 As shown, by utilizing continuous INS attitude and extrinsic parameters, it is possible to... The sampled lidar points are projected onto On the camera frame. The INS pose between two IMU samples can be obtained through linear interpolation. The position at a given moment can be represented as:

[0127]

[0128] in It is a proportionality coefficient. The world frame (w frame) is defined as the initial position of the navigation frame (n frame), i.e., the local northeast sky frame; b represents the IMU body frame (b frame). For attitude, we can interpolate the rotation vector:

[0129]

[0130] in Let represent the rotation vector. Therefore, we obtain in INS pose after time interpolation , Through camera-IMU extrinsic parameters You can convert an INS pose to a camera pose, as shown below:

[0131] The same process can be used to obtain Camera pose at any moment , Therefore, for A lidar point at a given time is denoted as . It can be converted to a C-frame, as follows:

[0132] With the help of and The camera's pose at any moment. The LiDAR points at any given time can be converted to The c-frame at time 1 is as follows:

[0133] Ultimately, the LiDAR points are projected onto the corresponding c-frames of the visual keyframes, without motion distortion. Sequentially sampled point clouds can be projected onto the same c-frame to obtain a relatively dense point cloud for depth association. However, more points consume more computational resources. Therefore, the accumulation time needs to be limited to control computational complexity. For a LiDAR system that can measure approximately 100,000 points per second, the point cloud density is sufficient for depth association. In contrast, the point cloud is sparser with an accumulation time of 0.25 seconds and denser with an accumulation time of 1 second. According to our experiments, accuracy does not significantly improve if the accumulation time exceeds 0.5 seconds. Therefore, to balance accuracy and computational complexity, the accumulation time is set to 0.5 seconds. The projected point cloud will be further processed for depth association.

[0134] Depth of visual landmarks is obtained using the projected point cloud. However, visual feature points lie in a two-dimensional image plane, while the point cloud lies in three-dimensional space. To achieve depth association, we can project the point cloud onto the image plane or project the visual feature points onto a c-frame. Compared to the large number of point clouds, the number of visual features is typically within a few hundred. Therefore, we project the visual features onto a c-frame for depth association, which significantly reduces computational complexity. The visual features projected onto the c-frame have no depth, so the association is performed on a unit sphere centered at the origin of the c-frame, such as... Figure 17 As shown, blue dots represent LiDAR points, and green dots represent visual feature points.

[0135] The projected point cloud is normalized and transformed onto a unit sphere. Furthermore, to maintain a constant angular density on the sphere, it is simultaneously downsampled. During downsampling, only points in the foreground are retained, thus removing some occluded points.

[0136] Higher angular density means more computation, while lower angular density means lower accuracy. However, due to motion, the field of view of the projected point cloud may be larger. Therefore, to ensure that more visual features can be correlated, the field of view of the projected point cloud is downsampled to 90°. The noise of feature tracking is typically within 1.5 pixels, meaning that the tracked features can be represented within a square with a width of 3 pixels. Therefore, considering the camera resolution, the downsampled angular density can be determined by the following formula:

[0137] Where h and v represent the horizontal and vertical directions, respectively. Based on the result of the above formula, the angle density is set to 0.2° to improve the accuracy of depth correlation. Therefore, the downsampled point cloud is located at a resolution of 450. On a unit sphere of 450°, visual feature points are also projected onto the unit sphere using a camera projection function.

[0138] 2. Plane inspection method A KD-tree is constructed using point clouds upsampled from a unit sphere to find corresponding points and obtain the depth of visual features. A plane fitting method is typically used, employing three selected LiDAR points to estimate the depth of visual features. However, using only three points cannot guarantee that the visual landmark lies on the true plane, potentially leading to inaccurate or incorrect depth estimates. Therefore, a plane checking algorithm can be used to verify the estimated depth, avoiding and reducing erroneous associations. Figure 17 As shown, we find the five nearest points to the visual feature point by searching in a KD-tree. We then use these five found points to fit a local plane around the visual landmark. We assume that the point farthest from the visual feature point found on the unit sphere is not... If the distance is within the specified range, the association process stops to avoid introducing potential outliers. Specifically, the five points found can be roughly represented on the image plane by a circle with a radius of r ≈ 3 × 3 = 9 pixels. In this depth association problem, it is expected that the plane will not pass through the origin. Therefore, the LiDAR point p on the plane can be represented as:

[0139] Where n is the normal vector of the plane. Therefore, an overdetermined linear equation can be constructed to solve for this plane:

[0140] in The LiDAR point with three-dimensional coordinates was found in frame c. The linear equation can be solved using methods such as QR decomposition to obtain the normal vector.

[0141] A plane-checking algorithm is employed to avoid erroneous associations and ensure that visual landmarks lie on the actual plane. More specifically, plane checking is performed by calculating the distance from points to the plane: ; If the distance (dis) of all five LiDAR points is less than 0.1m, a fitted plane is used to estimate the depth of the visual landmark; otherwise, depth association fails. Here, the 0.1m distance threshold is set based on the point-to-plane metric in the LiDAR-INS system. Occluded points are not explicitly handled, but they can be easily detected by the plane checking algorithm and are not used for depth association. Plane checking can significantly avoid erroneous depth associations, thereby improving the robustness and accuracy of the system.

[0142] With the normal vector n, we can obtain the landmark depth. We find the LiDAR point farthest from the visual feature on the unit sphere, i.e. Figure 17 In For visual feature points on a unit sphere We want to obtain its 3D coordinates in frame C. :

[0143] Where t is the distance to the center of frame c. The distance. Using plane equation 2.42, t can be solved:

[0144]

[0145] Finally, the three-dimensional coordinates of the visual landmarks in frame c. This can be obtained. In this visual keyframe, the landmark depth can be represented as:

[0146] The depth estimated by depth association can be directly used to constrain the inverse depth parameters of visual landmarks.

[0147] 3. Backend optimization based on factor graph A factor graph can be described as an undirected probabilistic model consisting of variable nodes and factor nodes. Variable nodes represent the state variables to be optimized, while factor nodes illustrate the observational constraints between the state variables. The joint probability distribution of a factor graph can typically be represented as the product of the factor terms, which simplifies to: (The normalization factor is then ignored.)

[0148] in, Represents the set of all variable nodes. This represents the corresponding set of factor functions.

[0149] By transforming the problem into a maximum a posteriori probability estimation task, the optimization can be rewritten as a least squares form of parameter solving:

[0150] In factor graph optimization, iSAM is a commonly used method, but it requires periodic reconstruction of the entire graph, resulting in a high computational burden and impacting real-time performance. To improve efficiency, a Bayesian tree structure is introduced, combined with a variable elimination strategy. During variable elimination, each operation is equivalent to performing marginal calculations on the conditional probability density function.

[0151]

[0152] in, For the variable nodes to be optimized, This represents the set of other variables that are directly connected to this variable.

[0153] When the above structure is further transformed into a Bayesian tree, each node represents a subset of the Bayesian network, let this set be... The joint probability density of this node is:

[0154] in, For set Intersection with its parent node The intersection, that is . This represents the variables that need to be eliminated in the current subgraph. This structure effectively avoids redundant calculations, significantly reduces memory usage, and significantly improves the algorithm's real-time response capability.

[0155] System factor diagram structure as follows Figure 18 As shown, it mainly includes four types of key factor nodes: first, the IMU pre-integration factor, which is used to constrain the motion changes caused by IMU integration; second, the laser odometry factor, which is used to constrain the feature matching between laser frames and map key frames; third, the visual odometry factor, which is used to constrain the relative motion between visual image frames; and fourth, the loop closure detection factor, which is used to construct closed-loop constraints to ensure the spatial consistency between the current frame and previous candidate key frames.

[0156] These factors together construct the graph structure required for system graph optimization, ultimately forming a joint optimization problem whose objective function can be written as:

[0157] in, and Let these represent the edge sets of IMU, visual odometry, LiDAR, and closed-loop constraints, respectively. It is the residual, defined as:

[0158] here, It is based on the relative pose measured by sensors. and These are the poses of two keyframes. It is usually represented by the SE(3) group, and its mathematical form is: Homogeneous transformation matrix:

[0159] in: yes The rotation matrix represents the rotation from the world coordinate system to the sensor coordinate system. yes The translation vector is the system's position coordinates.

[0160] Initial coordinates are typically derived from front-end processing, visual inertial odometry, and lidar inertial odometry. These front-end methods provide preliminary pose estimates, including position and orientation. Optimization uses a nonlinear least squares method to iteratively adjust the state variables. ,include To minimize the total residual, the update rule can be simplified to:

[0161] in, It is the Jacobian matrix of the positional residuals. It is the translation update vector obtained from the optimization.

[0162] After optimization and convergence, at each time step position Updated to the optimal estimate The final XYZ coordinates are from... Extraction of the translation portion: in, yes The translation vector.

[0163] 4. Verification Experiment of Multi-Source Fusion Algorithm To verify the feasibility of the multi-source fusion navigation algorithm, data was collected using sensors within the campus to verify the multi-source fusion navigation and positioning algorithm mentioned in this paper. The trajectory of the experimental data collection is shown by the red line in the figure, and the blue dots represent the starting positions.

[0164] As can be seen from the overall positioning trajectory comparison, the proposed method, R2LIVE, LIOSAM, and VINS can all follow the real trajectory well in most road sections. However, in some turns and areas with poor local lighting, the latter three algorithms exhibit varying degrees of cumulative error. Among them, VINS shows the most significant drift in scenes with drastic lighting changes; LIOSAM has slightly lower overall accuracy than R2LIVE, and exhibits more obvious deviations when turning; R2LIVE maintains a certain degree of stability during the fusion of LiDAR and visual information, but it still falls short of the accuracy and robustness of the proposed method in complex environments.

[0165] Table 4-1 APE Data Table for Multi-Source Fusion Experiment under Normal Illumination (Unit: m)

[0166] Statistical results for APE show that the method described in this embodiment significantly outperforms the comparative algorithms in terms of maximum error, mean error, and RMSE. Specifically, the average error of the method described in this embodiment is only 0.22m, while the average errors of R2LIVE, LIOSAM, and VINS are all higher than those of the proposed algorithm. Regarding the maximum error, the proposed method has only 1.60m, while VINS reaches 3.89m. This demonstrates that the proposed method has a significant advantage in global accuracy under varying lighting conditions and complex indoor and outdoor environments.

[0167] This implementation constructs a multi-sensor fusion framework based on factor graph optimization. Through collaborative optimization of IMU, vision, and LiDAR data, it significantly improves the system's positioning accuracy and robustness under low-light conditions. Detailed algorithm descriptions and experimental verifications show that effective collaboration between modules not only reduces accumulated errors but also improves the accuracy of depth estimation. Overall, the research results described in this implementation provide practical technical support for the real-world application of autonomous driving and robot navigation systems in complex environments.

[0168] Those skilled in the art will understand that the above description is merely a preferred embodiment of the present invention, and the features described in the various embodiments and / or claims of this disclosure can be combined or combined in various ways, even if such combinations or combinations are not explicitly described in this disclosure. This is not intended to limit the present invention. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art can still modify the technical solutions described in the foregoing embodiments or make equivalent substitutions for some of the technical features. 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.

[0169] Although preferred embodiments of the invention have been described, those skilled in the art, upon learning the basic inventive concept, can make other changes and modifications to these embodiments. Therefore, the appended claims are intended to be interpreted as including both the preferred embodiments and all changes and modifications falling within the scope of the invention. Clearly, those skilled in the art can make various alterations and modifications to the invention without departing from its spirit and scope. Thus, if these modifications and modifications of the invention fall within the scope of the claims and their equivalents, the invention is also intended to include these modifications and modifications.

Claims

1. A multi-sensor fusion localization method based on factor map optimization under low-light conditions, characterized in that, The method includes the following steps: Step 1: Construct visual inertial odometry and lidar inertial odometry, and extract key feature points and their corresponding descriptors from continuous image streams. Utilize the high-frequency attitude prior information provided by the inertial navigation system, and combine it with visual feature matching and lidar point cloud matching respectively to achieve high-precision state estimation of visual inertial odometry and lidar inertial odometry. Step 2: Project the LiDAR point cloud onto the visual keyframe and correct motion distortion. Use the planar inspection method to perform robust depth association. Step 3: Adopt a joint optimization strategy based on factor graphs to unify the data from inertial odometry, visual odometry and lidar odometry into a unified model, and achieve global state optimization through nonlinear optimization algorithms.

2. The multi-sensor fusion localization method based on factor map optimization under low-light conditions according to claim 1, characterized in that, Step 1 also includes constructing an IMU pre-integration model and an error propagation model to calculate the relative IMU measurements between two historical keyframes and convert them into constraint edges, i.e., processing noise and zero bias in the IMU measurements.

3. The multi-sensor fusion localization method based on factor map optimization under low-light conditions according to claim 1, characterized in that, The method for extracting key feature points and their corresponding descriptors from a continuous image stream in step 1 is as follows: the FAST algorithm is used to detect corner points in the continuous image, and the Lucas-Kanade algorithm in the sparse optical flow method is used to perform inter-frame tracking of the continuous image.

4. The multi-sensor fusion localization method based on factor map optimization under low-light conditions according to claim 3, characterized in that, The RANSAC algorithm is introduced into the matching results of inter-frame tracking of continuous images in the Lucas-Kanade algorithm to remove erroneous matches.

5. The multi-sensor fusion localization method based on factor map optimization under low-light conditions according to claim 1, characterized in that, The robust depth association method using the planar inspection approach in step 2 is as follows: Where n is the normal vector of the plane.

6. The multi-sensor fusion localization method based on factor map optimization under low-light conditions according to claim 1, characterized in that, In step 3, the factor graph structure includes an IMU pre-integration factor, used to constrain motion changes caused by IMU integration; a laser odometry factor, used to constrain feature matching between laser frames and map keyframes; a visual odometry factor, used to constrain relative motion between visual image frames; and a loop closure detection factor, used to construct closed-loop constraints to ensure spatial consistency between the current frame and previous candidate keyframes.

7. The multi-sensor fusion localization method based on factor map optimization under low-light conditions according to claim 1, characterized in that, The method for achieving global state optimization using a nonlinear optimization algorithm in step 3 is as follows: in, and Let these represent the edge sets of IMU, visual odometry, LiDAR, and closed-loop constraints, respectively. It is a residual; ,in, It is based on the relative pose measured by sensors. and These are the poses of two keyframes; the poses are: ,in: yes The rotation matrix represents the rotation from the world coordinate system to the sensor coordinate system. yes The translation vector is the system's position coordinates.

8. A multi-sensor fusion positioning system based on factor map optimization in low-light environments, characterized in that, The system includes: The state estimation module is used to construct visual inertial odometry and lidar inertial odometry, and extract key feature points and their corresponding descriptors from continuous image streams. It utilizes the high-frequency attitude prior information provided by inertial navigation system, and combines it with visual feature matching and lidar point cloud matching to achieve high-precision state estimation of visual inertial odometry and lidar inertial odometry. The correction module is used to project the LiDAR point cloud onto the visual keyframe and correct motion distortion, and uses a planar inspection method for robust depth association. The optimization module is used to adopt a joint optimization strategy based on factor graphs to unify the modeling of data from inertial odometry, visual odometry, and lidar odometry, and to achieve global state optimization through nonlinear optimization algorithms.

9. A computer storage medium storing a computer program, characterized in that, When the computer program is executed by a processor, it implements the method described in any one of claims 1-7.

10. A computer device, characterized in that, include: A memory, a processor, and a computer program stored in the memory and executable on the processor, the processor executing the program to implement the method of any one of claims 1-7.

Images