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

CN121977589ACN 121977589 ACN121977589 ACN 121977589ACN-121977589-A

Abstract

The present disclosure provides a multisource fusion navigation method and system based on voxel map association and ground constraint, and relates to the technical field of urban navigation positioning, comprising the steps of constructing a three-dimensional voxel grid map, associating planar feature points of a current frame with the voxel grid map, and extracting voxel factor residual errors and planar feature registration residual errors; the method comprises the steps of adopting a point cloud segmentation technology to extract ground plane points, constructing ground constraint, calculating ground constraint residual errors, calculating IMU pre-integration residual errors based on inertial navigation observation data, introducing a GNSS pseudo-range observation value and Doppler observation value constraint mechanism without a reference station to construct an original observation residual error, inputting voxel factor residual errors, plane feature registration residual errors, ground constraint residual errors, IMU pre-integration residual errors, pseudo-range single-point positioning residual errors and Doppler residual error constraint fusion into a factor graph optimizer, resolving, and outputting to obtain a navigation positioning result. The method and the device realize high-precision and high-reliability navigation positioning in a complex urban environment.

Inventors

XU TIANHE
ZHANG ZHEN
JIANG NAN
NIE WENFENG

Assignees

山东大学

Dates

Publication Date: 20260505
Application Date: 20260402

Claims (10)

1. The multi-source fusion navigation method based on voxel map association and ground constraint is characterized by comprising the following steps: acquiring real-time original point cloud data, inertial navigation observation data and GNSS observation data, and constructing a three-dimensional voxel grid map based on the point cloud data; extracting plane feature points of the current frame, associating the plane feature points of the current frame with a voxel grid map, extracting voxel factor residual errors, and calculating plane feature registration residual errors; Extracting ground plane points by adopting a point cloud segmentation technology, constructing ground constraint by fitting ground parameters and the characteristic that the Euclidean distance of a LiDAR sensor relative to the ground is unchanged, and calculating ground constraint residual errors; calculating IMU pre-integral residual errors based on inertial navigation observation data, introducing a constraint mechanism of GNSS pseudo-range observation values and Doppler observation values without reference stations, and constructing original observation residual errors; and carrying out constraint fusion on the voxel factor residual error, the plane feature registration residual error, the ground constraint residual error, the IMU pre-integration residual error, the pseudo-range single-point positioning residual error and the Doppler residual error, and carrying out joint estimation on the fusion residual error by adopting a unified optimization solving framework under a sliding window, so as to obtain a navigation positioning result by calculation.
2. The method of claim 1, wherein real-time origin cloud data, inertial navigation observations, and GNSS observations are obtained comprising carrier position, attitude, velocity gyro zero offset, and acceleration zero offset 15-dimensional state vectors.
3. The multi-source fusion navigation method based on voxel map correlation and ground constraint of claim 1, wherein the constructing a three-dimensional voxel grid map based on point cloud data comprises: when a frame of LiDAR Ping Miandian cloud is received, firstly converting the pose of the current frame into an M system, then calculating hash coordinates according to the three-dimensional coordinates of the point cloud, further calculating voxel grid indexes, and searching whether the indexes exist in the established grids, if not, creating a grid and storing the point, if so, directly storing the point into the corresponding grid, and for each voxel, characterizing the geometric characteristics of the point cloud quantity, the central point coordinates, the covariance, the characteristic values and the characteristic vectors in the voxel.
4. The method for multi-source fusion navigation based on voxel map correlation and ground constraint according to claim 1, wherein the extracting the planar feature point of the current frame, correlating the planar feature point of the current frame with a voxel grid map, extracting voxel factor residuals, comprises: Before registration, firstly, constructing a local map, converting the point cloud of LiDAR into a map coordinate system through the pose of a key frame, and accumulating the point cloud of a plurality of key frames to form the local map; converting the current frame point cloud for planar feature matching into a map coordinate system, and registering with a local map; and searching a plurality of points adjacent to the current frame point in the local map by utilizing the KD-Tree, fitting planes of the adjacent points, extracting plane parameters including a normal vector N and a plane parameter D, and calculating voxel factor residual errors.
5. The method for multi-source fusion navigation based on voxel map association and ground constraint according to claim 1, wherein the extracting ground plane points by using a point cloud segmentation technology, constructing ground constraint by fitting ground parameters and the characteristic that the LiDAR sensor is unchanged relative to the Euclidean distance of the ground, and calculating ground constraint residual errors comprises: For the LiDAR point cloud after de-distortion, calculating the pitch angle of each point by using three-dimensional coordinates of the LiDAR point cloud; Calculating the index of the wire harness where the point is located according to the number of wires and the coverage angle of the LiDAR; After the calculation of the wire harness index of the point cloud points is completed, performing ground point segmentation, discarding the point cloud with the wire harness index being more than 8, and calculating the actual distance and the theoretical distance of the point with the wire harness index being less than or equal to 8 to the LiDAR sensor; Extracting the surface characteristics of the ground by adopting a random sampling consistency method, acquiring the normal vector and the surface parameter of the ground surface, and simultaneously recording the internal points and the quantity of the internal points fitting the ground; Calculating a weight according to the distance from each internal point to the plane, and taking the weight as a confidence coefficient basis of ground constraint; And finally, establishing the association between the pose of the current frame and the ground characteristics, and constructing a ground constraint factor.
6. The method for multi-source fusion navigation based on voxel map association and ground constraint according to claim 1, wherein the calculating IMU pre-integration residual based on inertial navigation observation data, introducing a constraint mechanism of GNSS pseudo-range observation values and doppler observation values without reference stations, and constructing an original observation residual, comprises: constructing a GNSS pseudo-range observation equation; According to the time relation, searching two LiDAR key frames before and after the current pseudo-range observation time t, obtaining the position of a receiver under an ENU system through interpolation, converting the position into an ECEF system, calculating pseudo-range single-point positioning residual errors, determining the weight of pseudo-range factors by a GNSS altitude angle and an azimuth angle, and reducing the contribution of low-elevation satellite observation values by the greater altitude angle and the higher weight.
7. A multi-source fusion navigation system based on voxel map association and ground constraints, comprising: The voxel grid construction module is used for acquiring real-time original point cloud data, inertial navigation observation data and GNSS observation data and constructing a three-dimensional voxel grid map based on the point cloud data; the registration residual calculation module is used for extracting the plane feature points of the current frame, correlating the plane feature points of the current frame with the voxel grid map, extracting voxel factor residual and calculating plane feature registration residual; the constraint residual error construction module is used for extracting ground plane points by adopting a point cloud segmentation technology, constructing ground constraint by fitting the characteristic that ground parameters and LiDAR sensors are unchanged relative to the ground Euclidean distance, and calculating ground constraint residual error; the fusion calculation positioning module is used for carrying out constraint fusion on the voxel factor residual error, the plane characteristic registration residual error, the ground constraint residual error, the IMU pre-integration residual error, the pseudo-range single-point positioning residual error and the Doppler residual error, carrying out joint estimation on the fusion residual error by adopting a unified optimization solution framework under a sliding window, and calculating to obtain a navigation positioning result.
8. A computer program product comprising a computer program, characterized in that the computer program, when executed by a processor, implements the voxel map correlation and ground constraint based multisource fusion navigation method of any one of claims 1-6.
9. A non-transitory computer readable storage medium storing computer instructions which, when executed by a processor, implement the voxel map correlation and ground constraint based multi-source fusion navigation method of any one of claims 1-6.
10. An electronic device comprising a processor, a memory, and a computer program, wherein the processor is coupled to the memory, the computer program being stored in the memory, the processor executing the computer program stored in the memory when the electronic device is operating to cause the electronic device to perform a multi-source fusion navigation method based on voxel map correlation and ground constraints as claimed in any one of claims 1-6.

Description

Multi-source fusion navigation method and system based on voxel map association and ground constraint Technical Field The disclosure relates to the technical field of urban navigation positioning, in particular to a multisource fusion navigation method and system based on voxel map association and ground constraint. Background The statements in this section merely provide background information related to the present disclosure and may not necessarily constitute prior art. In complex urban environments, a robust and robust navigation positioning service is a precondition and basis for realizing automatic driving, and a single-sensor-based navigation technology is difficult to meet the requirement. In multi-source fusion navigation, an INS (Inertial Navigation System, an inertial navigation system) is used as an autonomous relative positioning system, and the pose calculation of a carrier is realized by calculating acceleration and angular velocity information acquired by an IMU (Inertial Measurement Unit, an inertial measurement unit). The method has the characteristics of no dependence on external signals, high output frequency, continuity, stability and the like. However, due to the cumulative effect of the inherent errors of the sensors, the positioning accuracy of INS diverges rapidly over time, and thus it is often necessary to use it in combination with other navigation sensors in practical applications. In the task of synchronous positioning and mapping, the Light Detection AND RANGING (laser radar) has high-precision ranging and excellent environment sensing capability, realizes relative positioning by acquiring point cloud data of surrounding environment, and synchronously constructs a two-dimensional or three-dimensional point cloud map of the environment. However, position errors accumulate over time, particularly in the elevation direction, and the constraint conditions are limited due to insufficient observation information, so that accurate and reliable positioning results are difficult to obtain. GNSS (Global Navigation SATELLITE SYSTEM, global satellite navigation system) is able to provide global position information with high accuracy, but the signals are susceptible to occlusion and multipath effects, which can lead to position discontinuities or large errors, which limit its reliability and applicability in complex scenarios. Therefore, in order to improve the robustness and reliability of the navigation system, the integrated navigation algorithm of the multi-source sensor fusion is widely researched and applied. In the prior art, dead reckoning or absolute position acquisition can be achieved using a single sensor, but this approach is vulnerable in complex urban environments. And combining IMU pre-integration with laser radar point cloud matching, providing a multi-source fusion navigation frame LIO-SAM based on factor graph optimization, realizing high-precision and low-drift long-time navigation and mapping, and optionally integrating a GNSS positioning result. Or an extended Kalman filter (IEKF) framework is adopted, so that the LIO fusion strategy is optimized, and the robustness of the system in a complex environment is improved. The recently proposed FAST-LIO2 method directly uses the original point cloud data and introduces an incremental kd-Tree (ikd-Tree), thereby realizing more efficient point cloud management and mapping. Or a tightly coupled GNSS/INS/LiDAR fusion frame, the complementary characteristics of the multi-source sensor are effectively integrated through a sliding window plane characteristic tracking method, and stable and reliable motion estimation is realized in urban environments with limited satellite signals. Or a tightly coupled GNSS RTK/INS/LiDAR system (FGO-GIL) based on a factor graph optimization framework, wherein the system adopts a key frame nonlinear optimization scheme, realizes inter-frame optimization through pre-integration of a non-key frame and an inertia measurement unit, and utilizes a sparse key frame to construct a laser radar factor for sliding window optimization, thereby effectively improving navigation performance in urban environment. Or a tightly coupled PPP/INS/Vision/LiDAR fusion method, directly fusing multi-source heterogeneous data in an observation layer through an extended Kalman filter, and realizing high-precision, continuous and reliable navigation positioning in a complex urban environment. According to the scheme, the speed and the resolving precision of the attitude parameters are effectively improved through the complementary fusion of the visual sparse road sign and the laser radar characteristic information. Or in the LiDAR-IMU navigation framework, vehicle pose estimation is optimized through ground point extraction, but the existing methods still have the following limitations: (1) The single LiDAR sensor acquires the surrounding environment point cloud through scanning, extracts characteristic points and accumulate