Vision-inertia-laser fusion positioning method and system based on virtual voxel map

Abstract

The invention discloses a vision-inertia-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 carrying out space-time alignment; constructing a laser radar local point cloud map, and updating a carrier pose based on a point-surface residual error; constructing a virtual voxel map used for storing camera visibility attributes corresponding to the spatial positions; the projection scores of the laser points in the view fields of the cameras are calculated through a point cloud projection score strategy, the camera with the highest score is selected as the optimal attribute to be stored in the virtual voxel map, the attributes are directly reused for registered voxels, and visuality marking is completed; and constructing a visual error model by using the mark points to execute visual measurement updating. According to the method, the camera observation relation is cached through the virtual voxel map, the problems of large calculation amount and insufficient robustness of a multi-view system are effectively solved, and the method is suitable for an unmanned aerial vehicle platform with limited calculation resources.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of unmanned aerial vehicle navigation positioning and mapping, and particularly relates to a visual-inertial-laser fusion positioning method and system based on a virtual voxel map. BACKGROUND

[0002] In the field of unmanned aerial vehicle autonomous navigation, multi-sensor fusion technology is the core of realizing high-precision positioning. A general visual-inertial-lidar fusion odometer is composed of a monocular camera, an inertial measurement unit (IMU) and a laser radar. The IMU provides high-frequency motion estimation, the visual sensor provides texture information, and the laser radar provides accurate three-dimensional geometric structure. The three are complementary and can realize robust positioning and mapping in complex environments.

[0003] In recent years, visual-inertial-lidar fusion technology has gradually increased in the field of unmanned aerial vehicle navigation research, which can be divided into two types: tight coupling and loose coupling. Tight coupling algorithms are superior to loose coupling algorithms due to their deeper data fusion, of which FASTLIVO2 and R3LIVO++ are the most typical. Both of these two methods are based on direct method for pose optimization, without feature extraction, low in calculation amount, and superior to other tight coupling algorithms in terms of precision and real-time performance. However, with the increasing requirement for environmental perception capability, the multi-camera scheme has become a trend in engineering practice due to its wider field of view and richer information.

[0004] When the above fusion odometer based on direct method is expanded to a multi-camera architecture, the following problems exist: when directly using point cloud projection to realize association with images, the projection calculation amount increases by a multiple with the number of cameras, which is difficult to meet the real-time requirements of high-frequency positioning of unmanned aerial vehicles, and the calculation efficiency is low; the existing projection matching does not fully consider the observation quality of the multi-camera overlapping area. The observation quality of the projection points located at the edge of the camera field of view or in the area with large distortion is poor, and if they are directly involved in solving, the system accuracy, robustness and scene adaptability will be reduced.

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

[0006] The purpose of the present application is to provide a visual-inertial-laser fusion positioning method and system based on a virtual voxel map, which solves the problems of low calculation efficiency and lack of observation quality screening mechanism when the existing technology is expanded to a multi-camera architecture by introducing a virtual voxel map and a projection scoring strategy.

[0007] Technical scheme: The visual-inertial-laser fusion positioning method based on a virtual voxel map comprises:

[0008] Step S1: Obtain the IMU data, laser radar point cloud data and multi-view image data collected by the inertial measurement unit, laser radar and multi-view camera; use the IMU data to obtain the carrier pose state by recursive operation, and align the laser radar point cloud data to the frame time of the multi-view image data after preprocessing to obtain the laser frame point cloud;

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

[0010] Step S3: Construct a virtual voxel map to establish a mapping relationship between the three-dimensional space voxel and the camera visibility attribute; for the scanning points in the laser frame point cloud, according to the registration state of the scanning points in the virtual voxel map, perform a search or calculation operation: if the voxel to which the point belongs has registered the camera attribute, directly read the attribute as the visibility mark of the scanning point; if not, calculate the projection score of the scanning point under each camera according to the point cloud projection score strategy, select the camera attribute corresponding to the highest score and register it to the virtual voxel map, and mark the visibility of the scanning point;

[0011] Step S4: Project the laser point with the visibility mark to the pixel coordinate system of the camera corresponding to the mark to construct a visual error model and perform visual measurement update to output the final carrier pose state.

[0012] Further, in step S1, the specific method of de-distortion processing and time alignment of the laser radar point cloud data is as follows:

[0013] The acceleration and angular velocity of the carrier are obtained by using the IMU data, and the transfer matrix of the IMU coordinate system from time t0to time t1is calculated.

[0014] Based on the known transfer matrix between the fixed radar coordinate system and the IMU coordinate system, the laser point collected at time t0is converted to the radar coordinate system at time t1by the following formula to complete the de-distortion:

[0015]

[0016]

[0017] ​​​​​​​​​Further, in step S2, constructing the laser radar local point cloud map comprises: employing a hash table combined with an octree data structure to manage the laser radar local point cloud map; dividing a three-dimensional space into fixed-size root voxels as index units of the hash table, and nesting octree nodes in the root voxels to recursively divide the space into leaf voxels; the leaf voxels are used to store plane features, including a plane normal vector and a plane center point; when a laser scanning point falls into an existing leaf voxel, the point is used to re-fit a plane in the voxel; if a change in plane parameters after fitting is less than a preset threshold or a plane fitting frequency reaches a preset upper limit, the leaf voxel is marked as a mature voxel, and the plane parameters of the leaf voxel are not updated subsequently.

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

[0019] The specific process of obtaining the highest-score camera attribute according to the point cloud projection score strategy is as follows: projecting scanning points in an unregistered voxel to imaging planes of each camera of a multi-camera; if a scanning point meets an invisible condition in a certain camera, the projection score of the camera is zero or an invalid value; if a scanning point meets a visible condition in a certain camera, the projection score is calculated according to a degree to which a projection position of the scanning point in a camera coordinate system deviates from a camera optical center, and the closer the projection position is to the camera optical center or the field of view center, the higher the projection score is; comparing the projection scores of the cameras, and selecting a camera ID with the highest total score as the camera attribute of the scanning point; if all the cameras are invisible, the camera attribute of the voxel in the virtual voxel map is registered as zero.

[0020] Further, the projection score calculation method comprises:

[0021] calculating three-dimensional coordinates of the scanning point in each camera coordinate system;

[0022] calculating a horizontal included angle of the scanning point in the camera coordinate system according to the three-dimensional coordinates and a vertical included angle .

[0023] The projection score is calculated by using the following formula:

[0024]

[0025]

[0026] wherein, and are a horizontal field of view angle and a vertical field of view angle of the camera respectively, and are the projection scores in the horizontal direction and the vertical direction respectively.

[0027] If the calculated projection score If the calculated projection score meets the preset threshold condition, the camera with the highest score is determined as the best projection camera, otherwise the scanning point is marked as invisible.

[0028] Further, in step S4, before constructing the visual error model, a feature point screening step is further included: judging 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 point belonging to the mature voxel is projected onto the camera image corresponding to the visibility mark as a feature point to participate in the construction and update of the visual error model.

[0029] Further, in step S3, before constructing the virtual voxel map, a visual key frame selection step is further included: judging whether the point-plane residual mean of the radar measurement equation in step S2 is out of limit, if it is out of limit, skipping the key frame selection and subsequent visual measurement update; if it is not out of limit, judging whether the position deviation between the current camera frame and the last key frame is greater than a preset distance, or whether the rotation angle deviation between the current camera frame and the last key frame is greater than a preset angle; if any of the above conditions is met, the current frame is determined as a key frame.

[0030] Further, the visual measurement update in step S4 can adopt a direct method (based on a photometric error model) or a semi-direct method (based on a re-projection error model);

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

[0032] If the semi-direct method is adopted, based on the optical flow tracking and feature point coordinate triangulation, the re-projection error model is constructed by calculating the deviation between the observed pixel coordinates and the re-projected coordinates of the feature point in the current frame, and the multi-state constraint Kalman filter MSCKF is used to perform state update.

[0033] Further, the present application also relates to a multi-view visual-inertial-lidar fusion positioning system, comprising an inertial measurement unit, a lidar, a multi-view camera, a memory and a processor, wherein the processor is configured to execute the above method.

[0034] Advantages: Compared with the prior art, the present application has the following advantages:

[0035] The virtual voxel map is innovatively proposed, and the three-dimensional space voxel is bound with the best observation camera ID by using the spatial consistency principle. For the point cloud of the registered area, the tedious geometric projection calculation does not need to be repeatedly performed, and the corresponding camera can be obtained only by looking up the table, so that the CPU occupancy rate in the multi-view system is greatly reduced, and the algorithm can be efficiently run on the resource-limited unmanned aerial vehicle embedded platform.

[0036] By introducing the projection scoring strategy based on the field of view center deviation degree, the system can automatically remove the laser points located at the edge of the camera field of view, the large distortion or the poor observation angle, and only the best observation data with the highest score is reserved to participate in the visual update, which effectively overcomes the positioning drift problem caused by the poor projection quality of the traditional method, and improves the quality of the feature points.

[0037] The laser radar has about 20w points per frame, and the general method needs to perform 20w*n (the number of cameras) times of projection calculation per frame; the lightweight multi-view vision-inertial-laser radar fusion positioning method based on the virtual voxel map proposed in the application stores the voxel position attributes in the radar coordinate system, and in the whole program running stage, the points at the same voxel position only need to be projected once, including n (the number of cameras) projection calculations, and the voxel positions that have been calculated do not need to be projected again, therefore, with the increase of the registered positions in the virtual voxel map, the projection amount gradually decreases over time, and the method has good expansibility and can be combined with various visual and laser radar measurement models. BRIEF DESCRIPTION OF DRAWINGS

[0038] Figure 1 The figure is the overall flowchart of the visual-inertial-laser fusion positioning method based on the virtual voxel map in the embodiment of the application;

[0039] Figure 2 The figure is the detailed step logic diagram of the method in the embodiment of the application;

[0040] Figure 3 The figure is the key frame selection strategy flowchart in the embodiment of the application;

[0041] Figure 4 The figure is the specific flowchart of constructing the virtual voxel map and the point cloud visibility marker in the embodiment of the application. DETAILED DESCRIPTION

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

[0043] Embodiment 1: As shown in Figure 1 and Figure 2 The visual-inertial-laser fusion positioning method based on the virtual voxel map proposed in the embodiment mainly includes four core steps:

[0044] Step 1, collect multi-sensor data, perform recursive operation on the acquired IMU and angular velocity to obtain the carrier pose state, perform de-distortion, down-sampling and other processing on the radar scan points, and align them to the image frame time obtained 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 according to the searched plane features to construct the measurement equation, and update the carrier pose state;

[0046] Step 3, if the camera frame meets the key frame condition, down-sample the coordinate positions of the point cloud in the laser frame to construct a virtual voxel map, obtain the highest score camera attribute of each down-sampled point according to the point cloud score projection strategy, register the attribute and the voxel coordinates of the corresponding down-sampled point to the virtual voxel map managed by the hash table, and complete the point cloud visibility marking in the registered voxel region through the virtual voxel map;

[0047] Step 4, use the laser radar measurement to update the pose and the point cloud data with camera visibility marking, project the point cloud onto the pixel coordinate system of the marked camera to add visual features to the point cloud data, execute visual update through the construction of photometric or re-projection error model, and output the final pose state of the carrier.

[0048] Step 1: Data acquisition and processing

[0049] The system collects the data of IMU, laser radar and multi-camera. The high-frequency data (acceleration and angular velocity) of IMU is used for state recursion to obtain the predicted pose at the current time. Since there is motion distortion in the laser radar acquisition process, the laser scan points need to be aligned to the camera image frame acquisition time . Specifically, according to equation (1), the laser points collected at are converted to the radar coordinate system at time, and de-distortion processing is completed.

[0050] (1)

[0051] Step 2: Laser radar measurement update

[0052] 1) Local point cloud map construction

[0053] ​The application is compatible with various point cloud storage structures, covering binary tree, octree, kd-tree, etc. In order to achieve efficient spatial query of point cloud in the process of constructing a measurement model, a radar point cloud voxel map is managed based on a hash table and an octree structure, and the specific implementation manner is as follows: a three-dimensional space is divided into root voxels with a fixed size as basic index units of the hash table. The scanning points in the radar coordinate system are projected into the world coordinate system based on the pose after the radar update, the root voxel space coordinates of the scanning points are inquired to obtain the octree nodes nested in the root voxel, the scanning points are inserted into the octree nodes, and the space is recursively divided according to the octree rules. The smallest voxel corresponding to the leaf node of the octree is a leaf voxel, which is used to store plane features, including a plane normal vector and a plane center point. When registering each frame of point cloud, if the scanning points fall into an existing leaf voxel, the plane in the voxel is refitted, when the plane parameter changes less than a threshold or the plane fitting times reach an upper limit (every time a new point comes, 10 points in the new points are reserved for fitting a plane, the included angle between the plane normal vector obtained by fitting and the original plane normal vector in the voxel is calculated by using a vector dot product formula, and if the included angle is less than 0.5° or the fitting times are greater than 10, the leaf voxel is marked as a mature voxel, and the plane parameter is no longer updated. In order to avoid memory expansion, only the root voxels within a distance L from the radar sensor position are reserved in the map.

[0054] 2) Radar measurement model construction and update

[0055] The lightweight multi-view vision-inertial-laser radar fusion positioning method constructed by the application supports various radar measurement models and vision measurement models, the radar measurement equation can be constructed based on point-plane residual, the optimal state solution is obtained through IESKF (iterative extended filter estimation), or the radar point cloud plane residual measurement equation is constructed and the optimal solution is obtained based on MSCKF (multi-state filter estimation). Taking the point-plane residual combined with IESKF as an example, the specific steps of radar update are as follows:

[0056] First, the laser frame scanning point coordinates are converted into the world coordinate system through 、 as shown in formula (2):

[0057] (2)

[0058] In the formula, represents the position coordinates of the point cloud in the radar coordinate system at time k, represents the transfer matrix of the laser to the IMU coordinate system, which is a fixed known matrix; represents the transfer matrix of the IMU coordinate system to the global world coordinate system, which can be obtained by pose recursion of the IMU. After all the above are known, the position of the current laser frame scanning point in the world coordinate system can be obtained.

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

[0060] (3)

[0061] In the formula, represents the point-plane residual corresponding to the i-th laser scanning point, and are the center point and plane normal vector stored in the leaf voxel corresponding to the scanning point, respectively. According to the above formula, the point-plane residual is constructed for the scanning point successfully matched to the plane, and the transformation matrix from the IMU coordinate system to the global world coordinate system is estimated based on the IESKF filter . and the pose state of the radar and camera sensor in the world coordinate system and is updated.

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

[0063] 1) Visual key frame selection

[0064] As shown in Figure 3 , after the laser radar update is completed at the current time, the visual key frame selection is performed. First, according to the constructed laser radar measurement model, it is judged whether the point-plane residual average is out of limit, if it is out of limit, the key frame selection is skipped, and the pre-processing operations such as de-warping and pixel down-sampling are performed and output. If it is not out of limit, it is judged whether the camera frame satisfies the key frame condition shown in equation (4):

[0065]

[0066] (4)

[0067] Among them, is the position of the camera at the current time , is the camera position at the last key frame time , and the deviation between the two is greater than the preset distance to satisfy the key frame condition; is the camera pose at the current time, is the camera pose at the last key frame time, and the rotation angle between the two is greater than the preset angle to satisfy the key frame condition; the pre-processing operation is performed on the key frame image that satisfies the condition and output, and if it is the first frame image, it is also regarded as a key frame. 2) Virtual voxel map construction and point cloud visibility marking

[0068]

[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 respectively, n represents the total number of cameras. If the above conditions are not met, the total projection score is selected The highest camera is taken as the laser point camera attribute, the visibility label of the point cloud is completed, and the camera attribute of the point is registered in the corresponding voxel position in the virtual voxel map.

[0078] Step 4: Visual measurement update

[0079] The present application supports multiple visual update methods, which can complete visual pose state update by constructing photometric error equation through direct method or constructing re-projection error equation through semi-direct method, and flexibly adapt to different scene requirements.

[0080] 1) Projection and feature point retrieval

[0081] Before constructing the visual measurement model, the projection and feature point retrieval steps are required for both direct method and semi-direct method, that is, the mature voxel plane center point in the local point cloud map is projected onto the pixel coordinate system of the current frame image, and the feature points are selected from it according to the specific retrieval condition. Specifically, it can be divided into the following steps:

[0082] Firstly, the voxel to which the visibility-labeled point belongs in the local point cloud map is calculated; it is judged whether the voxel is a mature voxel, if it is a mature voxel, the plane center point thereof is projected onto the pixel coordinate system of the camera to which the visibility-labeled point belongs according to formula (8);

[0083] (8)

[0084] wherein, represents the pixel coordinate; represents the depth value; represents the world coordinate of the plane center point stored in the voxel; and respectively represent the rotation and translation matrix of the camera coordinate system of the camera to which the visibility-labeled point belongs relative to the world coordinate system, which can be obtained by decomposing the camera sensor state updated by the radar; and represent the focal length of the camera to which the visibility-labeled point belongs in the x-axis and y-axis directions. The visibility-labeled points projected onto the pixel coordinate system are selected according to the conditions such as gray size and projection depth value, and the selected points are taken as feature points to participate in the subsequent visual update process.

[0085] 2) Visual measurement model construction and update

[0086] a. Direct method photometric error construction

[0087] The direct photometric error model is constructed based on the feature point photometric consistency assumption. The feature point storage information includes the feature point's position in the world coordinate system and the image frame information observed at that feature point. The 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 also be continuously updated. After the update count reaches the upper limit, the reference image patch is fixed, and the feature point only retains the reference frame information, deleting other image frame information.

[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] wherein, is the camera projection model; The updated camera pose is known for the lidar measurement. By constructing the reprojection error model shown in equation (10), visual measurement updates can be performed based on the MSCKF to estimate the multi-image frame poses within the sliding window , and according to the known and , the pose states of the lidar sensor and the IMU are updated.

[0095] Embodiment 2: The embodiment provides a multi-view vision-inertial-lidar fusion positioning system for implementing the above method. The hardware part includes:

[0096] Inertial measurement unit (IMU): collecting three-axis acceleration and angular velocity;

[0097] Lidar: collecting environmental point clouds;

[0098] Multi-view camera: at least containing two cameras (such as forward + downward, or ring view combination), for collecting environmental images;

[0099] Processor and memory: the memory stores a computer program, and the processor executes the program to realize each step in embodiment 1.

[0100] Those skilled in the art should understand that the present application realizes the optimization of "space for time" through a virtual voxel map, and guarantees the quality of multi-view fusion through projection scoring, and is not limited to a specific hardware model or parameter setting. Any improvement and modification based on the technical solutions of the present application should fall within the protection scope of the present application without departing from the principles of the present application.

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 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; 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; 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.

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: 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 ; 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 moment: ， 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 S2, constructing the local point cloud map of the LiDAR includes: A hash table combined with an octree data structure is used to manage the local point cloud map of LiDAR; the three-dimensional space is divided into root voxels of fixed size as the index unit of the hash table, and octree nodes are nested within the root voxels to recursively divide the space to 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 plane parameters is less than a preset threshold or the number of plane fitting attempts reaches a preset upper limit, the leaf voxel is marked as a mature voxel, and the plane parameters of the leaf voxel will not be updated thereafter.

4. The visual-inertial-laser fusion positioning method based on virtual voxel maps according to claim 1, characterized in that, 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; The specific process for obtaining the highest-scoring camera attribute based on the point cloud projection score strategy is as follows: The scan points within the unregistered voxels are projected onto the imaging planes of each camera in the multi-view camera. If a scan point is invisible in a camera, then the projection score of that camera is zero or an invalid value. If the scanning point meets the visibility condition in a certain camera, the projection score is calculated based on the degree to which the projection position of the scanning point in the camera coordinate system deviates from the camera optical center. The closer the projection position is to the camera optical center or the center of the field of view, the higher the projection score. Compare the projection scores of each camera and select the camera ID with the highest total score 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.

5. The visual-inertial-laser fusion positioning method based on virtual voxel maps according to claim 4, characterized in that, The projection score calculation method includes: Calculate the three-dimensional coordinates of the scan points in each camera coordinate system; Calculate the horizontal angle of the scanning point in the camera coordinate system based on the three-dimensional coordinates. Angle with vertical ; Calculate the projected score 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 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.

6. The visual-inertial-laser fusion positioning method based on virtual voxel maps according to claim 3, characterized in that, In step S4, before constructing the visual error model, a feature point selection step is also included: Determine whether the voxel to which the laser point with visual marker 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 projected onto the camera image corresponding to their visual markers to participate in the construction and updating of the visual error model.

7. 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 of 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 limit is not exceeded, determine whether the positional deviation between the current camera frame and the previous keyframe is greater than a preset distance, or whether the rotational angle deviation between the current camera frame and the previous keyframe is greater than a preset angle; if either of the above conditions is met, the current frame is determined to be a keyframe.

8. 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.

9. The visual-inertial-laser fusion positioning method based on virtual voxel maps according to claim 8, 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.

10. 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 9.

Images