Multi-source fusion navigation method and system based on voxel map association and ground constraint

Abstract

This disclosure provides a multi-source fusion navigation method and system based on voxel map association and ground constraints, relating to the field of urban navigation and positioning technology. The method includes: constructing a three-dimensional voxel grid map; associating planar feature points of the current frame with the voxel grid map; extracting voxel factor residuals and planar feature registration residuals; using point cloud segmentation technology to extract ground planar points, constructing ground constraints, and calculating ground constraint residuals; calculating IMU pre-integration residuals based on inertial navigation observation data; introducing a reference-station-free GNSS pseudorange and Doppler observation constraint mechanism to construct the original observation residuals; and fusing the voxel factor residuals, planar feature registration residuals, ground constraint residuals, IMU pre-integration residuals, pseudorange single-point positioning residuals, and Doppler residual constraints into a factor map optimizer for solution, outputting the navigation and positioning results. This disclosure achieves high-precision and high-reliability navigation and positioning in complex urban environments.

Description

Technical Field

[0001] This disclosure relates to the field of urban navigation and positioning technology, specifically to a multi-source fusion navigation method and system based on voxel map association and ground constraints. Background Technology

[0002] The statements in this section are merely background information relating to this disclosure and do not necessarily constitute prior art.

[0003] In complex urban environments, robust and reliable navigation and positioning services are the prerequisite and foundation for achieving autonomous driving, and navigation technology based on a single sensor is difficult to meet this requirement.

[0004] In multi-source fusion navigation, INS (Inertial Navigation System), as an autonomous relative positioning system, calculates the vehicle's pose by interpreting acceleration and angular velocity information collected by IMU (Inertial Measurement Unit). It features independence from external signals, high output frequency, and continuous stability. However, due to the cumulative effect of inherent sensor errors, the positioning accuracy of INS diverges rapidly over time; therefore, in practical applications, it usually needs to be used in combination with other navigation sensors. In simultaneous localization and mapping (SLAM) tasks, LiDAR (Light Detection and Ranging) offers high-precision ranging and excellent environmental perception capabilities. It achieves relative positioning by acquiring point cloud data of the surrounding environment and simultaneously constructs a two-dimensional or three-dimensional point cloud map of the environment. However, position errors accumulate over time, especially in the elevation direction. Due to insufficient observation information and limited constraints, accurate and reliable positioning results are difficult to obtain. GNSS (Global Navigation Satellite System) can provide high-precision global position information, but the signal is easily affected by blockage and multipath effects, which may lead to positioning interruption or large errors, thus limiting its reliability and applicability in complex scenarios.

[0005] Therefore, in order to improve the robustness and reliability of navigation systems, multi-source sensor fusion-based navigation algorithms have been widely studied and applied.

[0006] In existing technologies, dead reckoning or absolute position can be achieved using a single sensor, but this approach is relatively vulnerable in complex urban environments. A multi-source fusion navigation framework, LIO-SAM, based on factor graph optimization, is proposed, combining IMU pre-integration with LiDAR point cloud matching to achieve high-precision, low-drift long-term navigation and mapping, and can optionally incorporate GNSS positioning results. Alternatively, an Extended Kalman Filter (IEKF) framework is used to optimize the LIO fusion strategy, improving the system's robustness in complex environments. The recently proposed FAST-LIO2 method directly uses raw point cloud data and introduces an incremental kd-tree (ikd-Tree) for more efficient point cloud management and mapping. Alternatively, a tightly coupled GNSS / INS / LiDAR fusion framework, through a sliding window planar feature tracking method, effectively integrates the complementary characteristics of multiple sensors, achieving stable and reliable motion estimation in urban environments with limited satellite signals. Alternatively, a tightly coupled GNSSRTK / INS / LiDAR system (FGO-GIL) based on a factor graph optimization framework can be proposed. This system employs a keyframe nonlinear optimization scheme, achieving inter-frame optimization through pre-integration of non-keyframes and inertial measurement units, and utilizing sparse keyframes to construct LiDAR factors for sliding window optimization, effectively improving navigation performance in urban environments. Another approach is a tightly coupled PPP / INS / Vision / LiDAR fusion method, which directly fuses multi-source heterogeneous data at the observation layer using an extended Kalman filter, achieving high-precision, continuous, and reliable navigation and positioning in complex urban environments. This scheme effectively improves the accuracy of velocity and attitude parameter calculations through complementary fusion of visual sparse landmarks and LiDAR feature information. Finally, in the LiDAR-IMU navigation framework, vehicle attitude estimation is optimized through ground point extraction; however, existing methods still have the following limitations:

[0007] (1) A single LiDAR sensor acquires the point cloud of the surrounding environment by scanning, extracts feature points and accumulates a number of key frame feature points to construct a local or global map, and realizes the recursive calculation of the carrier's position and attitude through feature point registration. This method is highly dependent on environmental features, and it is difficult to extract effective feature points in unstructured environments. Furthermore, the system has cumulative drift, and the error gradually increases with running time and distance.

[0008] (2) The method of GNSS navigation and positioning is to calculate the absolute position of the carrier based on the principle of spatial distance intersection by measuring the pseudorange and carrier information between the receiver and the satellite. This method is susceptible to various error sources and interference, and the positioning accuracy will be greatly reduced in urban environments with severe signal blockage.

[0009] (3) Using a combination of GNSS and IMU to provide navigation services for vehicles, by fusing the high-precision absolute positioning information of GNSS with the high-frequency dynamic characteristics of IMU, GNSS information is used to correct IMU drift, and IMU data is used to make up for the defects of low GNSS update rate and signal interruption, thereby outputting a stable and reliable continuous pose; although this method can improve the continuity and accuracy of positioning, when GNSS is blocked and interfered with for a long time, the positioning error will continue to increase due to the inherent drift of IMU.

[0010] (4) By tightly coupling and fusing the point cloud matching results of LiDAR with the high-frequency motion prediction of IMU, the synchronous localization and mapping of the vehicle can be realized. The IMU is used to provide motion distortion correction and priors for LiDAR, and the high-precision observation of LiDAR is used to correct the cumulative drift of IMU, thereby calculating the smooth and drift-free six-degree-of-freedom pose of the vehicle in real time and constructing a map. This method can perceive the surrounding environment, but the point cloud generated in the complex environment will pollute the map and interfere with the matching, resulting in inaccurate localization.

[0011] (5) The combination of LiDAR / INS / GNSS provides continuous pose for the carrier, with INS as the core of high-frequency motion prediction, and the absolute position of GNSS and the relative geometric observation of LiDAR are introduced as dual correction constraints. Although this method can provide continuous and reliable navigation services in most scenarios, in practical applications, existing methods still cannot avoid large accumulation of position errors in the elevation direction;

[0012] (6) Although existing research can provide navigation services for carriers in urban environments, there are still problems such as unstable navigation results, inaccurate LiDAR registration leading to increased positioning errors, insufficient observation information in the elevation direction leading to limited constraints, and difficulty in obtaining reliable navigation services. Summary of the Invention

[0013] To address the aforementioned issues, this disclosure proposes a multi-source fusion navigation method and system based on voxel map association and ground constraints. It employs a LiDAR-constrained modeling approach combining real-time voxel grid association and local plane registration. Stable statistical features are extracted from voxel cells and combined with plane points in the current frame to construct registration residuals. Ground constraints are constructed based on ground point segmentation to suppress vertical error accumulation. Simultaneously, GNSS pseudorange observations and Doppler frequency shift information are integrated to provide the carrier with absolute position and velocity constraints from the original observation layer, achieving high-precision and high-reliability navigation and positioning in complex urban environments.

[0014] According to some embodiments, the present disclosure adopts the following technical solutions:

[0015] Multi-source fusion navigation methods based on voxel map association and ground constraints include:

[0016] Acquire real-time raw point cloud data, inertial navigation observation data, and GNSS observation data, and construct a three-dimensional voxel grid map based on the point cloud data;

[0017] Extract planar feature points from the current frame, associate the planar feature points of the current frame with the voxel grid map, extract voxel factor residuals, and calculate planar feature registration residuals;

[0018] Point cloud segmentation technology is used to extract ground plane points. Ground constraints are constructed by fitting ground parameters and the property that the Euclidean distance between the LiDAR sensor and the ground remains unchanged. Ground constraint residuals are then calculated.

[0019] The IMU pre-integration residual is calculated based on inertial navigation observation data. A constraint mechanism between GNSS pseudorange observations and Doppler observations without a reference station is introduced to construct the original observation residual.

[0020] The voxel factor residual, planar feature registration residual, ground constraint residual, IMU pre-integration residual, pseudorange single-point positioning residual, and Doppler residual constraint are fused together. A unified optimization solution framework under a sliding window is used to jointly estimate the fused residuals and calculate the navigation and positioning results.

[0021] According to some embodiments, the present disclosure adopts the following technical solutions:

[0022] A multi-source fusion navigation system based on voxel map association and ground constraints includes:

[0023] The voxel grid construction module is used to acquire real-time raw point cloud data, inertial navigation observation data and GNSS observation data, and to construct a three-dimensional voxel grid map based on the point cloud data;

[0024] The registration residual calculation module is used to extract planar feature points of the current frame, associate the planar feature points of the current frame with the voxel grid map, extract voxel factor residuals, and calculate planar feature registration residuals.

[0025] The constraint residual construction module is used to extract ground plane points using point cloud segmentation technology, construct ground constraints by fitting ground parameters and the property that the Euclidean distance of the LiDAR sensor relative to the ground is invariant, and calculate the ground constraint residual; the IMU pre-integration residual is calculated based on inertial navigation observation data, and a constraint mechanism for GNSS pseudorange observations and Doppler observations without reference stations is introduced to construct the original observation residual;

[0026] The fusion solution and positioning module is used to fuse voxel factor residuals, planar feature registration residuals, ground constraint residuals, IMU pre-integration residuals, pseudorange single-point positioning residuals, and Doppler residual constraints. A unified optimization solution framework under a sliding window is used to jointly estimate the fused residuals and calculate the navigation and positioning results.

[0027] According to some embodiments, the present disclosure adopts the following technical solutions:

[0028] A computer program product includes a computer program that, when executed by a processor, implements the multi-source fusion navigation method based on voxel map association and ground constraints.

[0029] According to some embodiments, the present disclosure adopts the following technical solutions:

[0030] A non-transitory computer-readable storage medium is provided for storing computer instructions, which, when executed by a processor, implement the multi-source fusion navigation method based on voxel map association and ground constraints.

[0031] According to some embodiments, the present disclosure adopts the following technical solutions:

[0032] An electronic device includes a processor, a memory, and a computer program; wherein the processor is connected to the memory, the computer program is stored in the memory, and when the electronic device is running, the processor executes the computer program stored in the memory to enable the electronic device to perform the multi-source fusion navigation method based on voxel map association and ground constraints.

[0033] Compared with the prior art, the beneficial effects of this disclosure are as follows:

[0034] This disclosure presents a multi-source fusion navigation method based on voxel map association and ground constraints. The method employs a long-term voxel grid model, extracts stable center point features from the feature point cloud within voxel cells, and constructs an accurate point cloud registration model by combining planar features. Ground constraints are constructed based on ground point segmentation to suppress vertical error accumulation. Simultaneously, GNSS pseudorange observations and Doppler frequency shift information are fused to provide reliable absolute position and velocity constraints for the carrier.

[0035] This disclosed multi-source fusion navigation method, based on voxel map association and ground constraints, employs multipath effect analysis to filter GNSS data for varying observation quality, and dynamically adjusts data reliability using satellite elevation angle and signal-to-noise ratio. Furthermore, it utilizes an IMU to calculate IMU pre-integration residuals and recursively assigns initial values ​​to the state to be estimated. A factor graph framework is used for fusion navigation calculations of the above sensors, and a sliding window is employed to improve computational efficiency. Through multi-level information fusion and constraint optimization, this method reduces navigation and positioning errors, achieving high-precision and high-reliability navigation and positioning in complex urban environments. Attached Figure Description

[0036] The accompanying drawings, which form part of this disclosure, are used to provide a further understanding of this disclosure. The illustrative embodiments of this disclosure and their descriptions are used to explain this disclosure and do not constitute an undue limitation of this disclosure.

[0037] Figure 1 This is a schematic diagram of the multi-source fusion navigation method based on voxel map association and ground constraints according to an embodiment of the present disclosure;

[0038] Figure 2 This is a LiDAR ground point segmentation map according to an embodiment of this disclosure;

[0039] Figure 3 This is a schematic diagram of voxel map construction and matching in an embodiment of this disclosure. Detailed Implementation

[0040] The present disclosure will be further described below with reference to the accompanying drawings and embodiments.

[0041] It should be noted that the following detailed descriptions are illustrative and intended to provide further explanation of this disclosure. Unless otherwise specified, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this disclosure pertains.

[0042] It should be noted that the terminology used herein is for the purpose of describing particular embodiments only and is not intended to limit the exemplary embodiments according to this disclosure. As used herein, the singular form is intended to include the plural form as well, unless the context clearly indicates otherwise. Furthermore, it should be understood that when the terms “comprising” and / or “including” are used in this specification, they indicate the presence of features, steps, operations, devices, components, and / or combinations thereof.

[0043] Example 1

[0044] One embodiment of this disclosure provides a multi-source fusion navigation method based on voxel map association and ground constraints, the method steps of which include:

[0045] Step 1: Acquire real-time raw point cloud data, inertial navigation observation data, and GNSS observation data, and construct a three-dimensional voxel grid map based on the point cloud data;

[0046] Step 2: Extract planar feature points of the current frame, associate the planar feature points of the current frame with the voxel grid map, extract voxel factor residuals, and calculate planar feature registration residuals;

[0047] Step 3: Using point cloud segmentation technology, extract ground plane points, construct ground constraints by fitting ground parameters and the property that the Euclidean distance between the LiDAR sensor and the ground remains unchanged, and calculate the ground constraint residuals;

[0048] Step 4: Calculate the IMU pre-integration residual based on inertial navigation observation data, introduce a constraint mechanism between GNSS pseudorange observations and Doppler observations without a reference station, and construct the original observation residual;

[0049] Step 5: The voxel factor residual, planar feature registration residual, ground constraint residual, IMU pre-integration residual, pseudorange single-point positioning residual, and Doppler residual constraint are fused. A unified optimization solution framework under a sliding window is used to jointly estimate the fused residuals and calculate the navigation and positioning results.

[0050] As one embodiment, this disclosure presents a multi-source fusion navigation method based on voxel map association and ground constraints. This method constructs a real-time 3D voxel grid based on LiDAR scanning and establishes associations between current frame planar points and voxel statistical geometric features to calculate registration residuals. Simultaneously, it combines planar constraints from local maps to complete registration. Point cloud segmentation technology is used to extract ground planar points. Ground constraints are constructed by fitting ground parameters and leveraging the invariant Euclidean distance between the LiDAR sensor and the ground, and ground constraint residuals are calculated. To reduce system divergence, a constraint scheme using GNSS pseudorange and Doppler observations without a reference station is introduced to directly construct the original observation residuals, rather than relying on GNSS odometry for pose fusion. IMU pre-integration residuals are calculated using an IMU, and initial values ​​are assigned to the state to be estimated through recursion. A unified optimization solution framework under a sliding window is used to jointly estimate the above residuals, improving computational efficiency. For GNSS observations with varying quality, multipath effects are used for data quality checks to filter GNSS data, and data reliability is dynamically adjusted based on satellite elevation angle and signal-to-noise ratio. The LiDAR front-end of this disclosed method focuses on the construction of voxel statistical geometric constraints, local plane constraints, and ground constraints. The specific implementation process is as follows:

[0051] Step 1: Acquire real-time raw point cloud data, inertial navigation observation data, and GNSS observation data, and construct a three-dimensional voxel grid map based on the point cloud data;

[0052] Specifically, the system is in a state of estimation, acquiring real-time raw point cloud data, inertial navigation observation data, and GNSS observation data, including the carrier position, attitude, velocity gyroscope bias, and 15-dimensional state vector of acceleration bias:

[0053]

[0054]

[0055] in, Let k be the state vector, and k be a frame in the sliding window. , , For the position, attitude quaternions, and velocity of the keyframes in the East-North-Sky coordinate system (M-frame), and The zero bias of the IMU's accelerometer and gyroscope on-board system (B system). n The GNSS epoch number within the window. For the first i The receiver clock bias of each epoch.

[0056] Step 2: Extract planar feature points of the current frame, associate the planar feature points of the current frame with the voxel grid map, extract voxel factor residuals, and calculate planar feature registration residuals;

[0057] Specifically, when a frame of LiDAR planar point cloud is received First, the pose of the current frame is transformed to the M-frame. Then, hash coordinates are calculated based on the 3D coordinates of the point, and the voxel mesh index is further calculated. This voxel mesh is used for real-time correlation modeling between the current frame and existing voxel statistical features, focusing on geometric residual construction. Then, the existence of the index is checked in the existing mesh: if it does not exist, a new mesh is created and the point is stored therein; if it already exists, the point is directly stored in the corresponding mesh. For each voxel... voxel Point cloud quantity, center point coordinates Covariance eigenvalues and eigenvectors Characterizing its geometric features:

[0058]

[0059] in, voxels Planar points in the intrinsic MAP system for A point inside, when At that point, no new points will be stored for that voxel, and the corresponding attributes will no longer be updated.

[0060] For the current frame point cloud a point in First, check if the voxel mesh containing it has been built, and the following conditions must be met before it can be added. :

[0061]

[0062]

[0063]

[0064] in, For multiplication of quaternions and vectors, This is the distance threshold. After satisfying the above conditions, the voxel factor residuals are constructed:

[0065]

[0066] Furthermore, the process of constructing planar feature factors and calculating planar feature registration residuals includes:

[0067] Planar point cloud Except Remaining point cloud outside Used to construct LiDAR planar feature factors. Before registration, a local map is first constructed:

[0068] By using the pose of keyframes, the point cloud in the LiDAR system is transformed into the map coordinate system, and the point cloud of several keyframes is accumulated to form a local map.

[0069] Furthermore, the current frame point cloud will be used for planar feature matching. Convert to map coordinate system to obtain And register it with the local map. Use KD-Tree to search for points in the local map that match the current frame. For several nearby points, fit the plane containing these nearby points and extract the plane parameters, including the normal vector N and plane parameters D. Calculate the residuals. :

[0070]

[0071] in, It is based on the pose of the current frame. , [Points under LiDAR system] The result is transformed to a map coordinate system. Weights are calculated based on the residuals. The smaller the residual, the greater the weight. Points with excessively small weights are not used by the system.

[0072] Step 3: Using point cloud segmentation technology, extract ground plane points. Construct ground constraints by fitting ground parameters and leveraging the property that the Euclidean distance between the LiDAR sensor and the ground remains constant. Calculate the distance residual. Specifically:

[0073] (1) Ground point extraction and segmentation. For the distortion-free LiDAR point cloud, the pitch angle of each point is calculated using its three-dimensional coordinates:

[0074]

[0075] in, These are the coordinates of the LiDAR point.

[0076] Calculate the index of the beamwidth at which the point is located, based on the number of lines and coverage angle of the LiDAR used (using a Velodyne 16-line LiDAR as an example):

[0077]

[0078] After calculating the point cloud point bundle indices, perform ground point segmentation. Discard point clouds with bundle indices greater than 8 (because bundles with indices greater than 8 correspond to pitch angles). If the value is positive, it means that the scan starts from the sensor and moves upwards; these points cannot be ground points. For example... Figure 2 As shown, for points with a harness index less than or equal to 8, calculate their actual and theoretical distances to the LiDAR sensor:

[0079]

[0080] in, This is the actual distance. For theoretical distance, This represents the radar sensor's altitude. By comparing the actual distance with the theoretical distance, possible ground points can be identified.

[0081] (2) Ground constraint factor construction. Distance filters are used to remove excessively distant ground points. Subsequently, the random sampling consistency method is used to extract ground plane features and obtain the normal vector of the ground plane. and plane parameters Simultaneously record the interior points of the fitted ground. and their quantity The weights are calculated based on the distance from each inlier to the plane, serving as the confidence level for the ground constraints. Finally, the correlation between the current frame pose and ground features is established to construct the ground constraint factors.

[0082]

[0083] Step 4: Calculate the IMU pre-integration residuals based on inertial navigation observation data, which is constructed in the same way as existing integrated navigation technology schemes. A constraint scheme using GNSS pseudorange and Doppler observations without a reference station is introduced to construct the original observation residuals, as follows:

[0084] (1) GNSS quality check. Using GNSS observations from n epochs before and after the current keyframe, calculate the multipath value for each epoch. The multipath value for a specific satellite at a given epoch is calculated using the following formula:

[0085]

[0086]

[0087] in, and These represent the multipath effect combinations obtained by combining pseudorange and carrier phase observations from the L1 / L2 and B1 / B2 bands, respectively. These are pseudorange observations. For carrier phase observations, For wavelength, , The carrier phase frequency.

[0088] Next, calculate the multipath change values ​​between the two epochs:

[0089]

[0090]

[0091] in, Based on this, the cumulative multipath effect value of the satellite over n epochs is calculated:

[0092]

[0093]

[0094] Finally, utilizing multiple satellites and The standard deviation is used to characterize the intensity of multipath effects in the current environment. When 0.35 and At 0.45, this segment of GNSS data can be used for system fusion.

[0095] (2) Pseudorange factor construction. Constraint terms are constructed using raw GNSS observation information, where pseudorange information is used for position constraints. The GNSS pseudorange observation equations are as follows:

[0096]

[0097] in, This represents the pseudorange observation value obtained by receiver r from satellite s at time t. Indicates the position of receiver r To satellite position s The geometric distance, where c is the speed of light. The signal is delayed by the ionosphere. For tropospheric delay, This is for measuring the noise term.

[0098] Based on the temporal relationship, search for the two LiDAR keyframes before and after the current pseudorange observation time t; their positions are respectively... and The corresponding time is and The receiver's position in the ENU system is obtained through interpolation. And convert it to the ECEF system:

[0099]

[0100] in, The lever arm between GNSS and LiDAR. For the anchor point's ECEF coordinate system, This is the rotation matrix calculated using the latitude and longitude of the anchor points.

[0101] The formula for calculating pseudorange residuals is as follows:

[0102]

[0103] The weight of the pseudorange factor is determined by the GNSS elevation angle and azimuth angle. The larger the elevation angle, the higher the weight, in order to reduce the contribution of low elevation angle satellite observations.

[0104] (3) Doppler factor

[0105] Doppler shift is obtained by measuring the frequency difference between the received carrier signal and the designed carrier signal; it reflects the relative motion between the receiver and the satellite along the signal propagation path. The Doppler observation equation is as follows:

[0106]

[0107] in, and The speeds of the satellite and the receiver are respectively. The carrier signal wavelength, The unit vector from the receiver to the satellite. For receiver clock drift rate, The drift rate of the satellite clock bias. Noise for Doppler measurement.

[0108] Based on the above observation equation, the Doppler residual can be expressed as:

[0109]

[0110] Step 5: Fuse the voxel factor residual, plane feature registration residual, ground constraint residual, IMU pre-integration residual, pseudorange single-point positioning residual, and Doppler residual constraint, and use a unified optimization solution framework under a sliding window to jointly estimate the fused residuals to calculate the navigation and positioning results.

[0111] Specifically, the estimated parameters are solved by maximizing the posterior probability of a given observation. Assuming that the observations are independent and the noise follows a zero-mean Gaussian distribution, a sliding window joint optimization is performed to solve for the voxel factor, LiDAR plane characteristic factor, IMU pre-integration factor, ground constraint factor, GNSS pseudorange factor, Doppler factor, and related constraint terms.

[0112]

[0113] in, Indicates marginalized items, Voxel factor, The corresponding weights; For LiDAR plane eigenvalues, For the corresponding weights; Indicates the IMU pre-integration factor. As weight; Ground constraint factor, For the corresponding weights; It is a pseudo-distance factor. As corresponding weights; Doppler factor, For the corresponding weights; It is a non-integrity constraint factor. As weight.

[0114] Example 2

[0115] One embodiment of this disclosure provides a multi-source fusion navigation system based on voxel map association and ground constraints, including:

[0116] A multi-source fusion navigation system based on voxel map association and ground constraints includes:

[0117] The voxel grid construction module is used to acquire real-time raw point cloud data, inertial navigation observation data and GNSS observation data, and to construct a three-dimensional voxel grid map based on the point cloud data;

[0118] The registration residual calculation module is used to extract planar feature points of the current frame, associate the planar feature points of the current frame with the voxel grid map, extract voxel factor residuals, and calculate planar feature registration residuals.

[0119] The constraint residual construction module is used to extract ground plane points using point cloud segmentation technology, construct ground constraints by fitting ground parameters and the property that the Euclidean distance of the LiDAR sensor relative to the ground is invariant, and calculate the ground constraint residual; the IMU pre-integration residual is calculated based on inertial navigation observation data, and a constraint mechanism for GNSS pseudorange observations and Doppler observations without reference stations is introduced to construct the original observation residual;

[0120] The fusion solution and positioning module is used to fuse voxel factor residuals, planar feature registration residuals, ground constraint residuals, IMU pre-integration residuals, pseudorange single-point positioning residuals, and Doppler residual constraints. A unified optimization solution framework under a sliding window is used to jointly estimate the fused residuals and calculate the navigation and positioning results.

[0121] Example 3

[0122] One embodiment of this disclosure provides a computer program product, including a computer program that, when executed by a processor, implements the multi-source fusion navigation method based on voxel map association and ground constraints.

[0123] Example 4

[0124] One embodiment of this disclosure provides a non-transitory computer-readable storage medium for storing computer instructions. When executed by a processor, the computer instructions implement the multi-source fusion navigation method based on voxel map association and ground constraints.

[0125] Example 5

[0126] One embodiment of this disclosure provides an electronic device, including a processor, a memory, and a computer program; wherein the processor is connected to the memory, and the computer program is stored in the memory. When the electronic device is running, the processor executes the computer program stored in the memory to enable the electronic device to perform the multi-source fusion navigation method based on voxel map association and ground constraints.

[0127] This disclosure is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of this disclosure. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, create a machine for implementing the flowchart illustrations and / or block diagrams. Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0128] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0129] While the specific embodiments of this disclosure have been described above in conjunction with the accompanying drawings, this is not intended to limit the scope of protection of this disclosure. Those skilled in the art should understand that various modifications or variations that can be made by those skilled in the art without creative effort based on the technical solutions of this disclosure are still within the scope of protection of this disclosure.

Claims

1. A multi-source fusion navigation method based on voxel map association and ground constraints, characterized in that, include: Acquire real-time raw point cloud data, inertial navigation observation data, and GNSS observation data, and construct a three-dimensional voxel grid map based on the point cloud data; The construction of a 3D voxel grid map based on point cloud data includes: When a LiDAR planar point cloud is received, it is first converted to the M-frame using the pose of the current frame. Then, the hash coordinates are calculated based on the 3D coordinates of the point cloud, and the voxel mesh index is further calculated. The index is checked in the existing mesh: if it does not exist, a new mesh is created and the point is stored in it; if it already exists, the point is directly stored in the corresponding mesh. For each voxel, its geometric features are characterized by the number of points in the voxel, the coordinates of the center point, the covariance, the eigenvalues, and the eigenvectors. Extract planar feature points from the current frame, associate the planar feature points of the current frame with the voxel grid map, extract voxel factor residuals, and calculate planar feature registration residuals; The step of extracting planar feature points of the current frame, associating the planar feature points of the current frame with a voxel grid map, and extracting voxel factor residuals includes: Before registration, a local map is first constructed. The point cloud of LiDAR is transformed into the map coordinate system by the pose of key frames, and the point cloud of several key frames is accumulated to form a local map. The current frame point cloud used for planar feature matching is transformed into the map coordinate system and registered with the local map; Using KD-Tree, search for several points near the current frame point in the local map, fit the plane where these nearby points are located, and extract the plane parameters, including the normal vector N and the plane parameters D, and calculate the voxel factor residual. Point cloud segmentation technology is used to extract ground plane points. Ground constraints are constructed by fitting ground parameters and the property that the Euclidean distance between the LiDAR sensor and the ground remains unchanged. Ground constraint residuals are then calculated. The process employs point cloud segmentation technology to extract ground plane points. Ground constraints are constructed by fitting ground parameters and leveraging the invariant Euclidean distance between the LiDAR sensor and the ground. Ground constraint residuals are then calculated, including: For the distortion-free LiDAR point cloud, the pitch angle of each point is calculated using its three-dimensional coordinates. Calculate the index of the bundle in which the point is located, based on the number of lines and coverage angle of the LiDAR used; After completing the calculation of the point cloud point bundle index, perform ground point segmentation, discard point clouds with bundle index greater than 8, and for points with bundle index less than or equal to 8, calculate their actual distance and theoretical distance to the LiDAR sensor. The random sampling consistency method is used to extract ground plane features, obtain the normal vector and plane parameters of the ground plane, and record the interior points of the fitted ground and their number. The weights are calculated based on the distance from each interior point to the plane, and these weights serve as the basis for the confidence level of the ground constraints. Finally, the correlation between the current frame pose and ground features is established, and ground constraint factors are constructed. The IMU pre-integration residual is calculated based on inertial navigation observation data. A constraint mechanism between GNSS pseudorange observations and Doppler observations without a reference station is introduced to construct the original observation residual. The voxel factor residual, planar feature registration residual, ground constraint residual, IMU pre-integration residual, pseudorange single-point positioning residual, and Doppler residual constraint are fused together. A unified optimization solution framework under a sliding window is used to jointly estimate the fused residuals and calculate the navigation and positioning results.

2. The multi-source fusion navigation method based on voxel map association and ground constraints as described in claim 1, characterized in that, Acquire real-time raw point cloud data, inertial navigation observation data, and GNSS observation data, including the carrier's position, attitude, velocity gyroscope zero bias, and 15-dimensional state vector of acceleration zero bias.

3. The multi-source fusion navigation method based on voxel map association and ground constraints as described in claim 1, characterized in that, The calculation of IMU pre-integration residuals based on inertial navigation observation data introduces a constraint mechanism for GNSS pseudorange and Doppler observations without a reference station, constructing the original observation residuals, including: Construct GNSS pseudorange observation equations; Based on the time relationship, search for two LiDAR keyframes before and after the current pseudorange observation time t, obtain the receiver's position in the ENU system by interpolation, and convert it to the ECEF system to calculate the pseudorange single-point positioning residual. The weight of the pseudorange factor is determined by the GNSS elevation angle and azimuth angle. The larger the elevation angle, the higher the weight, in order to reduce the contribution of low elevation angle satellite observations.

4. A multi-source fusion navigation system based on voxel map association and ground constraints, characterized in that, Specifically, the multi-source fusion navigation method based on voxel map association and ground constraints as described in any one of claims 1-3 includes: The voxel grid construction module is used to acquire real-time raw point cloud data, inertial navigation observation data and GNSS observation data, and to construct a three-dimensional voxel grid map based on the point cloud data; The registration residual calculation module is used to extract planar feature points of the current frame, associate the planar feature points of the current frame with the voxel grid map, extract voxel factor residuals, and calculate planar feature registration residuals. The constraint residual construction module is used to extract ground plane points using point cloud segmentation technology, construct ground constraints by fitting ground parameters and the property that the Euclidean distance of the LiDAR sensor relative to the ground is invariant, and calculate the ground constraint residual; the IMU pre-integration residual is calculated based on inertial navigation observation data, and a constraint mechanism for GNSS pseudorange observations and Doppler observations without reference stations is introduced to construct the original observation residual; The fusion solution and positioning module is used to fuse voxel factor residuals, planar feature registration residuals, ground constraint residuals, IMU pre-integration residuals, pseudorange single-point positioning residuals, and Doppler residual constraints. A unified optimization solution framework under a sliding window is used to jointly estimate the fused residuals and calculate the navigation and positioning results.

5. A computer program product, comprising a computer program, characterized in that, When the computer program is executed by the processor, it implements the multi-source fusion navigation method based on voxel map association and ground constraints as described in any one of claims 1-3.

6. A non-transitory computer-readable storage medium, comprising: The non-transitory computer-readable storage medium is used to store computer instructions, which, when executed by a processor, implement the multi-source fusion navigation method based on voxel map association and ground constraints as described in any one of claims 1-3.

7. An electronic device, comprising: include: The device includes a processor, a memory, and a computer program; wherein the processor is connected to the memory, the computer program is stored in the memory, and when the electronic device is running, the processor executes the computer program stored in the memory to enable the electronic device to perform the multi-source fusion navigation method based on voxel map association and ground constraints as described in any one of claims 1-3.

Images