A high-precision positioning device for unmanned system based on multi-sensor fusion

Abstract

The application discloses a kind of high-precision positioning device of unmanned system based on multi-sensor fusion, belong to unmanned system positioning navigation technical field.The present application is aimed at the defects that the scene adaptability of existing positioning technology is poor, the space-time synchronization accuracy of multi-source data is low, the positioning continuity is insufficient under complex environment, and the cumulative drift of long voyage time is difficult to suppress, and a multi-sensor redundant architecture and hierarchical fusion scheme are designed.The core of the device includes GNSS positioning module, inertial sensing module, environment sensing module, global space-time synchronization unit, multi-source data preprocessing unit, hierarchical fusion solution unit, fault diagnosis and system reconfiguration unit;Through hardware-level microsecond space-time synchronization, the fusion framework of loose coupling and tight coupling is combined to realize the reference unification of multi-source data, and the whole scene continuous positioning is realized by cooperating with the fault dynamic reconfiguration mechanism.The application can realize high-precision, high-robustness positioning under complex environment, and adapt to the positioning and navigation needs of various unmanned systems.

Description

Technical Field

[0001] This invention belongs to the field of unmanned system positioning and navigation technology, specifically involving a multi-sensor fusion positioning device that meets automotive-grade functional safety requirements. It can be adapted to various unmanned platforms such as L2-L5 level unmanned vehicles, industrial autonomous mobile robots, low-altitude logistics drones, and unmanned surface vessels, meeting the centimeter-level high precision, high robustness, and continuous positioning and navigation needs in all scenarios, including open areas, urban canyons, underground spaces, and indoor factories. Background Technology

[0002] With the large-scale deployment of unmanned systems technology, scenarios such as autonomous driving, intelligent warehousing, and low-altitude economy place four core requirements on positioning and navigation systems: centimeter-level accuracy, millisecond-level real-time performance, full-scenario continuity, and functional safety-level reliability. Existing positioning technologies suffer from the following inherent and insurmountable defects:

[0003] 1. Single sensors have inherent limitations: Pure Global Navigation Satellite System (GNSS) satellite positioning is prone to failure in obstructed or multipath scenarios, and is completely unusable in urban canyons and indoor environments; Pure Inertial Navigation System (INS) has zero-bias drift characteristics, and the cumulative position error can exceed 5m after GNSS loses lock for 30 seconds; Pure laser / visual odometry can only achieve relative pose calculation, and the cumulative drift rate during long-endurance driving can reach 0.5%-1% of the driving distance, which cannot meet the global positioning requirements of long-distance operations.

[0004] 2. Insufficient spatiotemporal synchronization accuracy limits the upper limit of fusion: Existing multi-sensor solutions mostly adopt software-level time synchronization, with synchronization errors generally ranging from 1ms to 10ms. When the speed of an unmanned platform reaches 100km / h, a 1ms time error will directly lead to a spatial position deviation of 0.027m. The positioning accuracy is severely limited in high-speed motion scenarios. For example, Chinese patent publication number CN114111775A, published on March 1, 2022, proposes a multi-sensor fusion positioning method for autonomous driving in parks, which adopts a software-level linear interpolation time synchronization method. Most solutions have not completed the unified calibration of the spatial extrinsic parameters of all sensors, and the spatial benchmark of multi-source data is not unified, which further amplifies the fusion error.

[0005] 3. Fixed Fusion Architecture, Insufficient Fault Tolerance and Functional Safety: Most mainstream GNSS / INS dual-sensor combined positioning systems adopt a fixed loosely coupled framework. After GNSS loss of lock, it directly degenerates into pure INS, and the positioning accuracy diverges rapidly. For example, Chinese Patent Publication No. CN112362051A, published on February 12, 2021, discloses a mobile robot navigation and positioning system based on GNSS / INS / LiDAR SLAM information fusion, which uses a single EKF loosely coupled fusion framework. A few tightly coupled solutions lack redundancy backup capabilities, and the failure of a single sensor will cause the system to collapse, failing to meet the ASIL-B and above requirements of the ISO 26262 Road Vehicle Functional Safety Standard. For example, Chinese Patent Publication No. CN115265551A, published on November 1, 2022, proposes a multi-sensor fusion positioning method for complex scenarios of autonomous driving, but in terms of fault tolerance, it only has GNSS fault detection and one factor, and the LiDAR lacks redundant backup.

[0006] 4. Long-term cumulative drift cannot be effectively suppressed: Existing solutions mostly focus only on local inter-frame pose optimization and lack a global error closure mechanism. The cumulative drift cannot be eliminated after long-distance and long-term operations, which cannot meet the operational requirements of unmanned systems for cross-regional and long-term operations. For example, Chinese patent publication number CN114264301A, published on April 1, 2022, proposed a dual-combination inertial navigation redundant positioning system, which lacks a loop closure detection mechanism, relies on pre-built maps, and cannot eliminate online cumulative drift.

[0007] Addressing the core pain points of the aforementioned existing technologies, this invention innovates across the entire chain from four dimensions: hardware architecture, spatiotemporal synchronization, fusion algorithm, and fault tolerance mechanism, proposing a modular, all-scenario, highly fault-tolerant, and highly accurate multi-sensor fusion unmanned system positioning device. Summary of the Invention

[0008] The purpose of this invention is to provide a high-precision positioning device for unmanned systems based on multi-sensor fusion. It efficiently utilizes the spatiotemporal complementarity and redundancy characteristics of multi-source sensors, and employs a hierarchical fusion framework and dynamic fault reconfiguration mechanism to construct a highly robust all-scenario positioning solution system. This solves the problems of insufficient utilization of multi-source information and insufficient flexibility of the fusion architecture in existing positioning schemes, while avoiding the dispersion of positioning accuracy and severe cumulative drift during long-endurance operations caused by the failure of a single sensor. This invention provides a high-precision positioning device for unmanned systems based on multi-sensor fusion, which can be applied in autonomous driving on urban roads and navigation operations of unmanned systems in underground spaces.

[0009] Core coordinate system definition

[0010] All algorithms and formulas in this invention are based on the following four unified coordinate systems, and the notation system remains consistent throughout:

[0011] 1. Navigation coordinate system (n system): The northeast-northeast (ENU) coordinate system is adopted. The origin is the initial position of the unmanned system, the X-axis points east, the Y-axis points north, and the Z-axis is perpendicular to the ground and upward. It is the reference coordinate system for global pose calculation.

[0012] 2. Vehicle coordinate system (b system): The origin is the rear axle center of the unmanned system, the X-axis is along the forward direction of the vehicle, the Y-axis is horizontal to the left along the vehicle, and the Z-axis is perpendicular to the vehicle plane and upward. It is rigidly connected to the vehicle and serves as a unified spatial reference for multi-sensor data.

[0013] 3. Sensor coordinate system (S-frame): The inherent coordinate system of each sensor, including the IMU coordinate system, the LiDAR coordinate system, and the camera coordinate system, which is transformed to the vehicle coordinate system through extrinsic parameter calibration.

[0014] 4. Geocentric-fixed coordinate system (e system): WGS84 coordinate system, with the origin at the Earth's center of mass, used for coordinate datum transformation of GNSS positioning results.

[0015] Core symbol definition:

[0016] : The rotation matrix from coordinate system a to coordinate system b;

[0017] : The translation vector of the origin of coordinate system a in coordinate system b;

[0018] : The state vector of the system at time k;

[0019] : The observation vector of the system at time k;

[0020] b: Three-axis angular velocity measured by a gyroscope in the system;

[0021] : b-series accelerometer-measured triaxial specific force;

[0022] , : Zero bias of gyroscope and zero bias of accelerometer;

[0023] , Gaussian white noise for gyroscopes and Gaussian white noise for accelerometers.

[0024] To achieve the above objectives, the technical solution of the present invention is as follows: a high-precision positioning device for unmanned systems based on multi-sensor fusion, comprising a Global Navigation Satellite System (GNSS) positioning module, an inertial sensing module, and further comprising an environmental sensing module, a global spatiotemporal synchronization unit, a multi-source data preprocessing unit, a hierarchical fusion solution unit, a fault diagnosis and system reconfiguration unit, and a positioning result output unit.

[0025] The multiple input terminals of the global spatiotemporal synchronization unit are electrically connected one-to-one with the signal output terminals of the GNSS positioning module, inertial sensing module, and environmental sensing module, respectively, for hardware-level time synchronization and unified calibration of spatial coordinate reference for the raw data output by all sensors, thereby achieving spatiotemporal reference unification of multi-source data.

[0026] The input terminal of the multi-source data preprocessing unit is electrically connected to the synchronization data output terminal of the global spatiotemporal synchronization unit. It is used to perform preprocessing operations such as noise reduction, correction, and feature extraction on the raw data of each sensor after spatiotemporal synchronization is completed, and output standardized and effective observation data.

[0027] The input end of the hierarchical fusion solution unit is electrically connected to the effective observation data output end of the multi-source data preprocessing unit. It is used to perform loosely coupled and tightly coupled fusion solution on the effective observation data of multiple sources based on a preset hierarchical fusion framework, and output the initial positioning pose result. The hierarchical fusion framework takes into account both the high-frequency real-time performance of the positioning output and the solution accuracy.

[0028] The fault diagnosis and system reconfiguration unit is bidirectionally connected to the multi-source data preprocessing unit and the hierarchical fusion solution unit, respectively. It is used to monitor the validity of the observation data of each sensor and the running status of the fusion solution in real time. When the sensor data fails or the solution status is abnormal, it triggers the dynamic reconfiguration of the fusion framework, removes the failed sensor data, and switches the appropriate fusion strategy.

[0029] The input end of the high-precision map matching unit is electrically connected to the pose output end of the hierarchical fusion solution unit, and is used to match and correct the pose result obtained by fusion solution with the preloaded high-precision prior map, and output the optimized global pose result.

[0030] The input terminal of the positioning result output unit is electrically connected to the final pose output terminal of the high-precision map matching unit, and is used to output the final positioning pose result to the main controller of the unmanned system.

[0031] The GNSS positioning module includes a dual-frequency multi-system GNSS receiver and an anti-multipath choke antenna. The signal output terminal of the anti-multipath choke antenna is electrically connected to the radio frequency input terminal of the dual-frequency multi-system GNSS receiver, and the signal output terminal of the dual-frequency multi-system GNSS receiver is electrically connected to the corresponding input terminal of the global spatiotemporal synchronization unit. The dual-frequency multi-system GNSS receiver supports joint calculation of dual-frequency observations from BeiDou-3, GPS, GLONASS, and Galileo global satellite navigation systems. It can output raw pseudorange, carrier phase, Doppler frequency shift observations, and initial positioning calculation values, while simultaneously outputting pulse-per-second (PPS) signals and Coordinated Universal Time (UTC) timestamps to provide a global time reference for the entire system.

[0032] The inertial sensing module is a high update rate inertial measurement unit (IMU). The inertial measurement unit includes at least a three-axis gyroscope and a three-axis accelerometer, and its sampling frequency is not less than 1 kHz. The signal output terminal of the inertial measurement unit is electrically connected to the corresponding input terminal of the global spatiotemporal synchronization unit, and is used to output the raw observation data of the three-axis angular rate and three-axis specific force of the unmanned system, so as to provide a high-frequency attitude recursive reference for the system.

[0033] The environmental perception module includes a lidar submodule and a visual odometry submodule. The lidar submodule is a multi-line mechanical lidar or a solid-state lidar, used to output three-dimensional point cloud data of the surrounding environment of the unmanned system. The visual odometry submodule is a binocular global shutter camera, used to output synchronous binocular image data of the surrounding environment of the unmanned system. The signal output terminals of the lidar submodule and the visual odometry submodule are electrically connected to the corresponding input terminals of the global spatiotemporal synchronization unit, respectively, to provide relative pose observation values ​​under environmental constraints in GNSS signal failure scenarios, and to suppress the cumulative drift of inertial navigation.

[0034] The global spatiotemporal synchronization unit includes a hardware time synchronization submodule and a spatial calibration submodule, which are implemented as follows:

[0035] 1. Hardware Time Synchronization Submodule: Based on the second pulse (PPS) signal output by the GNSS positioning module and the UTC timestamp, it provides a unified hardware trigger signal and global time reference for the inertial sensing module and the environmental sensing module through a field-programmable gate array (FPGA) hardware circuit. The time synchronization accuracy constraint formula is as follows:

[0036]

[0037] In the formula, For time synchronization error, The hardware timestamp for the moment when sensor data is sampled. The UTC global timestamp provided for GNSS.

[0038] This module achieves strict alignment of sampling times for all sensors through FPGA hardware triggering, completely eliminating interruption delay errors in software time synchronization, with a synchronization accuracy of up to 0.5μs.

[0039] 2. Spatial Calibration Submodule: Pre-stores the extrinsic parameter calibration results of each sensor relative to the vehicle coordinate system, and performs a unified conversion of all sensor observation data to the vehicle coordinate system using the following formula:

[0040]

[0041] In the formula, The coordinates of the observation point in the sensor coordinate system. To transform the coordinates of the observation points to the vehicle coordinate system, This is the rotation matrix from the sensor coordinate system to the vehicle coordinate system. This is the translation vector from the sensor coordinate system to the vehicle coordinate system.

[0042] The objective function for extrinsic parameter calibration is:

[0043]

[0044] In the formula, N is the number of calibration matching point pairs. The optimal extrinsic parameters are solved by least squares optimization to achieve the unification of spatial coordinate reference.

[0045] The multi-source data preprocessing unit includes a GNSS data preprocessing submodule, an IMU data preprocessing submodule, a lidar data preprocessing submodule, and a visual data preprocessing submodule. The core implementation and formulas of each submodule are as follows:

[0046] 1. IMU Data Preprocessing Submodule

[0047] First, establish the IMU measurement model:

[0048]

[0049] In the formula, This represents the true angular velocity in the b-frame. For the true comparison in the n-system, Let be the gravitational acceleration vector in the n-system.

[0050] After completing the zero-bias correction and temperature compensation of the raw data, pre-integration calculation is performed on the IMU data to obtain the relative pose pre-integral quantity between two keyframes i and j. The discrete-time pre-integration formula is as follows:

[0051]

[0052] In the formula, , , These are the pre-integral values ​​for rotation, velocity, and position between two keyframes. Let be the IMU sampling time step, and Exp(⋅) be the exponential mapping from the Lie algebra to the Lie group.

[0053] Simultaneously, the error propagation covariance matrix of the pre-integrated quantity is calculated to provide a noise weighting benchmark for subsequent fusion solution.

[0054] 2. GNSS Data Preprocessing Submodule

[0055] The Mel-Wibner combination method (MW combination method) is used to complete carrier phase cycle slip detection and repair. The formula for the MW combination observation value is:

[0056]

[0057] In the formula, , For GNSS dual-frequency carrier frequencies, , These are dual-frequency carrier phase observations. , These are dual-frequency pseudorange observations.

[0058] When the combined difference of the MW values ​​of consecutive observations exceeds 4 times the root mean square error, a cycle slip is determined to have occurred, and a polynomial fitting method is used to complete the cycle slip repair.

[0059] The Klobuchar model was used to perform ionospheric delay correction, and the Saastamoinen model was used to perform tropospheric delay correction, thus eliminating the influence of atmospheric errors on GNSS observations.

[0060] 3. LiDAR Data Preprocessing Submodule

[0061] Based on synchronized IMU high-frequency pose data, point cloud motion distortion correction is performed. The correction formula is as follows:

[0062]

[0063] In the formula, The coordinates of the original distorted point cloud. The corrected point cloud coordinates, , The vehicle pose transformation matrix at point cloud sampling time k relative to frame start time 0 is obtained through high-frequency recursion by the IMU.

[0064] After correction, statistical filtering is used to remove outliers, and the Random Sampling Consensus Algorithm (RANSAC) is used to segment ground points, extract corner and surface features, and output standardized point cloud data.

[0065] 4. Visual Data Preprocessing Submodule

[0066] Based on the distortion coefficients inherent in the binocular camera, image distortion correction and stereo rectification are performed, and the corrected binocular image satisfies the epipolar horizontal alignment constraint; the depth values ​​of feature points are obtained through disparity calculation, and the depth transformation formula is as follows:

[0067]

[0068] In the formula, z is the depth value of the feature point, f is the camera focal length, b is the baseline length of the binocular camera, and d is the binocular parallax value of the feature point.

[0069] Image feature point extraction and matching are performed using the directional FAST and rotation BRIEF (ORB) algorithms. The relative pose between adjacent frames is calculated using the perspective n-point algorithm (PNP algorithm), and visual odometry observations are output.

[0070] The hierarchical fusion solution unit includes a primary loosely coupled fusion submodule, a secondary tightly coupled fusion submodule, and a global pose optimization submodule. The core algorithms and formulas of each submodule are as follows:

[0071] 1. Primary loosely coupled fusion submodule

[0072] Based on a 15-dimensional extended Kalman filter (EKF) framework, it is used to output high-frequency pose initial values ​​to meet the real-time requirements of the underlying control of unmanned systems.

[0073] The system state vector is defined as:

[0074]

[0075] In the formula, ∈ The position in the n-system. ∈ The velocity in the n-system is... ∈ For attitude angle error, ∈ To achieve zero bias in the gyroscope, ∈ To achieve zero bias in the accelerometer, the total state dimension is 15.

[0076] The system state prediction equation is:

[0077]

[0078] In the formula, Here is the state transition matrix. To control the input matrix, Use the IMU measurements as control inputs.

[0079] The formula for updating the predicted covariance is:

[0080]

[0081] In the formula, P is the state covariance matrix and Q is the process noise covariance matrix.

[0082] The system observation equation is:

[0083]

[0084] In the formula, These are the position observations output by GNSS. For the observation matrix, To observe noise.

[0085] The Kalman gain calculation and state update formulas are as follows:

[0086]

[0087] In the formula, For Kalman gain, To observe the noise covariance matrix, It is an identity matrix.

[0088] This module performs state prediction at a frequency of 2kHz, updates GNSS observations at a frequency of 1Hz, and outputs initial pose values ​​at a high frequency of 2kHz.

[0089] 2. Secondary tightly coupled fusion submodule

[0090] Based on the Factor Graph Optimization (FGO) framework, a sliding window optimization mechanism is adopted to achieve tight coupling and high-precision solution of multi-source data.

[0091] The global objective function for factor graph optimization is:

[0092]

[0093] In the formula, X is the set of state variables for all keyframes within the sliding window. For the set of all factors, Let i be the residual term of the i-th factor. Let be the covariance matrix of the corresponding factors. This is the Huber robust kernel function, used to suppress the effects of outliers.

[0094] The residuals of each factor are defined as follows:

[0095] IMU pre-integral factor residuals:

[0096]

[0097] In the formula, These are the state variables for keyframes i and j, respectively. The time interval between two keyframes.

[0098] GNSS observation factor residuals:

[0099]

[0100] In the formula, These are the position observations output by GNSS.

[0101] Laser odometry factor residual:

[0102]

[0103] In the formula, Pose transformation of adjacent frames obtained by laser point cloud registration. , Let i be the pose Lie group of keyframes i and j.

[0104] Visual odometry factor residuals:

[0105]

[0106] In the formula, The pose transformation between adjacent frames obtained from visual odometry calculation.

[0107] The Levenberg-Marquardt nonlinear optimization algorithm is used to solve the optimal solution of the objective function, and the output is a high-precision pose result of 10Hz with a positioning accuracy of ±2cm.

[0108] 3. Global Pose Optimization Submodule

[0109] Based on loop closure detection and pose graph optimization, this method is used to eliminate cumulative drift over long flight times. The objective function for pose graph optimization is:

[0110]

[0111] In the formula, T is the global pose set of all keyframes, and E is the set of loop closure constraint edges. The inter-frame pose constraints are obtained from loop closure detection. is the covariance matrix of the lapsing constraint.

[0112] This module uses the Scan Context algorithm for laser loop closure detection and the bag-of-words model for visual loop closure detection. After a valid loop closure is detected, global pose graph optimization is performed to completely eliminate long-endurance cumulative drift.

[0113] The fault diagnosis and system reconfiguration unit includes a sensor fault detection submodule, a fusion status monitoring submodule, and a fusion strategy reconfiguration submodule. The core implementation and formulas are as follows:

[0114] 1. Sensor Fault Detection Submodule: Employs the normalized residual chi-square test criterion to detect the validity of sensor observation data in real time. The test formula is:

[0115]

[0116] In the formula, Let k be the chi-square test statistic at time k. To observe the residuals, The residual covariance matrix is... The chi-square distribution threshold is given by n degrees of freedom and α confidence level.

[0117] when > If so, the sensor's observed data is determined to be abnormal or malfunctioning.

[0118] 2. Fusion Status Monitoring Submodule: Real-time monitoring of the convergence of the filter covariance matrix, the residual distribution of factor graph optimization, and the confidence level of the positioning results. When the covariance matrix continues to diverge or the optimization residual exceeds the threshold, the fusion solution status is determined to be abnormal.

[0119] 3. Fusion Strategy Reconfiguration Submodule: Based on fault detection results, this submodule dynamically adjusts the objective function of the factor graph optimization, removes factor terms corresponding to failed sensors, and achieves dynamic reconfiguration of the fusion framework. The reconfigured objective function is:

[0120]

[0121] In the formula, Fvalid is the set of factors corresponding to effective sensors. For factor weights, the weights corresponding to the failed sensors. =0, the effective sensor dynamically adjusts the weights based on the observation quality.

[0122] This module can adaptively switch fusion strategies in multiple scenarios: when GNSS is available, full sensor tight coupling fusion is enabled; when GNSS fails, it switches to laser-INS-vision tight coupling fusion; when vision fails, it switches to GNSS-INS-laser tight coupling fusion; in extreme scenarios where only the IMU is available, pure INS pose recursion is enabled for a short time, and fault warnings are sent to ensure the continuity of positioning results.

[0123] The high-precision map matching unit uses the Normal Distribution Transform (NDT) algorithm to match real-time point clouds with high-precision prior maps. The objective function for matching optimization is:

[0124]

[0125] In the formula, T is the pose transformation matrix to be optimized. To convert to a real-time point cloud in a map coordinate system, , This represents the mean and covariance matrix of the map raster.

[0126] After matching is completed, the pose result of the fusion solution is globally corrected to further improve the positioning accuracy to ±1cm.

[0127] The positioning result output unit communicates with the unmanned system main controller via automotive-grade Controller Area Network (CAN) bus / Ethernet, with an output frequency of not less than 1kHz. The output data includes: global position, velocity, attitude angle, corresponding timestamp, positioning result confidence, and sensor health status code, meeting the full-process requirements of unmanned system path planning and decision control.

[0128] The unmanned system includes at least one of unmanned vehicles, unmanned delivery robots, industrial autonomous mobile robots, drones, and unmanned ships. This device adopts a modular design and can be flexibly adapted to the hardware interfaces and performance requirements of different unmanned platforms.

[0129] Beneficial effects:

[0130] Compared with the prior art, the present invention has the following significant quantitative benefits:

[0131] 1. Excellent continuous positioning capability across all scenarios: Employing a four-sensor redundancy architecture of GNSS + INS + LiDAR + vision, it achieves full-scene coverage from the sensing principle level. Based on a factor graph optimization framework, the fault diagnosis unit updates factor weights and topology in real time, reconstructing and optimizing the objective function online. Simultaneously, combined with a dynamic fault reconfiguration mechanism, it suppresses drift divergence through mutual constraints among remaining sensors, achieving seamless coverage across all scenarios including open outdoor areas, urban canyons, tunnels, underground parking garages, and indoor factories, thus solving the problem of poor scene adaptability of single sensors.

[0132] 2. Suppressing cumulative drift over long runtime: This invention employs a Scan Context based on laser point clouds and a bag-of-words model based on visual images for independent loop closure detection, scene recognition, and loop closure candidate frame retrieval. The two modalities work in tandem. Laser is unaffected by changes in illumination intensity, while visual imaging captures texture details, improving loop closure recall and false alarm suppression in complex environments. Furthermore, a global pose graph is constructed to perform nonlinear optimization, jointly adjusting the positions of all historical keyframes to eliminate drift generated by frame-by-frame recursion.

[0133] 3. Balancing Real-Time Performance and Accuracy: This system is the first to adopt a three-tier architecture: loosely coupled primary EKF, tightly coupled secondary factor graph, and global pose graph optimization. The three tiers work in parallel and collaboratively. The first tier pushes real-time pose data to the controller at a high frequency of 2kHz, while simultaneously passing the IMU pre-integration results to the second tier as optimization priors. The second tier updates the high-precision pose every 10ms and feeds back the filtering state of the first tier to correct it. The third tier globally adjusts the historical trajectory upon detecting a loop closure, inversely optimizing the sliding window of the second tier. This novel architecture ensures both high accuracy and high real-time performance through clear division of labor and collaborative mechanisms.

[0134] 4. Fault Detection and Fault Tolerance Mechanism: Unlike existing solutions that only replace data at the observation level while maintaining the fusion model structure, this invention achieves online reconstruction at the factor graph structure level. It incorporates a built-in normalized residual chi-square test to detect the status of each sensor in real time, dynamically reconstructs the factor graph to optimize the objective function, automatically removes factor terms corresponding to failed sensors, and adjusts the weight of effective factors according to the observation quality. It supports switching between multiple failure modes such as GNSS failure, visual failure, laser failure, and IMU-only failure, eliminating the risk of system collapse caused by sensor failure and improving the system's fault tolerance to various fault states.

[0135] 5. High-precision map localization: The high-precision pose output by the hierarchical fusion solution unit provides reliable initial values ​​for NDT iteration, eliminating the reliance on coarse initial values ​​or global search for NDT registration, thus reducing matching time and failure rate. The matching unit registers the real-time point cloud with the pre-loaded high-precision map, directly constraining and correcting the absolute pose of the fusion solution, and outputting an optimized global pose, further reducing localization errors and improving localization capabilities.

[0136] 6. High spatiotemporal synchronization: This invention adopts an FPGA hardware-level synchronization architecture, using the second pulse and UTC timestamp output by the GNSS receiver as a globally unique time reference. The FPGA hardware circuit directly generates synchronization trigger signals and hardware timestamps for the IMU, LiDAR, and binocular camera. Deterministic triggering replaces nondeterministic alignment, reducing the randomness error problem in software synchronization and ensuring the spatiotemporal alignment of multi-source data. Attached Figure Description

[0137] Figure 1 is an overall structural block diagram of the high-precision positioning device for unmanned systems based on multi-sensor fusion according to an embodiment of the present invention.

[0138] Figure 2 is a structural block diagram of the global spatiotemporal synchronization unit according to an embodiment of the present invention.

[0139] Figure 3 is a structural block diagram of the hierarchical fusion solution unit according to an embodiment of the present invention.

[0140] Figure 4 is a schematic diagram of the workflow of the fault diagnosis and system reconfiguration unit according to an embodiment of the present invention. Detailed Implementation

[0141] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments. This embodiment takes L4 level autonomous passenger vehicle as the application scenario. All formulas and parameters correspond one-to-one with the aforementioned technical solutions. The protection scope of the present invention is not limited to the following embodiments.

[0142] like Figure 1 As shown in the figure, this embodiment discloses a high-precision positioning device for unmanned systems based on multi-sensor fusion, the core of which includes a GNSS positioning module, an inertial sensing module, an environmental sensing module, a global spatiotemporal synchronization unit, a multi-source data preprocessing unit, a hierarchical fusion solution unit, a fault diagnosis and system reconfiguration unit, a high-precision map matching unit, and a positioning result output unit.

[0143] The hardware selection, connection relationships, and specific parameter implementations for each module are as follows:

[0144] 1. GNSS positioning module

[0145] In this embodiment, the GNSS positioning module uses a Hexin Xingtong UB4B0 dual-band multi-system GNSS receiver and a matching anti-multipath choke antenna. The anti-multipath choke antenna is installed at the center of the roof of the unmanned vehicle, and its signal output terminal is electrically connected to the RF input terminal of the UB4B0 receiver via a coaxial cable. The signal output terminal of the UB4B0 receiver is electrically connected to the corresponding input terminal of the global time-space synchronization unit via a serial port.

[0146] This module supports joint calculation of dual-frequency observations from four major global satellite navigation systems: BeiDou-3 B1I / B2a, GPS L1 / L2, GLONASS G1 / G2, and Galileo E1 / E5b. It outputs 1Hz raw pseudorange, carrier phase, Doppler shift observations, and initial values ​​for real-time kinematic (RTK) positioning. It also outputs a 1Hz pulse-per-second (PPS) signal and a UTC timestamp, providing a global time reference for the entire positioning device.

[0147] 2. Inertial Sensing Module

[0148] In this embodiment, the inertial sensing module adopts an ADI ADIS16488 tactical-grade inertial measurement unit (IMU), which is installed at the center of the rear axle of the unmanned vehicle and rigidly connected to the vehicle's coordinate system. The sampling frequency of the IMU is set to 2kHz, corresponding to the pre-integration step Δt=0.0005s in formula (5). Its signal output terminal is electrically connected to the corresponding input terminal of the global spatiotemporal synchronization unit through the SPI interface, which can synchronously output the original observation data of the three-axis angular rate and three-axis specific force of the unmanned system.

[0149] For low-cost application scenarios, this module can be replaced with the BMI088 automotive-grade microelectromechanical system (MEMS) IMU, with a sampling frequency of not less than 1kHz, which can meet the basic positioning requirements.

[0150] 3. Environmental Perception Module

[0151] In this embodiment, the environmental perception module includes a lidar submodule and a visual odometry submodule.

[0152] The lidar submodule uses the Hesai AT128 automotive-grade 128-line mechanical lidar, which is installed on the front of the vehicle roof. Its signal output terminal is electrically connected to the corresponding input terminal of the global spatiotemporal synchronization unit via Ethernet, outputting 10Hz three-dimensional point cloud data. The point cloud ranging range can reach 200m, and the ranging accuracy is ±2cm.

[0153] The visual odometry submodule uses two Basler acA2040-180km global shutter industrial cameras to form a binocular camera, which is installed on the inside of the vehicle's windshield. The binocular baseline length b=0.12m. The signal output terminals of the two cameras are electrically connected to the corresponding input terminals of the global spatiotemporal synchronization unit via Gigabit Ethernet, outputting 20Hz synchronous binocular image data with an image resolution of 2048×1536.

[0154] 4. Global Spatiotemporal Synchronization Unit

[0155] As shown in Figure 2, the global spatiotemporal synchronization unit in this embodiment is built on the Xilinx Artix-7 FPGA chip and includes a hardware time synchronization submodule and a spatial calibration submodule.

[0156] The input of the hardware time synchronization submodule receives the PPS second pulse signal and UTC timestamp output by the GNSS positioning module. Using the UTC time provided by GNSS as the globally unique time reference, the FPGA hardware circuit generates multiple synchronization trigger signals, which are output to the IMU, LiDAR, and binocular camera respectively, to achieve hardware trigger synchronization of all sensors. The time synchronization accuracy can reach 0.5μs, which meets the error constraint requirements of formula (1).

[0157] The spatial calibration submodule has a built-in Flash storage chip, which pre-stores the external parameter calibration results of each sensor relative to the vehicle body coordinate system. The unified conversion of all sensor data to the vehicle body coordinate system is completed through formula (2). The external parameter calibration results are obtained in advance through least squares optimization of formula (3).

[0158] 5. Multi-source data preprocessing unit

[0159] The multi-source data preprocessing unit in this embodiment is built on an ARM Cortex-A72 processor. Its input is electrically connected to the synchronization data output of the global spatiotemporal synchronization unit via a high-speed bus. It includes a GNSS data preprocessing submodule, an IMU data preprocessing submodule, a lidar data preprocessing submodule, and a visual data preprocessing submodule. Each submodule performs standardized preprocessing on the synchronized multi-source data. The core algorithm strictly follows the constraints of formulas (4) to (8).

[0160] 6. Hierarchical fusion solution unit

[0161] As shown in Figure 3, the hierarchical fusion solution unit in this embodiment is built on the NVIDIA Jetson Orin NX automotive-grade system-on-a-chip (SOC) chip. The input end is electrically connected to the effective observation data output end of the multi-source data preprocessing unit, and includes a primary loosely coupled fusion submodule, a secondary tightly coupled fusion submodule, and a global pose optimization submodule.

[0162] Primary loosely coupled fusion submodule: Based on the 15-dimensional EKF framework of formula (9)-formula (13), it completes IMU state prediction at a frequency of 2kHz, completes GNSS observation update at a frequency of 1Hz, and outputs high-frequency pose initial value of 2kHz to meet the real-time control requirements of the underlying drive-by-wire system of unmanned vehicles.

[0163] Secondary tightly coupled fusion submodule: Based on the factor graph optimization framework of formula (14)-formula (18), the sliding window length is set to 10 key frames, and the Levenberg-Marquardt algorithm is used to complete the nonlinear optimization, outputting a high-precision pose result of 10Hz.

[0164] Global pose optimization submodule: Based on the pose graph optimization framework of formula (19), the ScanContext algorithm is used to realize laser loop closure detection. After a valid loop closure is detected, global pose graph optimization is performed to eliminate long-endurance cumulative drift.

[0165] 7. Fault Diagnosis and System Reconfiguration Unit

[0166] In this embodiment, the fault diagnosis and system reconfiguration unit is bidirectionally connected to the multi-source data preprocessing unit and the hierarchical fusion solution unit, and its workflow is shown in Figure 4.

[0167] The sensor fault detection submodule adopts the chi-square test criterion of formula (20), sets the confidence level α=95%, the degree of freedom n=3, and the corresponding chi-square threshold χ0.95,32=7.815 to judge the validity of the observation data of each sensor in real time; the fusion status monitoring submodule monitors the convergence of the fusion solution in real time; the fusion strategy reconfiguration submodule dynamically adjusts the fusion framework based on formula (21) to realize adaptive switching under sensor failure scenarios.

[0168] 8. High-precision map matching unit

[0169] The high-precision map matching unit in this embodiment has a pre-loaded lane-level high-precision point cloud map built in. It uses the NDT point cloud registration algorithm of formula (22) to match and align the real-time point cloud with the high-precision map, perform global correction on the fused pose result, and output the optimized global pose result.

[0170] 9. Positioning Result Output Unit

[0171] In this embodiment, the input terminal of the positioning result output unit is electrically connected to the final pose output terminal of the high-precision map matching unit. It is also electrically connected to the on-board autonomous driving domain controller via an automotive-grade controller area network bus with flexible data rate (CAN FD bus). The final positioning result is output to the vehicle main controller at a frequency of 2kHz. The output positioning data includes: vehicle global position, speed, attitude angle, corresponding timestamp, positioning result confidence, and sensor health status code, which meets the full-process requirements of path planning, decision control, and obstacle avoidance of autonomous vehicles.

[0172] The complete workflow of this embodiment is as follows:

[0173] Step 1: After the system is powered on, all sensors and units are initialized, and the global spatiotemporal synchronization unit completes the hardware time synchronization and spatial reference unification of multiple sensors.

[0174] Step 2: Each sensor synchronously collects raw observation data and transmits it to the multi-source data preprocessing unit to complete standardized preprocessing such as noise reduction, correction, and feature extraction.

[0175] Step 3: The hierarchical fusion solution unit performs three-level fusion solution. The primary loosely coupled module outputs high-frequency initial pose values, the secondary tightly coupled module outputs high-precision pose results, and the global pose optimization module eliminates cumulative drift.

[0176] Step 4: The fault diagnosis and system reconfiguration unit monitors the sensor health status and fusion solution status in real time, and dynamically reconfigures the fusion strategy when an abnormality occurs;

[0177] Step 5: The high-precision map matching unit performs map matching correction on the fused pose, and finally outputs the positioning result to the unmanned system main controller through the positioning result output unit.

Claims

1. A high-precision positioning device for unmanned systems based on multi-sensor fusion, comprising a Global Navigation Satellite System (GNSS) positioning module and an inertial sensing module, characterized in that: It also includes an environmental perception module, a global spatiotemporal synchronization unit, a multi-source data preprocessing unit, a hierarchical fusion solution unit, a fault diagnosis and system reconfiguration unit, and a positioning result output unit; The multiple input terminals of the global spatiotemporal synchronization unit are electrically connected one-to-one with the signal output terminals of the GNSS positioning module, the inertial sensing module, and the environmental sensing module, respectively, for hardware-level time synchronization and unified calibration of the spatial coordinate reference of the raw data output by all sensors. The input of the multi-source data preprocessing unit is electrically connected to the synchronization data output of the global spatiotemporal synchronization unit. It is used to preprocess the raw data of each sensor after spatiotemporal synchronization is completed and output standardized effective observation data. The input end of the hierarchical fusion solution unit is electrically connected to the effective observation data output end of the multi-source data preprocessing unit. It is used to perform loosely coupled and tightly coupled fusion solution on the effective observation data of multiple sources based on a preset hierarchical fusion framework, and output the initial positioning pose result. The fault diagnosis and system reconfiguration unit is bidirectionally connected to the multi-source data preprocessing unit and the hierarchical fusion solution unit, respectively, to monitor the validity of the observation data of each sensor and the running status of the fusion solution in real time, and to trigger the dynamic reconfiguration of the fusion framework when the sensor data fails or the solution status is abnormal. The input terminal of the positioning result output unit is electrically connected to the final pose output terminal of the hierarchical fusion calculation unit, and is used to output the positioning result to the main controller of the unmanned system.

2. The high-precision positioning device for unmanned systems based on multi-sensor fusion according to claim 1, characterized in that: The GNSS positioning module includes a dual-frequency multi-system GNSS receiver and an anti-multipath choke antenna; the signal output terminal of the anti-multipath choke antenna is electrically connected to the RF input terminal of the dual-frequency multi-system GNSS receiver, and the signal output terminal of the dual-frequency multi-system GNSS receiver is electrically connected to the corresponding input terminal of the global spatiotemporal synchronization unit; the dual-frequency multi-system GNSS receiver supports joint calculation of dual-frequency observations from BeiDou-3, GPS, GLONASS, and Galileo global satellite navigation systems, and outputs pseudorange, carrier phase, Doppler frequency shift observations and initial positioning calculation values.

3. The high-precision positioning device for unmanned systems based on multi-sensor fusion according to claim 1, characterized in that: The inertial sensing module is a high update rate inertial measurement unit (IMU). The inertial measurement unit includes at least a three-axis gyroscope and a three-axis accelerometer, and its sampling frequency is not less than 1kHz. The signal output terminal of the inertial measurement unit is electrically connected to the corresponding input terminal of the global spatiotemporal synchronization unit, and is used to output the raw observation data of the three-axis angular rate and three-axis specific force of the unmanned system.

4. The high-precision positioning device for unmanned systems based on multi-sensor fusion according to claim 1, characterized in that: The environmental perception module includes a lidar submodule and a visual odometry submodule. The lidar submodule is a multi-line mechanical lidar or a solid-state lidar, used to output three-dimensional point cloud data of the surrounding environment of the unmanned system. The visual odometry submodule is a binocular global shutter camera, used to output synchronized binocular image data of the surrounding environment of the unmanned system. The signal output terminals of the lidar submodule and the visual odometry submodule are electrically connected to the corresponding input terminals of the global spatiotemporal synchronization unit, respectively.

5. The high-precision positioning device for unmanned systems based on multi-sensor fusion according to claim 1, characterized in that: The global spatiotemporal synchronization unit includes a hardware time synchronization submodule and a spatial calibration submodule; The hardware time synchronization submodule provides a unified hardware trigger signal and time reference for the inertial sensing module and the environmental sensing module based on the pulse-per-second (PPS) signal output by the GNSS positioning module and the Coordinated Universal Time (UTC) timestamp, so as to realize the time synchronization of all sensor data with a synchronization accuracy of not less than 1 microsecond. The spatial calibration submodule pre-stores the external parameter calibration results of each sensor relative to the vehicle coordinate system of the unmanned system. It is used to uniformly convert the observation data of all sensors to the vehicle coordinate system and complete the unification of the spatial coordinate reference.

6. The high-precision positioning device for unmanned systems based on multi-sensor fusion according to claim 1, characterized in that: The multi-source data preprocessing unit includes a GNSS data preprocessing submodule, an IMU data preprocessing submodule, a lidar data preprocessing submodule, and a visual data preprocessing submodule; The GNSS data preprocessing submodule is used to complete cycle slip detection and repair, ionospheric / tropospheric error correction, and outlier removal for GNSS observations; The IMU data preprocessing submodule is used to complete the zero bias correction, temperature compensation, gravity compensation, and IMU pre-integral calculation of the raw data of the inertial measurement unit; The lidar data preprocessing submodule is used to complete motion distortion correction, noise reduction, ground point segmentation, and feature point extraction of point cloud data; The visual data preprocessing submodule is used to perform distortion correction, stereo correction, feature point extraction and matching, and disparity calculation for binocular images; Specifically as follows: IMU Data Preprocessing Submodule: First, establish the IMU measurement model: In the formula, This represents the true angular velocity in the b-frame. For the true comparison in the n-system, Let be the gravitational acceleration vector in the n-system. After completing the zero-bias correction and temperature compensation of the raw data, pre-integration calculation is performed on the IMU data to obtain the relative pose pre-integral quantity between two keyframes i and j. The discrete-time pre-integration formula is as follows: In the formula, , , These are the pre-integral values ​​for rotation, velocity, and position between two keyframes. Let be the IMU sampling time step, and Exp(⋅) be the exponential mapping from the Lie algebra to the Lie group; Simultaneously, the error propagation covariance matrix of the pre-integrated quantity is calculated to provide a noise weighting benchmark for subsequent fusion solution. GNSS data preprocessing submodule: The Mel-Wibner combination method (MW combination method) is used to complete carrier phase cycle slip detection and repair. The formula for the MW combination observation value is: In the formula, , For GNSS dual-frequency carrier frequencies, , These are dual-frequency carrier phase observations. , These are dual-frequency pseudorange observations. When the combined difference of the MW values ​​of consecutive observations exceeds 4 times the root mean square error, a cycle slip is determined to have occurred, and a polynomial fitting method is used to complete the cycle slip repair. The Klobuchar model was used for ionospheric delay correction, and the Saastamoinen model was used for tropospheric delay correction, thus eliminating the influence of atmospheric errors on GNSS observations. LiDAR data preprocessing submodule: Based on synchronized IMU high-frequency pose data, point cloud motion distortion correction is performed. The correction formula is as follows: In the formula, The coordinates of the original distorted point cloud. The corrected point cloud coordinates, , The vehicle pose transformation matrix at point cloud sampling time k relative to frame start time 0 is obtained through high-frequency recursion by the IMU. After correction, statistical filtering is used to remove outliers, and the Random Sampling Consensus Algorithm (RANSAC) is used to segment ground points, extract corner and surface features, and output standardized point cloud data. Visual data preprocessing submodule: Based on the distortion coefficients inherent in the binocular camera, image distortion correction and stereo rectification are performed, and the corrected binocular image satisfies the epipolar horizontal alignment constraint; the depth values ​​of feature points are obtained through disparity calculation, and the depth transformation formula is as follows: In the formula, z is the depth value of the feature point, f is the camera focal length, b is the baseline length of the binocular camera, and d is the binocular parallax value of the feature point. Image feature point extraction and matching are performed using the directional FAST and rotation BRIEF (ORB) algorithms. The relative pose between adjacent frames is calculated using the perspective n-point algorithm (PNP algorithm), and visual odometry observations are output.

7. The high-precision positioning device for unmanned systems based on multi-sensor fusion according to claim 1, characterized in that: The hierarchical fusion solution unit includes a primary loosely coupled fusion submodule, a secondary tightly coupled fusion submodule, and a global pose optimization submodule. The primary loosely coupled fusion submodule is based on the extended Kalman filter (EKF) framework. It uses GNSS positioning results as observations and IMU mechanical recursion results as state predictions to complete GNSS / IMU loosely coupled combined positioning and output high-frequency pose initial values. The secondary tightly coupled fusion submodule is based on the Factor Graph Optimization (FGO) framework, which constructs a unified factor graph from IMU pre-integration factors, GNSS pseudorange / carrier phase observation factors, lidar point cloud matching factors, and visual odometry factors, to complete the tightly coupled joint solution of multi-source data. The global pose optimization submodule is used to construct a global pose graph based on LiDAR or visual loop closure detection results and perform nonlinear optimization to eliminate the cumulative drift error of the localization results.

8. The high-precision positioning device for unmanned systems based on multi-sensor fusion according to claim 1, characterized in that: The fault diagnosis and system reconfiguration unit includes a sensor fault detection submodule, a fusion status monitoring submodule, and a fusion strategy reconfiguration submodule. The sensor fault detection submodule determines in real time whether the observation data of each sensor is invalid, abnormal or interfered with, based on preset observation thresholds, data statistical characteristics and chi-square test criteria. The fusion status monitoring submodule is used to monitor the convergence, confidence and residual distribution of the fusion solution results in real time, and to determine whether the solution status is abnormal. The fusion strategy reconfiguration submodule is used to dynamically remove the observation data of the failed sensor when single or multiple sensor data fail, and switch to the optimal fusion framework and solution strategy corresponding to the remaining valid sensor to ensure the continuity and reliability of the positioning results. Specifically as follows: Sensor fault detection submodule: Employs the normalized residual chi-square test criterion to detect the validity of sensor observation data in real time. The test formula is: In the formula, Let k be the chi-square test statistic at time k. To observe the residuals, The residual covariance matrix is... The chi-square distribution threshold corresponding to degrees of freedom n and confidence level α. when > When this happens, the sensor is determined to have abnormal or malfunctioning observation data. The fusion status monitoring submodule monitors in real time the convergence of the filtering covariance matrix, the residual distribution of factor graph optimization, and the confidence level of the positioning results. When the covariance matrix continues to diverge or the optimization residual exceeds the threshold, the fusion solution status is determined to be abnormal. The fusion strategy reconfiguration submodule dynamically adjusts the objective function of factor graph optimization based on fault detection results, removes factor terms corresponding to failed sensors, and realizes dynamic reconfiguration of the fusion framework. The reconfigured objective function is as follows: In the formula, Fvalid is the set of factors corresponding to effective sensors. For factor weights, the weights corresponding to the failed sensors. =0, the effective sensor dynamically adjusts the weights based on the observation quality.

9. The high-precision positioning device for unmanned systems based on multi-sensor fusion according to claim 1, characterized in that: It also includes a high-precision map matching unit. The input end of the high-precision map matching unit is electrically connected to the pose output end of the hierarchical fusion solution unit, and the output end of the high-precision map matching unit is electrically connected to the input end of the positioning result output unit. The high-precision map matching unit is used to match and correct the pose result obtained by fusion solution with the preloaded high-precision prior map, and output the optimized global pose result.

10. The high-precision positioning device for unmanned systems based on multi-sensor fusion according to any one of claims 1 to 9, characterized in that: Unmanned systems include at least one of the following: driverless vehicles, unmanned delivery robots, industrial autonomous mobile robots, drones, and unmanned ships.

Images