Virtual voxel map-based visual-inertial-laser fusion positioning method and system

Abstract

The application discloses a visual-inertial-laser fusion positioning method and system based on a virtual voxel map, and belongs to the technical field of unmanned aerial vehicle navigation positioning. The method comprises the following steps: collecting multi-sensor data and performing space-time alignment; constructing a local point cloud map of a laser radar, and updating a carrier pose based on a point-surface residual; constructing a virtual voxel map for storing camera visibility attributes corresponding to spatial positions; calculating a projection score of laser points in the field of view of each camera through a point cloud projection score strategy, selecting the camera with the highest score as the best attribute, storing the attribute in the virtual voxel map, directly reusing the attribute for registered voxels, and completing visibility marking; and constructing a visual error model using the marked points to perform visual measurement updating. The application buffers camera observation relationships through the virtual voxel map, effectively solves the problems of large calculation amount and insufficient robustness of a multi-camera system, and is suitable for unmanned aerial vehicle platforms with limited computing resources.

Description

Technical Field

[0001] This invention belongs to the field of UAV navigation, positioning and mapping technology, specifically relating to a visual-inertial-laser fusion positioning method and system based on virtual voxel maps. Background Technology

[0002] In the field of autonomous navigation for unmanned aerial vehicles (UAVs), multi-sensor fusion technology is the core for achieving high-precision positioning. A typical vision-inertial-lidar fusion odometry system consists of a monocular camera, an inertial measurement unit (IMU), and a lidar system. The IMU provides high-frequency motion estimation, the visual sensor provides texture information, and the lidar provides accurate 3D geometry. These three components complement each other, enabling robust positioning and mapping in complex environments.

[0003] In recent years, research on vision-inertial-LiDAR fusion technology in UAV navigation has gradually increased, and it can be divided into two categories: tightly coupled and loosely coupled methods. Tightly coupled algorithms are superior to loosely coupled algorithms due to their deeper data fusion, with FASTLIVO2 and R3LIVO++ being the most typical examples. Both of these methods are based on direct pose optimization, requiring no feature extraction, resulting in low computational cost and superior accuracy and real-time performance compared to other tightly coupled algorithms. However, with increasing demands for environmental perception capabilities, multi-camera solutions have become a trend in engineering practice due to their wider field of view and richer information content.

[0004] When the aforementioned direct-method-based fusion odometry is extended to a multi-camera architecture, the following problems arise: When directly using point cloud projection to correlate with images, the computational load increases exponentially with the number of cameras, making it difficult to meet the real-time requirements of high-frequency UAV positioning and resulting in low computational efficiency. Furthermore, existing projection matching does not adequately consider the observation quality in overlapping multi-camera areas. Projection points located at the edge of the camera's field of view or in areas with significant distortion have poor observation quality; directly involving them in the calculation would reduce system accuracy, leading to insufficient robustness and scene adaptability.

[0005] Therefore, there is an urgent need for a lightweight fusion positioning method that can effectively reduce computational load while ensuring the quality of multi-view observations. Summary of the Invention

[0006] Purpose of the invention: The purpose of this invention is to address the shortcomings of existing technologies by providing a visual-inertial-laser fusion positioning method and system based on virtual voxel maps. By introducing virtual voxel maps and projection scoring strategies, this invention solves the problems of low computational efficiency and lack of observation quality screening mechanisms in existing technologies when extended to multi-view cameras.

[0007] Technical solution: The visual-inertial-laser fusion positioning method based on virtual voxel maps described in this invention includes:

[0008] Step S1: Acquire IMU data, LiDAR point cloud data, and multi-view image data collected by the inertial measurement unit, LiDAR, and multi-view camera; use the IMU data to perform recursive calculations to obtain the carrier pose state; preprocess the LiDAR point cloud data and align it to the frame time of the multi-view image data to obtain the LiDAR frame point cloud.

[0009] Step S2: Construct a local point cloud map of the lidar and find the nearest neighbor plane of each scan point in the lidar frame point cloud, construct the radar measurement equation and update the carrier pose;

[0010] Step S3: Construct a virtual voxel map to establish the mapping relationship between 3D spatial voxels and camera visibility attributes; for scan points in the laser frame point cloud, perform a search or calculation operation according to their registration status in the virtual voxel map: if the voxel to which the point belongs has registered a camera attribute, then directly read the attribute as the visibility mark of the scan point; if not registered, calculate the projection score of the scan point under each camera according to the point cloud projection score strategy, select the camera attribute corresponding to the highest score to register in the virtual voxel map, and mark the scan point with a visibility mark;

[0011] Step S4: Using laser points with visible markings, project them onto the pixel coordinate system of the camera corresponding to the markings, construct a visual error model, perform visual measurement updates, and output the final pose state of the carrier.

[0012] Furthermore, in step S1, the specific method for performing distortion correction and time alignment on the lidar point cloud data is as follows:

[0013] The acceleration and angular velocity of the carrier are obtained using IMU data, and the IMU coordinate system is calculated. Time's up Transition matrix at time step ;

[0014] Based on the known fixed transition matrix between the radar coordinate system and the IMU coordinate system The following formula will be used to... Laser points collected in real time Switch to Distortion correction is performed in the radar coordinate system at that time:

[0015]

[0016] in, These are the coordinates of the laser point after conversion.

[0017] Further, in step S2, constructing the local point cloud map of the LiDAR includes: managing the local point cloud map of the LiDAR using a data structure combining a hash table and an octree; dividing the three-dimensional space into root voxels of fixed size as index units of the hash table, and nesting octree nodes within the root voxels to recursively divide the space into leaf voxels; the leaf voxels are used to store planar features, including the planar normal vector and the planar center point; when the laser scanning point falls into an existing leaf voxel, the plane within the voxel is refitted using that point; if the change in the fitted planar parameters is less than a preset threshold or the number of planar fittings reaches a preset upper limit, the leaf voxel is marked as a mature voxel, and the planar parameters of the leaf voxel are no longer updated subsequently.

[0018] Furthermore, in step S3, the virtual voxel map is implemented based on a hash table, with the voxel block position as the key and the camera attribute as the value;

[0019] The specific process for obtaining the highest-scoring camera attribute based on the point cloud projection score strategy is as follows: Scan points within unregistered voxels are projected onto the imaging planes of each of the multi-view cameras; if a scan point meets the invisible condition in a certain camera, the projection score corresponding to that camera is zero or invalid; if a scan point meets the visible condition in a certain camera, the projection score is calculated based on the degree to which the projection position of the scan point in the camera coordinate system deviates from the camera's optical center; the closer the projection position is to the camera's optical center or field of view center, the higher the projection score; the projection scores of each camera are compared, and the camera ID with the highest total score is selected as the camera attribute of that scan point; if all cameras are invisible, the camera attribute of that voxel is registered as zero in the virtual voxel map.

[0020] Furthermore, the method for calculating the projection score includes:

[0021] Calculate the three-dimensional coordinates of the scan points in each camera coordinate system;

[0022] Calculate the horizontal angle of the scanning point in the camera coordinate system based on the three-dimensional coordinates. Angle with vertical ;

[0023] Calculate the projected score using the following formula:

[0024]

[0025]

[0026] in, and These are the camera's horizontal and vertical field of view, respectively. and These are the projection fractions in the horizontal and vertical directions, respectively;

[0027] If the calculated projection score If the preset threshold condition is met, the camera with the highest score will be selected as the best projection camera; otherwise, the scan point will be marked as invisible.

[0028] Furthermore, in step S4, before constructing the visual error model, a feature point screening step is also included: determining whether the voxel to which the laser point with the visibility mark belongs in the local point cloud map of the lidar is a mature voxel; only the laser points belonging to mature voxels are used as feature points and projected onto the camera image corresponding to their visibility mark to participate in the construction and updating of the visual error model.

[0029] Furthermore, in step S3, before constructing the virtual voxel map, a visual keyframe selection step is also included: determining whether the mean value of the point-area residuals of the radar measurement equation in step S2 exceeds the limit; if it exceeds the limit, the keyframe selection and subsequent visual measurement update are skipped; if it does not exceed the limit, determining whether the positional deviation between the current camera frame and the previous keyframe is greater than a preset distance, or whether the rotation angle deviation between the current camera frame and the previous keyframe is greater than a preset angle; if any of the above conditions are met, the current frame is determined to be a keyframe.

[0030] Furthermore, the visual measurement update in step S4 can be performed using either a direct method (based on a photometric error model) or a semi-direct method (based on a reprojection error model).

[0031] If the direct method is used, it is based on the feature point photometric consistency assumption, aligning the feature point image block obtained from the current frame with the reference image block of the feature point to construct the photometric error measurement equation, and using the iterative extended Kalman filter IESKF to estimate the carrier pose;

[0032] If a semi-direct method is used, it is based on optical flow tracking and feature point coordinate triangulation. The deviation between the observed pixel coordinates and the reprojection coordinates of the feature points in the current frame is calculated to construct a reprojection error model, and a multi-state constrained Kalman filter (MSCKF) is used to perform state updates.

[0033] Furthermore, the present invention also relates to a multi-view vision-inertial-lidar fusion positioning system, including an inertial measurement unit, a lidar, a multi-view camera, a memory, and a processor, wherein the processor is configured to execute the above-described method.

[0034] Beneficial effects: Compared with the prior art, the advantages of the present invention are as follows:

[0035] This invention innovatively proposes a virtual voxel map, which uses the principle of spatial consistency to bind 3D spatial voxels to the ID of the best observation camera. For point clouds of registered areas, there is no need to repeatedly perform tedious geometric projection calculations; the corresponding camera can be found simply by looking up a table. This significantly reduces the CPU usage in multi-camera systems, enabling the algorithm to run efficiently on resource-constrained UAV embedded platforms.

[0036] By introducing a projection scoring strategy based on the deviation of the field of view center, the system can automatically remove laser points located at the edge of the camera's field of view, with large distortion, or with poor observation angles, and only retain the best observation data with the "highest score" to participate in visual updates. This effectively overcomes the positioning drift problem caused by poor projection quality in traditional methods and improves the quality of feature points.

[0037] A single frame of a LiDAR system contains approximately 200,000 points. Conventional methods require 200,000 * n (number of cameras) projection calculations per frame. The lightweight multi-view vision-inertial-LiDAR fusion localization method proposed in this invention, based on a virtual voxel map, stores the voxel position attributes in the radar coordinate system. During the entire program execution phase, points at the same voxel position only need to be projected once, including n (number of cameras) projection calculations. Voxel positions that have already been calculated do not need to be projected again. Therefore, as the number of registered positions in the virtual voxel map increases, the amount of projection gradually decreases over time, exhibiting good scalability and the ability to be combined with various vision and LiDAR measurement models. Attached Figure Description

[0038] Figure 1 This is a flowchart illustrating the overall process of the visual-inertial-laser fusion positioning method based on virtual voxel maps in this embodiment of the invention.

[0039] Figure 2 This is a detailed step logic diagram of the method in the embodiments of the present invention;

[0040] Figure 3 This is a flowchart of the keyframe selection strategy in an embodiment of the present invention;

[0041] Figure 4 This is a flowchart illustrating the specific process of constructing a virtual voxel map and point cloud visibility markers in an embodiment of the present invention. Detailed Implementation

[0042] The technical solution of the present invention will be described in detail below with reference to the accompanying drawings, but the scope of protection of the present invention is not limited to the embodiments described.

[0043] Example 1: As Figure 1 and Figure 2 As shown, the visual-inertial-laser fusion localization method based on virtual voxel maps proposed in this embodiment mainly includes four core steps:

[0044] Step 1: Collect data from multiple sensors, recursively calculate the carrier pose state by the acquired IMU acceleration and angular velocity, perform distortion correction and downsampling on the radar scan points, and align them to the image frame time acquired by the camera.

[0045] Step 2: Find the nearest neighbor plane of each radar scan point in the voxel map, calculate the point-plane residual based on the features of the searched plane, construct the measurement equation, and update the carrier pose state.

[0046] Step 3: If the camera frame meets the key frame condition, downsample the coordinate position of the point cloud in the local system within the laser frame to construct a virtual voxel map. Obtain the highest-scoring camera attribute of each downsampled point according to the point cloud fractional projection strategy. Register the attribute and the voxel coordinates of the corresponding downsampled point to the virtual voxel map managed by the hash table. Complete the point cloud visibility marking in the registered voxel area through the virtual voxel map.

[0047] Step 4: Utilize lidar to measure and update the pose and point cloud data with camera visibility markers, project the point cloud onto the pixel coordinate system of the marked camera to add visual features to the point cloud data, perform visual updates by constructing a photometric or reprojection error model, and output the final pose state of the carrier.

[0048] Step 1: Data Acquisition and Processing

[0049] The system acquires data from an IMU, LiDAR, and multi-view cameras. High-frequency data (acceleration and angular velocity) from the IMU is used for state recursion to obtain the predicted pose at the current moment. Due to motion distortion during LiDAR acquisition, the LiDAR scanning points need to be aligned with the camera image frame acquisition time. Specifically, using the pose calculated by IMU pre-integration, according to equation (1), the pose is... Laser points collected in real time Switch to Distortion removal is performed in the radar coordinate system at any given time.

[0050] (1)

[0051] Step 2: LiDAR Measurement Update

[0052] 1) Construction of local point cloud map

[0053] This invention is compatible with various point cloud storage structures, including binary trees, octrees, and kd-trees. To achieve efficient spatial querying of point clouds during measurement model construction, a hash table and octree structure are used to manage radar point cloud voxel maps. Specifically, the three-dimensional space is divided into root voxels of fixed size, which serve as the basic index units of the hash table. Scan points in the radar coordinate system are projected to the world coordinate system based on their pose after radar updates. Based on the root voxel's spatial coordinates, the hash table is queried to obtain the nested octree nodes within the root voxel, and the scan points are inserted into them. The space is recursively divided according to octree rules. The smallest voxel corresponding to an octree leaf node is the leaf voxel, used to store planar features, including the planar normal vector and the planar center point. During point cloud registration for each frame, if a scanned point falls into an existing leaf voxel, the in-voxel plane is refitted. When the change in plane parameters is less than a threshold or the number of plane fitting attempts reaches the upper limit (for each new point, 10 points are retained for plane fitting; the angle between the fitted plane normal vector and the original in-voxel plane normal vector is calculated using the dot product formula; if the angle is less than 0.5° or the number of fitting attempts is greater than 10, it is considered a mature voxel), the leaf voxel is marked as a mature voxel, and the plane parameters are no longer updated. To avoid memory bloat, only root voxels within a distance of L from the radar sensor are retained in the map.

[0054] 2) Radar Measurement Model Construction and Update

[0055] The lightweight multi-view vision-inertial-lidar fusion localization method constructed in this invention supports various radar measurement models and vision measurement models. The radar measurement equations can be constructed based on point-area residuals, and the optimal state solution can be obtained through IESKF (Iterative Extended Filter Estimation). Alternatively, radar point cloud planar residual measurement equations can be constructed and the optimal solution can be obtained based on MSCKF (Multi-State Filter Estimation). Taking point-area residuals combined with IESKF as an example, the specific steps for radar update are as follows:

[0056] First, the coordinates of the laser frame scan points are obtained through... , Transform to the world coordinate system, as shown in equation (2):

[0057] (2)

[0058] In the formula, This represents the point cloud position coordinates in the radar coordinate system at time k. The transfer matrix from the laser radar to the IMU coordinate system is a fixed and known matrix; This represents the transition matrix from the IMU coordinate system to the global coordinate system, which can be obtained through the pose recursion of the IMU. Once all the information is known, the position of the current laser frame scanning point in the world coordinate system can be determined. .

[0059] The nearest neighbor plane of the scanned point is quickly queried using a hash table and an octree structure, and the point-plane residual is calculated as shown in equation (3):

[0060] (3)

[0061] In the formula, This represents the point-to-surface residual corresponding to the i-th laser scanning point. and These represent the plane center point and plane normal vector stored in the leaf voxel corresponding to the scan point, respectively. Based on the above formula, a point-plane residual is constructed for the scan point that has successfully matched the plane, and the transition matrix from the IMU coordinate system to the global world coordinate system is estimated based on the IESKF filter. . and Given fixed positions and poses of the radar and camera sensors in the world coordinate system. and renew.

[0062] Step 3: Lightweight laser point cloud projection and filtering (virtual voxel map construction)

[0063] 1) Selection of visual keyframes

[0064] like Figure 3 As shown, after the LiDAR update is completed at the current moment, visual keyframe selection is performed. First, based on the constructed LiDAR measurement model, it is determined whether the mean value of the point-to-surface residual exceeds the limit. If it does, the keyframe selection is skipped, and preprocessing operations such as distortion correction and pixel downsampling are performed before output. If it does not exceed the limit, it is determined whether the camera frame satisfies the keyframe condition shown in equation (4):

[0065]

[0066] (4)

[0067] in, For the camera at the current moment The lower position, For the previous keyframe moment When the camera position at the time of the event deviates from the preset distance, the keyframe condition is met. The current camera pose. The camera pose at the previous keyframe moment is considered as a keyframe if the rotation angle between the two is greater than a preset angle. Preprocessing operations are performed on the keyframe images that meet the conditions and then output. If it is the first frame image, it is also considered a keyframe.

[0068] 2) Virtual voxel map construction and point cloud visibility marking

[0069] A virtual voxel map, `key_map`, is constructed within the detection range of the LiDAR sensor. `key_map` is implemented using a hash table, with voxel block positions as keys and camera attributes as values, enabling rapid lookup of corresponding camera attributes based on voxel locations. Based on the constructed virtual voxel map, invisible LiDAR scan points within the camera's field of view are quickly filtered out, and the optimal projected camera attributes are assigned to visible LiDAR scan points. During operation, the relative positions between the camera and the LiDAR remain fixed, so the camera attributes registered in `key_map` do not change. `key_map` is continuously updated based on the LiDAR scan coverage area for each frame, without being reset.

[0070] like Figure 4 As shown, the specific implementation process is as follows: Set the voxel size in the virtual voxel map. Determine the voxel block position corresponding to the scan point by dividing the coordinates of the laser intra-frame point cloud after distortion correction by the voxel size. Locate the voxel block position in the map and confirm whether it is registered. If registered, further check if the camera attribute within the voxel is 0. If 0, it means the point is within the camera's field of view and the scan point is deleted, not proceeding to the subsequent visual update process; if not zero, assign the camera attribute from the corresponding voxel block in the map to the scan point. If not registered, project the point to the multi-view camera sensor and calculate the projection score. The projection score calculation strategy is as follows:

[0071]

[0072]

[0073] (5)

[0074] in, , , The three-dimensional coordinates of the laser point in the camera coordinate system. and These represent the horizontal and vertical angles of the laser point in the camera coordinate system. and These are the horizontal and vertical field of view of the camera, respectively. If the laser point satisfies any of the following conditions (6) and (7), it is considered that the laser point is located in the invisible range of the camera, and the voxel block position and camera attribute (set to 0) of the point are added to the camera attribute map.

[0075] (6)

[0076] (7)

[0077] in, This represents the z-axis coordinate of the laser point in the i-th camera coordinate system. and Let represent the horizontal and vertical projection fractions of the laser point projected onto the i-th camera, respectively, and n represent the total number of cameras. If none of the above conditions are met, the total projection fraction is selected. The highest camera is used as the camera attribute for that laser point to complete the visibility marking of the point cloud, and the camera attribute of that point is registered at the corresponding voxel position in the virtual voxel map.

[0078] Step 4: Visual Measurement Update

[0079] This invention supports multiple visual update methods. Visual pose state updates can be completed by constructing photometric error equations through direct methods or reprojection error equations through semi-direct methods, flexibly adapting to different scenario requirements.

[0080] 1) Projection and Feature Point Retrieval

[0081] Before constructing a visual measurement model, both direct and semi-direct methods require projection and feature point retrieval steps. This involves projecting the center points of mature voxel planes in the local point cloud map onto the pixel coordinate system of the current frame image and selecting feature points based on specific retrieval criteria. Specifically, this can be divided into the following steps:

[0082] First, calculate the voxel to which the point after visual marking belongs in the local point cloud map; determine whether the voxel is a mature voxel. If it is a mature voxel, project its plane center point onto the pixel coordinate system of the camera to which the visual marking point belongs according to Equation (8).

[0083] (8)

[0084] in, Represents pixel coordinates; Indicates depth value; This represents the world coordinates of the center point of the plane stored in the voxel; and These represent the rotation and translation matrices of the camera coordinate system relative to the world coordinate system of the camera to which the visible marker belongs, respectively, and can be obtained from the camera sensor state updated by radar. Obtained by disassembly; and This represents the focal length of the camera to which the visible marker belongs in the x and y axes. The visible markers projected onto the pixel coordinate system are selected based on criteria such as grayscale value and projection depth. The selected points are then used as feature points in the subsequent visual update process.

[0085] 2) Visual measurement model construction and updating

[0086] a. Construction of direct photometric error

[0087] The direct photometric error model is built upon the feature point photometric consistency assumption. Feature point storage information includes the feature point's position in the world coordinate system and the image frame information of the observed feature point. Image frame information includes the frame pose and the corresponding feature point image patch. Based on a similarity scoring strategy, a reference image patch for the feature point can be obtained by filtering from past frame image patches. As the number of frames observing the same feature point increases, the reference image patch for the feature point will be continuously updated. After the update count reaches its limit, the reference image patch is fixed, and the feature point only retains the reference frame information, deleting information from other image frames.

[0088] By directly aligning the feature point image patch obtained from the current frame's filtering with the reference image patch for that feature point, a visual photometric error measurement model can be constructed as follows:

[0089] (9)

[0090] in, It is a camera projection model (projecting points in the camera coordinate system to the pixel coordinate system). It is the pose of the global frame G relative to the reference frame Cr. This represents the position of the i-th current block in the local map coordinate system. It is the affine transformation matrix that transforms pixels from the i-th current block to the reference block. It is the relative pixel position relative to the center of the current block. and These represent the exposure durations of the reference and current frame images, respectively. and represents the pixel values ​​of the reference and current frame images, respectively. To reduce the impact of low-quality images on visual updates, the aforementioned photometric error needs to be less than a certain threshold. By constructing the photometric error model shown in equation (9), the pose state of the camera in the world coordinate system at the current moment can be updated based on IESKF. . and Given fixed positions and orientations of the lidar sensor and the IMU in the world coordinate system. and renew.

[0091] b. Construction of Semi-Direct Reprojection Error

[0092] The semi-direct reprojection error is constructed based on optical flow tracking and feature point coordinate triangulation. Feature point storage information includes the pixel coordinates of feature points in different tracking frames and their pose states in the corresponding frames. Feature point information within 5-10 frames is stored using a sliding window. When optical flow tracking is lost, feature point triangulation is performed to obtain a more accurate feature point position in the world coordinate system. Assuming feature points The pixel coordinates in the latest frame are The reprojection error model can be constructed as follows:

[0093] (10)

[0094] In the formula, It is a camera projection model; Given the updated camera pose from the lidar measurements, the visual measurement update can be performed based on MSCKF by constructing the reprojection error model shown in equation (10), thus estimating the pose of multiple image frames within the sliding window. and based on known and Update the pose status of the lidar sensor and IMU.

[0095] Example 2: This example provides a multi-view vision-inertial-lidar fusion positioning system that implements the above method. The hardware includes:

[0096] Inertial Measurement Unit (IMU): Acquires triaxial acceleration and angular velocity;

[0097] LiDAR: Collects environmental point clouds;

[0098] Multi-view camera: Includes at least two cameras (such as forward + downward, or a combination of surround view) to acquire environmental images;

[0099] Processor and memory: The memory stores a computer program, which the processor executes to implement the steps in Example 1.

[0100] Those skilled in the art should understand that this invention achieves "space-for-time" optimization through virtual voxel mapping and ensures the quality of multi-view fusion through projection scoring, and is not limited to specific hardware models or parameter settings. Any improvements and modifications based on the technical solutions of this invention without departing from the principles of this invention should fall within the protection scope of this invention.

Claims

1. A visual-inertial-laser fusion positioning method based on virtual voxel maps, characterized in that, The method includes: Step S1: Acquire IMU data, LiDAR point cloud data, and multi-view image data collected by the inertial measurement unit, LiDAR, and multi-view camera; use the IMU data to perform recursive calculations to obtain the carrier pose state; preprocess the LiDAR point cloud data and align it to the frame time of the multi-view image data to obtain the LiDAR frame point cloud. Step S2: Construct a local point cloud map for the LiDAR, including: managing the local point cloud map using a hash table combined with an octree data structure; dividing the three-dimensional space into root voxels of fixed size as index units of the hash table; nesting octree nodes within the root voxels to recursively divide the space into leaf voxels; the leaf voxels are used to store planar features, including the planar normal vector and the planar center point; when a laser scanning point falls into an existing leaf voxel, the plane within the voxel is refitted using that point; if the change in the fitted planar parameters is less than a preset threshold or the number of planar fittings reaches a preset upper limit, the leaf voxel is marked as a mature voxel, and its planar parameters are no longer updated; finding the nearest neighbor plane of each scanning point in the laser frame point cloud in the local point cloud map of the LiDAR, constructing the radar measurement equation, and updating the carrier pose. Step S3: Construct a virtual voxel map. This virtual voxel map is implemented using a hash table, with voxel block positions as keys and camera visibility attributes as values, to establish a mapping relationship between 3D spatial voxels and camera visibility attributes. For scan points in the laser frame point cloud, a lookup or calculation operation is performed based on their registration status in the virtual voxel map: If the voxel to which the point belongs has registered a camera visibility attribute, that attribute is directly read as the visibility marker for the scan point; if not registered, the projection score of the scan point under each camera is calculated according to the point cloud projection score strategy, including: projecting scan points within unregistered voxels onto the imaging planes of each multi-view camera; if a scan point meets the invisible condition in a certain camera, the projection score corresponding to that camera is zero or an invalid value; if a scan point meets the visible condition in a certain camera, the projection score is calculated based on the degree to which the projection position of the scan point in the camera coordinate system deviates from the camera's optical center. The closer the projection position is to the camera's optical center or the center of the field of view, the higher the projection score. The projection score calculation method includes: calculating the three-dimensional coordinates of the scanning point in each camera coordinate system; and calculating the horizontal angle of the scanning point in the camera coordinate system based on the three-dimensional coordinates. Angle with vertical The projection score is calculated using the following formula: , ,in, and These are the camera's horizontal and vertical field of view, respectively. and These are the projection fractions in the horizontal and vertical directions, respectively; if the calculated projection fractions If the preset threshold condition is met, the camera ID with the highest total projection score is selected as the camera visibility attribute of the voxel and registered in the virtual voxel map, and the corresponding visibility mark is marked for the scan point; if all cameras are invisible, the camera visibility attribute of the voxel is registered as zero in the virtual voxel map. Step S4: Feature point screening for laser points with visible markings: Determine whether the voxel to which the laser point with visible markings belongs in the local point cloud map of the lidar is a mature voxel; Only laser points belonging to mature voxels are used as feature points, and the selected feature points are projected onto the pixel coordinate system of the camera corresponding to their visible markings, a visual error model is constructed to perform visual measurement updates, and the final pose state of the carrier is output.

2. The visual-inertial-laser fusion positioning method based on virtual voxel maps according to claim 1, characterized in that, In step S1, the specific method for performing distortion correction and time alignment on the lidar point cloud data is as follows: Using IMU data to acquire the acceleration and angular velocity of the carrier, the IMU coordinate system is calculated. Time's up Transition matrix at time step ; Based on the known fixed transition matrix between the radar coordinate system and the IMU coordinate system The following formula will be used to... Laser points collected in real time Switch to Distortion correction is performed in the radar coordinate system at that time: ， in, These are the coordinates of the laser point after conversion.

3. The visual-inertial-laser fusion positioning method based on virtual voxel maps according to claim 1, characterized in that, In step S3, a visual keyframe selection step is included before constructing the virtual voxel map: Determine whether the mean value of the point-area residual in the radar measurement equation in step S2 exceeds the limit. If it does, skip the key frame selection and subsequent visual measurement update. If the positional deviation between the current camera frame and the previous key frame is greater than the preset distance or the rotation angle deviation between the current camera frame and the previous key frame is greater than the preset angle, then determine that the current frame is a key frame.

4. The visual-inertial-laser fusion positioning method based on virtual voxel maps according to claim 1, characterized in that, The visual measurement update in step S4 uses either a direct method or a semi-direct method: If the direct method is used, it is based on the feature point photometric consistency assumption, aligning the feature point image block obtained from the current frame with the reference image block of the feature point to construct the photometric error measurement equation, and using the iterative extended Kalman filter IESKF to estimate the carrier pose; If a semi-direct method is used, it is based on optical flow tracking and feature point coordinate triangulation. The deviation between the observed pixel coordinates and the reprojection coordinates of the feature points in the current frame is calculated to construct a reprojection error model, and a multi-state constrained Kalman filter (MSCKF) is used to perform state updates.

5. The visual-inertial-laser fusion positioning method based on virtual voxel maps according to claim 4, characterized in that, The equation for measuring photometric error is as follows: ， in, For camera projection model, , These are the image pixel values ​​for the current frame and the reference frame, respectively. , These are the exposure durations for the current frame and the reference frame, respectively. This represents the current pose of the camera in the world coordinate system. For the world coordinates of the feature point, Relative pixel position The affine transformation matrix that transforms pixels from the current block to the reference block. For the global frame G relative to the reference frame The posture; The reprojection error model is as follows: , in, These are the observed pixel coordinates of the feature point in the current frame. For camera projection model, To measure the updated camera pose using LiDAR, Triangulation is performed to obtain the position of feature points in the world coordinate system when tracking is lost.

6. A vision-inertial-laser fusion positioning system based on virtual voxel maps, characterized in that, The system includes: An inertial measurement unit is used to collect acceleration and angular velocity data of the carrier. LiDAR is used to collect point cloud data of the environment; Multi-view cameras are used to acquire image data of the environment; Memory, used to store computer programs; The processor, connected to the inertial measurement unit, lidar, multi-view camera, and memory, is used to execute the computer program stored in the memory to implement the method as described in any one of claims 1 to 5.

Images