Unmanned system autonomous navigation method based on Beidou and multi-source information adaptive fusion

Abstract

The invention provides an unmanned system autonomous navigation method based on Beidou and multi-source information adaptive fusion, and belongs to the technical field of high-precision intelligent navigation. According to the invention, autonomous navigation is realized by constructing a three-layer navigation architecture of space-time alignment, intelligent fusion and behavior control. In the space-time alignment layer, performing time and space alignment on the acquired multi-source sensing data; in the intelligent fusion layer, environment types are identified through CNN, modeling is carried out on observation uncertainty of GNSS, LiDAR and IMU in combination with variational Bayesian reasoning, confidence weights are dynamically updated, and a layered federated filtering architecture is adopted to output a full-parameter navigation solution; and in the behavior control layer, the trigger threshold value and the safety margin of the behavior state are dynamically adjusted in the path planning execution process, the different behavior states are subjected to priority ranking and dynamic switching, linear speed and angular speed control instructions are output according to the behavior states, and autonomous navigation is achieved. The problems of low navigation precision, high fusion rigidity, decision disjunction and the like are solved.

Description

Technical Field

[0001] This invention belongs to the field of autonomous navigation for intelligent vehicles, and relates to an autonomous navigation method for unmanned systems based on BeiDou + multi-source information adaptive fusion. Background Technology

[0002] With the continuous development of intelligent and automated technologies, autonomous navigation systems have been widely used in fields such as driverless cars, drones, robots, and intelligent equipment. Achieving high-precision and high-reliability navigation and positioning capabilities is key to improving the autonomous operation capabilities of equipment. Traditional navigation methods mainly rely on Global Navigation Satellite Systems (GNSS), such as the BeiDou Navigation Satellite System (BDS), to provide absolute position information. As a satellite navigation system independently constructed and operated by my country, the BeiDou system has advantages such as global coverage and multi-frequency band service, and has broad application prospects in urban, rural, and remote areas.

[0003] However, relying solely on the BeiDou system for navigation and positioning has certain limitations. In environments with dense urban buildings, such as "urban canyons," tunnels, or underground spaces where GNSS signals are severely blocked or interfered with, the positioning accuracy and availability of the BeiDou system decrease significantly, failing to meet the requirements for high-reliability navigation. To overcome this problem, navigation methods that integrate information from multiple sensors have been gradually developed in recent years. Common auxiliary sensors include inertial measurement units (IMUs), lidar (LiDAR), wheel speedometers, and visual cameras. These sensors can provide information such as pose, velocity, and odometer readings, serving as an aid to or replacement for GNSS navigation.

[0004] While multi-source information fusion technology can improve the robustness and accuracy of navigation, traditional fusion algorithms often employ filters with fixed structures (such as Extended Kalman Filter (EKF) and Unscented Kalman Filter (UKF), lacking sufficient adaptability and robustness in the face of dynamic environmental changes, sensor quality fluctuations, or malfunctions. Furthermore, issues such as time synchronization between multi-source data, coordinate transformation errors, and sensor redundancy handling place higher demands on system design. Therefore, there is an urgent need for a multi-source information fusion navigation method and system with high accuracy, high robustness, and high adaptability, capable of adaptively adjusting the fusion strategy according to different environments and sensor states. Summary of the Invention

[0005] In view of this, the purpose of this invention is to provide an autonomous navigation method for unmanned systems based on BeiDou and multi-source information adaptive fusion. By constructing a three-layer architecture of "spatiotemporal alignment - intelligent fusion - behavior control", it solves key problems such as low navigation accuracy, strong fusion rigidity, and decision-making disconnect in complex environments.

[0006] To achieve the above objectives, the present invention provides the following technical solution:

[0007] An autonomous navigation method for unmanned systems based on BeiDou and multi-source information adaptive fusion is proposed. The method includes: constructing a three-layer navigation architecture of spatiotemporal alignment, intelligent fusion and behavior control to achieve autonomous navigation.

[0008] At the spatiotemporal alignment layer, the input GNSS, IMU, and LiDAR data are processed. Then, using the atomic time provided by the BeiDou B2b signal as a unified time base, time interpolation and resampling of the LiDAR and IMU data are performed through a synchronization mechanism. In the spatial dimension, an objective function is constructed using a multi-source sensor spatial transfer calibration method based on self-supervised learning. The objective function is iteratively optimized to obtain the IMU state sequence, LiDAR→GNSS extrinsic parameters, and IMU→GNSS extrinsic parameters. A unified reference coordinate framework is constructed in the WGS84 geocentric-ground-fixed coordinate system to achieve spatial alignment.

[0009] In the intelligent fusion layer, convolutional neural networks are used to identify environment types, extract semantic scene information, and adjust the prior distribution of sensor observation modeling based on the semantic scene information; variational Bayesian inference is combined to model the observation uncertainties of GNSS, LiDAR and IMU, and the confidence weights are dynamically updated; a hierarchical federated filtering architecture is used to output the full-parameter navigation solution.

[0010] At the behavior control layer, global path planning is performed based on the laser SLAM map and target point. At the same time, local path optimization algorithms are used to achieve obstacle avoidance and trajectory smoothing by combining dynamic obstacle information. In the decision execution stage, a state adaptive mechanism driven by multi-source navigation accuracy is introduced. The trigger threshold and safety margin of the behavior state are dynamically adjusted according to the real-time accuracy index of the full-parameter navigation solution. A state priority switching strategy that incorporates task intent is adopted. The task objective is transformed into a weight factor, and different behavior states are prioritized and dynamically switched. Linear velocity and angular velocity control commands are output according to the behavior state to achieve autonomous navigation.

[0011] Furthermore, the processing of input GNSS, IMU, and LiDAR data includes: for GNSS data, using WGS84 parameters to convert latitude and longitude into geodetic coordinates based on BeiDou-3 B1C / B2a dual-frequency signals and PPP-B2b corrections; for LiDAR data, using the NDT matching algorithm to extract road feature point cloud clusters; and for IMU six-axis data, using temperature-compensated mechanical choreography to calculate short-term high-precision attitude.

[0012] Furthermore, spatial alignment is performed based on the processed data using a multi-source sensor spatial transfer calibration method based on self-supervised learning: First, the absolute position solution provided by GNSS, the short-time high-frequency pose integration result output by IMU, and the relative motion estimate obtained by LiDAR based on point cloud matching are obtained respectively; Under a unified reference coordinate system, the absolute position of GNSS is used as a global constraint to construct the absolute position residual between the GNSS output and the IMU-LiDAR estimated position; at the same time, the predicted motion trajectory of IMU is compared with the relative motion obtained by LiDAR point cloud registration to construct the relative motion residual.

[0013] Subsequently, within the sliding window optimization framework, a weighted sum of the absolute position residual and the relative motion residual is jointly minimized to establish the objective function. The IMU state sequence, LiDAR→GNSS extrinsic parameters, and IMU→GNSS extrinsic parameters are used as optimization variables, and the parameters are continuously updated using nonlinear iterative optimization. The objective function is expressed as:

[0014]

[0015] In the formula, W represents the world coordinate system, G represents the GNSS antenna coordinate system, I represents the IMU coordinate system, and L represents the LiDAR coordinate system; J is the objective function, and minimizing J yields estimates of the IMU state sequence, LiDAR→GNSS extrinsic parameters, and IMU→GNSS extrinsic parameters; {T W←I (i)} represents the pose of the IMU relative to the world coordinate system W at time i, X G←I X represents the constant rigid body transformation from the IMU coordinate system to the GNSS antenna coordinate system. G←L This represents the constant rigid body transformation from the LiDAR coordinate system to the GNSS (BeiDou) antenna coordinate system; r i (G) This represents the absolute position residual of the GNSS. This indicates a set of time indices containing GNSS observations; W G The covariance matrix representing the GNSS residuals; ε represents the relative pose residual of the IMU. I Represents the set of IMU observation pairs; ε represents the relative pose residual of LiDAR. L Represents the set of LiDAR observation pairs; W I and W L Let i and j represent the covariance matrices of the IMU and LiDAR residuals, respectively; ij represents the relative change between time i and time j. ρ(·) represents the weighted L2 norm, and ρ(·) represents the robust kernel function.

[0016] Secondly, an online verification and update mechanism is introduced: when the new observation matches well with the residual model, the extrinsic parameters remain stable; when a systematic shift in the residual is detected, the online correction of the extrinsic parameters is triggered to ensure that the multi-source spatial alignment effect can be maintained with high consistency in long-term operation and complex environments.

[0017] Finally, after iterating to obtain the IMU state sequence, LiDAR→GNSS extrinsic parameters, and IMU→GNSS extrinsic parameters, a unified reference coordinate framework is constructed in the world coordinate system to achieve spatial alignment.

[0018] Furthermore, in the intelligent fusion layer, the current environment type is identified through a convolutional neural network. The environment types include open areas, urban canyons, and tunnel / indoor environments. Semantic scene features are extracted, and the environment classification results are subjected to stability filtering using a sliding window strategy to generate a probability vector s = [s1, s2, s3] that reflects the characteristics of the current scene. s1, s2, and s3 correspond to the credibility of the three environment types: open areas, urban canyons, and tunnel / indoor environments, respectively.

[0019] Based on environmental perception results, a probability distribution of the sensor observation model is established using the variational Bayesian method. The confidence weights of GNSS, lidar, and IMU are then dynamically updated using Bayesian inference, achieving fusion adaptive adjustment driven by environmental perception.

[0020] L(q)=E q(x,θ) [logp(z|x,θ)]-KL(q(x,θ)||p(x,θ|s))

[0021]

[0022] In the formula, L(q) represents the variational lower bound; q(x,θ) represents the variational distribution of the state variable x and the sensor parameter θ; z represents the fused observation set, including observations from GNSS, LiDAR, and IMU; p(z|x,θ) represents the observation likelihood function; p(x,θ|s) represents the joint prior distribution of state and parameters under the scene prior; KL(·) represents the Kullback-Leibler divergence; E q(x,θ) Denotes the mathematical expectation of the joint variational distribution q(x,θ); w n This represents the original confidence weight of the nth sensor. z represents the normalized weights; n Var represents the observation value of the nth sensor at the current moment. q (z n |x) represents the variance of the sensor observations estimated based on variational inference q under state variable x; θ n Var represents the observation model parameters of the nth sensor. q (θn ) represents the parameter θ n The variational posterior distribution variance; Res n f represents the observation residual of the nth sensor; k (s) represents the scene mapping function of the k-th adjustment factor, reflecting the degree of attention to the three uncertainties under different environments, k = 1, 2, 3; N represents the total number of sensors.

[0023] Furthermore, by combining normalized sensor confidence weights, a hierarchical federated filtering structure is adopted to obtain a full-parameter navigation solution that includes multi-source information;

[0024] In the hierarchical federated filtering structure, the bottom layer performs fast integral prediction of attitude and position based on IMU high-frequency data, and uses the median integration method to mitigate the drift accumulation error caused by gyroscope zero bias; the middle layer introduces a residual modeling mechanism between GNSS and laser SLAM, compares the position information output by the two in real time, constructs drift observations and implements dynamic error compensation to constrain the medium- and long-term navigation drift trend; the top layer uses a federated extended Kalman filter to fuse the estimation results of each layer with sensor observations, and outputs a full-parameter navigation solution including position, attitude, and velocity.

[0025]

[0026] In the formula, This represents the global fusion state estimation result, where N represents the total number of sensors. This represents the global prior state estimate. K represents the local state estimate of the nth sub-filter. n This represents the Kalman gain of the nth sub-filter.

[0027] Furthermore, at the behavior control layer, based on the full-parameter navigation solution and the laser SLAM map and target point settings, the global path is planned using the HyBrid A* algorithm. Then, combined with the obstacle point cloud, the obstacle avoidance and trajectory smoothing adjustment are achieved using the dynamic window method.

[0028] The execution path planning incorporates a multi-source navigation accuracy-driven state adaptive mechanism, dynamically adjusting the trigger thresholds and safety margins of the behavioral state based on the real-time accuracy metrics of the full-parameter navigation solution.

[0029]

[0030] In the formula, T ξ T represents the real-time trigger threshold for the behavior state ξ. ξ0 The baseline trigger threshold, k, represents the behavioral state ξ. ξ σ represents the sensitivity coefficient of the behavioral state ξ to changes in navigation accuracy. x σ yσ represents the standard deviation of the position in the X and Y directions, respectively. θ α represents the standard deviation of the attitude angle, and α represents the proportionality coefficient that converts the attitude error into the equivalent position error.

[0031] Furthermore, a state priority switching strategy incorporating task intent is introduced during the switching process of behavioral states. The current task mode is transformed into a quantifiable weight vector, and this vector is combined with multi-dimensional indicators to calculate the comprehensive priority score of each behavioral state. When multiple candidate behavioral states exist, the behavioral state with the highest score is executed first. The behavioral state with the highest score is represented as follows:

[0032]

[0033] In the formula, ξ * M represents the current selected action state to be executed. ξ S represents the score of behavioral state ξ in the task objective dimension. ξ E represents the score of behavioral state ξ on the security dimension. ξ This represents the score of behavioral state ξ on the efficiency dimension; w mission w represents the weight of the task objective. safety w represents the security weight. eff w represents the efficiency weight. mission +w safety +w eff =1.

[0034] The beneficial effects of this invention are as follows:

[0035] 1) This invention uses a multi-source sensor spatiotemporal alignment and residual optimization mechanism to dynamically separate multipath effects and random noise, effectively suppressing satellite positioning drift. It can maintain stable centimeter-level positioning capability even in areas with severe signal obstruction such as tunnels and urban canyons, where traditional navigation fails, thus solving the failure problem of a single Beidou system in complex environments.

[0036] 2) Based on real-time environmental perception, the weight of multi-source data fusion is intelligently adjusted to achieve smooth switching within 1.5 seconds after satellite signal interruption, which significantly improves the reliability of continuous navigation in complex scenarios.

[0037] 3) It adopts a hierarchical federated filtering architecture, which ensures navigation accuracy while being compatible with low-cost vehicle hardware, effectively reduces the consumption of computing resources, supports all-weather autonomous navigation in the absence of high-precision maps, and provides a cost-effective lightweight solution for intelligent vehicles such as driverless cars and drones.

[0038] Overall, this invention combines high precision, high robustness, and strong adaptability, and can meet the autonomous navigation needs in a variety of complex scenarios.

[0039] Other advantages, objectives, and features of the invention will be set forth in part in the description which follows, and in part will be apparent to those skilled in the art from the following examination, or may be learned from practice of the invention. The objectives and other advantages of the invention can be realized and obtained through the following description. Attached Figure Description

[0040] To make the objectives, technical solutions, and advantages of the present invention clearer, the preferred embodiments of the present invention will be described in detail below with reference to the accompanying drawings, wherein:

[0041] Figure 1 This is a schematic diagram of the autonomous navigation method for unmanned systems based on BeiDou + multi-source information adaptive fusion, provided in an embodiment of the present invention. Detailed Implementation

[0042] The following specific examples illustrate the implementation of the present invention. Those skilled in the art can easily understand other advantages and effects of the present invention from the content disclosed in this specification. The present invention can also be implemented or applied through other different specific embodiments, and various details in this specification can be modified or changed based on different viewpoints and applications without departing from the spirit of the present invention. It should be noted that the illustrations provided in the following embodiments are only schematic representations of the basic concept of the present invention. Unless otherwise specified, the following embodiments and features can be combined with each other.

[0043] The accompanying drawings are for illustrative purposes only and are schematic diagrams, not actual pictures. They should not be construed as limiting the invention. To better illustrate the embodiments of the invention, some parts in the drawings may be omitted, enlarged, or reduced, and do not represent the actual product dimensions. It is understandable to those skilled in the art that some well-known structures and their descriptions may be omitted in the drawings.

[0044] In the accompanying drawings of the embodiments of the present invention, the same or similar reference numerals correspond to the same or similar components. In the description of the present invention, it should be understood that if terms such as "upper," "lower," "left," "right," "front," and "rear" indicate the orientation or positional relationship based on the orientation or positional relationship shown in the drawings, they are only for the convenience of describing the present invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation. Therefore, the terms used to describe positional relationships in the drawings are only for illustrative purposes and should not be construed as limiting the present invention. For those skilled in the art, the specific meaning of the above terms can be understood according to the specific circumstances.

[0045] In this embodiment, BeiDou-3 satellites, a 128-line lidar, and an IMU are selected as data acquisition sensors. An algorithm is written in the ROS system to realize an autonomous navigation method for unmanned systems based on BeiDou and multi-source information adaptive fusion. This method first proposes a multi-source sensor spatial transfer calibration method based on self-supervised learning to achieve high-precision alignment of GNSS, IMU, and LiDAR data in the world coordinate system. Second, a multimodal adaptive fusion positioning method based on neural perception guidance and variational Bayesian inference is constructed. A convolutional neural network (CNN) is used to identify the environment type, extract semantic scene information, and adjust the prior distribution of sensor observation modeling accordingly. Variational Bayesian inference is combined to model the observation uncertainties of GNSS, lidar, and IMU, dynamically updating the confidence weight. Hierarchical federated filtering is used to optimize the global state estimation. Finally, a high-precision navigation solution and mission intent path planning and autonomous decision-making mechanism are designed to complete the linear velocity and angular velocity command output, achieving closed-loop autonomous navigation control. This invention has technical advantages such as high precision, adaptability, and full-process integration, and is applicable to various autonomous systems such as unmanned vehicles and surveying robots.

[0046] like Figure 1 As shown, the method specifically includes the following steps:

[0047] 1. Processing input data: Based on the BeiDou-3 B1C / B2a dual-frequency signal and PPP-B2b correction, latitude and longitude are converted into geodetic coordinates using WGS84 parameters; simultaneously, lidar point cloud data is processed, and road feature point cloud clusters are extracted using the NDT matching algorithm; at the same time, combined with IMU six-axis data, short-term high-precision attitude is calculated through temperature-compensated mechanical choreography.

[0048] Spatiotemporal alignment is achieved through a multi-source sensor spatiotemporal coordination method based on the BeiDou master clock reference. The atomic time provided by the BeiDou B2b signal is used as a unified time base, and time interpolation and resampling of lidar and IMU data are performed through a high-precision synchronization mechanism.

[0049] In the spatial dimension, a multi-source sensor spatial transfer calibration method based on self-supervised learning is proposed: In online self-supervised calibration, it is assumed that LiDAR and IMU are associated with the GNSS (BeiDou) reference system through unknown rigid body transformations. By constructing cross-sensor geometric / trajectory consistency residuals (GNSS absolute position residuals + LiDAR–IMU relative motion residuals), these self-supervised residuals are minimized in a sliding window. The constant rigid body external parameters of LiDAR→GNSS and IMU→GNSS are iteratively optimized from streaming data and verified and updated online.

[0050] A unified reference coordinate framework is constructed under the WGS84 geocentric-ground-fixed coordinate system to provide highly consistent and stable multi-source inputs for subsequent fusion positioning and path planning.

[0051] Specifically, step one includes:

[0052] S11: To obtain better point cloud data, the LiDAR is installed in the center of the roof of the intelligent connected vehicle, 1.99 meters above the ground, and collects surrounding point cloud data at a rate of 10 frames per second. To ensure that the origin of the geodetic coordinate system is consistent with the origin of the local two-dimensional coordinate system, the GNSS receiver is installed directly below the LiDAR, 1.6 meters above the ground, and collects positioning data at a rate of 10 frames per second; the inertial sensor is installed directly above the LiDAR, 2.2 meters above the ground, and collects six-axis data at a rate of 200 frames per second.

[0053] S12: Processes input data, converts latitude and longitude into geodetic coordinates based on BeiDou-3 B1C / B2a dual-frequency signals and PPP-B2b corrections, and uses WGS84 parameters; synchronously processes lidar point cloud data, and uses the NDT matching algorithm to extract road feature point cloud clusters; at the same time, it combines IMU six-axis data and calculates short-term high-precision attitude through temperature-compensated mechanical choreography.

[0054] Subsequently, time synchronization was performed on the multi-source inputs, using the atomic time t provided by the BeiDou PPP-B2b signal. BDS To establish a common benchmark, a synchronization mechanism is constructed between data frames from multiple sensor sources. For IMUs and LiDARs with asynchronous sampling rates, a fusion of linear interpolation and extrapolation is used to achieve time resampling.

[0055] For spatial synchronization, a multi-source sensor spatial transfer calibration method based on self-supervised learning is proposed: First, the absolute position solution provided by GNSS, the short-time high-frequency pose integration result output by IMU, and the relative motion estimate obtained by LiDAR based on point cloud matching are obtained respectively. Under a unified reference coordinate system, the absolute position of GNSS is used as a global constraint to construct the absolute position residual between the GNSS output and the IMU-LiDAR estimated position; simultaneously, the predicted motion trajectory of IMU is compared with the relative motion obtained by LiDAR point cloud registration to construct the relative motion residual, so as to characterize the difference between the two in local dynamics; then, in a sliding window optimization framework, the weighted sum of the above absolute position residual and relative motion residual is jointly minimized to establish the objective function, and the parameters are continuously updated using nonlinear iterative optimization methods (such as Levenberg-Marquardt or incremental methods based on graph optimization) with the IMU state sequence, LiDAR→GNSS extrinsic parameters and IMU→GNSS extrinsic parameters as optimization variables. As new data flows in, optimization iterates continuously within a sliding window, gradually converging and outputting constant rigid extrinsic parameters for LiDAR→GNSS and IMU→GNSS. Finally, an online verification and update mechanism is introduced: when new observations match the residual model well, the extrinsic parameters remain stable; when a systematic shift in the residuals is detected, online correction of the extrinsic parameters is triggered to ensure that the multi-source spatial alignment effect remains highly consistent even during long-term operation and in complex environments.

[0056] A unified reference coordinate framework is constructed under the world coordinate system to provide highly consistent and stable multi-source inputs for subsequent fusion positioning and path planning.

[0057] The specific formula for the objective function is as follows:

[0058]

[0059] Where W represents the world coordinate system, G represents the GNSS antenna coordinate system, I represents the IMU coordinate system, and L represents the LiDAR coordinate system. J represents the objective function (scalar), minimizing J yields estimates of the IMU state sequence and two constant rigid external parameters. W←I (i)} represents the pose of the IMU relative to the world coordinate system W at time i, X G←I X represents the constant rigid body transformation from the IMU coordinate system to the GNSS (BeiDou) antenna coordinate system. G←L This represents the constant rigid body transformation from the LiDAR coordinate system to the GNSS (BeiDou) antenna coordinate system. i (G) This represents the absolute position residual of the GNSS. This indicates a set of time indices containing GNSS observations. W GThe covariance matrix representing the GNSS residuals is used as a metric in the weighted norm. ε represents the relative pose residual of the IMU. I This represents the set of IMU observation pairs. ε represents the relative pose residual of LiDAR. L This represents the set of LiDAR observation pairs. W I and W L Let represent the covariance matrices of the IMU and LiDAR residuals, respectively, which are also used as a measure in the weighted norm. Let ij represent the relative change between time i and time j. Let denot be the weighted L2 norm, and ρ(·) denote the robust kernel function.

[0060] Second, in the intelligent fusion layer, a multimodal adaptive fusion localization method based on neural perception guidance and variational Bayesian inference mechanism is proposed: First, the current environment type (such as open area, urban canyon, tunnel, etc.) is automatically identified through convolutional neural network (CNN) and semantic scene features are extracted; then, the probability distribution of the sensor observation model is established by combining variational Bayesian method, and the confidence weights of GNSS, lidar and IMU are dynamically updated by Bayesian inference, so as to realize fusion adaptive adjustment driven by environmental perception.

[0061] In state estimation, a hierarchical federated filtering structure is adopted: the bottom layer performs high-frequency integral prediction using IMU, the middle layer combines GNSS and laser SLAM to construct a residual observation model, and uses variational Bayesian methods to jointly model and correct local drift and model uncertainty; the top layer fuses multi-source state variables through federated extended Kalman filtering (Federated EKF) to achieve global consistency optimization.

[0062] The final output is a 10Hz high-frequency full-parameter navigation solution, including real-time position, attitude, velocity and other state information, which improves the positioning accuracy and robustness of the system in complex dynamic environments.

[0063] Specifically, step two includes:

[0064] S21: The Environmental Perception and Neural Perception Engine (CNN) module extracts convolutional features from LiDAR projection map data to identify the type of external environment in which the vehicle is located in real time, such as complex scenes like open areas, urban canyons, tunnels / indoor environments, etc. This module uses a pre-trained lightweight convolutional neural network structure and combines a sliding window strategy to perform stability filtering on the environment classification results, thereby generating a probability vector s = [s1, s2, s3] that reflects the characteristics of the current scene, corresponding to the confidence level in three different environments, which is used to drive the dynamic adjustment of the confidence level of subsequent sensors.

[0065] S22: Based on the environmental perception results, the probability distribution of the sensor observation model is established by combining variational Bayesian methods. The confidence weights of GNSS, lidar, and IMU are dynamically updated using Bayesian inference, thereby realizing fusion adaptive adjustment driven by environmental perception, as shown in the following equation:

[0066] L(q)=E q(x,θ) [logp(z|x,θ)]-KL(q(x,θ)||p(x,θ|s)) (2)

[0067]

[0068] Where L(q) represents the Evidence Lower Bound (ELBO), used to optimize the variational distribution; q(x,θ) represents the variational distribution of the state variable x and the sensor parameter θ; z represents the fused observation set, including observations from GNSS, LiDAR, and IMU; p(z|x,θ) represents the observation likelihood function; p(x,θ|s) represents the joint prior distribution of state and parameters under scene priors; s represents the scene type label output by the CNN; KL(·) represents the Kullback-Leibler divergence, used to measure the difference between the approximate distribution and the true distribution; E q(x,θ) Denotes the mathematical expectation of the joint variational distribution q(x,θ); w n This represents the original confidence weight of the nth sensor. z represents the normalized confidence weights; n Var represents the current observation value of the nth sensor; q (z n |x) represents the variance of sensor observations estimated by variational inference q under state variable x; θ n Var represents the observation model parameters of the nth sensor. q (θ n ) represents the parameter θ n The variational posterior distribution variance; Res n The nth sensor's observation residual is represented by s; the scene type label is s; and f is f. k (s) represents the scene mapping function of the k-th adjustment factor, reflecting the degree of attention to the three uncertainties under different environments; N represents the total number of sensors (GNSS, LiDAR, IMU: N=3).

[0069] S23: The hierarchical federated filtering structure enables efficient decoupling and hierarchical fusion of multi-source information, improving the navigation accuracy and robustness of the system.

[0070] In the hierarchical federated filtering structure, the bottom layer performs high-frequency integral prediction of attitude and position based on IMU high-frequency data, and uses the median integration method to mitigate the drift accumulation error caused by gyroscope zero bias, ensuring stable inertial navigation capability in a short period of time. The middle layer introduces a residual modeling mechanism between GNSS and laser SLAM. By comparing the position solutions output by the two in a unified coordinate system, residuals are constructed and their statistical characteristics are established to characterize the consistency differences between GNSS and laser SLAM. On this basis, the residuals are further transformed into drift observations to estimate the cumulative drift error generated by laser SLAM during long-term operation, thereby realizing the dynamic correlation between the laser SLAM solution and the absolute position of GNSS. Finally, dynamic error compensation is performed on the laser SLAM or fused solution based on the real-time estimated drift amount, maintaining the global consistency of the trajectory and the continuous stability of the navigation solution even when the GNSS signal is blocked or restored. The top layer employs a Federated Extended Kalman Filter (EKF) to fuse the estimation results from each layer with sensor observations, achieving globally consistent optimization of the system state and outputting a 10Hz high-frequency full-parameter navigation solution including position, attitude, and velocity, as shown in the following equation:

[0071]

[0072] in, This represents the global fusion state estimation result, where N represents the total number of sensors. This represents the global prior state estimate. K represents the local state estimate of the nth sub-filter. n This represents the Kalman gain of the nth sub-filter. P n H represents the covariance matrix estimated by the nth sub-filter. n This represents the observation matrix of the nth sub-filter. R represents the transpose of the observation matrix of the nth sub-filter. n Let represent the noise covariance matrix of the nth sensor.

[0073] Third, at the behavior control layer, relying on the full-parameter navigation solution, a path planning and autonomous decision-making mechanism based on high-precision navigation solution and task intent is constructed:

[0074] First, global path planning (Hybrid A*) is performed based on the laser SLAM map and target point settings. Then, combined with dynamic obstacle information, a local path optimization algorithm (dynamic window method) is used to achieve obstacle avoidance and trajectory smoothing adjustment. In the decision-making and execution stage, a state adaptive mechanism driven by multi-source navigation accuracy is introduced. Based on the real-time accuracy index of the full-parameter navigation solution, the trigger thresholds and safety margins of behaviors such as lane changing, deceleration, and obstacle avoidance are dynamically adjusted. At the same time, a state priority switching strategy incorporating task intent is adopted, transforming task objectives (such as rapid arrival, energy-saving driving, and risk minimization) into weight factors to prioritize and dynamically switch different behavioral states. Finally, linear velocity and angular velocity control commands are output to achieve end-to-end autonomous navigation closed loop.

[0075] Specifically, step three includes:

[0076] S31: Relying on the high-frequency full-parameter navigation solution output by the intelligent fusion layer, utilizing Hybrid A * The algorithm constructs a highly feasible path with limited curvature based on a high-precision map and navigation target points. Then, during local path planning, it combines the vehicle's current position with the obstacle point cloud, uses a dynamic window method to calculate the current controllable speed space, and performs real-time trajectory smoothing and obstacle avoidance.

[0077] S32: In the decision-making and execution phase, the triggering conditions for behavioral states are adaptively adjusted based on dynamic changes in navigation accuracy. For example, when position accuracy decreases or attitude drift increases, the trigger distance for lane changing or obstacle avoidance is automatically increased, and deceleration operations are performed earlier, thereby improving operational safety margins. Conversely, when navigation accuracy is high, the triggering conditions can be appropriately relaxed to improve the efficiency and comfort of path tracking. As shown below:

[0078]

[0079] Among them, T ξ T represents the real-time trigger threshold for a behavioral state ξ (such as lane change, deceleration, obstacle avoidance). ξ0 k represents the baseline trigger threshold for behavior state ξ under calibration conditions. ξ σ represents the sensitivity coefficient of the behavioral state ξ to changes in navigation accuracy. x σ y σ represents the standard deviation of the position in the X and Y directions, respectively. θ α represents the standard deviation of the attitude angle, and α represents the proportionality coefficient that converts the attitude error into the equivalent position error.

[0080] S33: Finally, a state priority switching strategy incorporating task intent is introduced during state transitions. This strategy transforms the current task mode (such as rapid arrival, energy-saving driving, risk minimization, etc.) into a quantifiable weight vector, which is then combined with multi-dimensional indicators such as safety and efficiency to calculate the comprehensive priority score of each behavioral state. When multiple alternative behavioral states exist (such as both lane changing and deceleration can solve the obstacle problem), the behavioral state with the highest score is executed first, thereby achieving dynamic optimization of decision-making and task consistency. Ultimately, after the above state decision processing, precise linear and angular velocity control commands are generated to drive the vehicle to execute the end-to-end autonomous navigation closed loop. As shown in the following equation:

[0081]

[0082] Where, ξ * M represents the current selected action state to be executed. ξ S represents the score (between 0 and 1) of the behavioral state ξ in the task objective dimension. ξ E represents the score (between 0 and 1) of the behavioral state ξ on the safety dimension. ξ The score (between 0 and 1) for behavioral state ξ on the efficiency dimension reflects the positive contribution of behavioral state ξ to arrival time or path quality. mission w represents the task objective weight (between 0 and 1). safety Represents the security weight (between 0 and 1), w eff w represents the efficiency weight (between 0 and 1). mission +w safety +w eff =1.

[0083] In summary, this invention provides an autonomous navigation method for unmanned systems based on BeiDou and multi-source information adaptive fusion. By dynamically separating multipath effects and random noise, it effectively suppresses satellite positioning drift and maintains stable centimeter-level positioning capability in areas where traditional navigation fails, such as tunnels and urban canyons. Simultaneously, based on real-time environmental perception, it intelligently adjusts the fusion weights of multi-source data, achieving smooth switching within 1.5 seconds after satellite signal interruption, thus improving the reliability of continuous navigation in complex scenarios. Furthermore, this invention's innovative filtering architecture is compatible with low-cost vehicle hardware, reducing computational power consumption and supporting all-weather autonomous navigation without high-precision maps, providing a lightweight technical solution for intelligent vehicles.

[0084] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit it. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can be made to the technical solutions of the present invention without departing from the spirit and scope of the present invention, and all such modifications or substitutions should be covered within the scope of the claims of the present invention.

Claims

1. An autonomous navigation method for unmanned systems based on BeiDou + multi-source information adaptive fusion, characterized in that, To achieve autonomous navigation, a three-layer navigation architecture consisting of spatiotemporal alignment, intelligent fusion, and behavioral control is constructed. At the spatiotemporal alignment layer, the input GNSS, IMU, and LiDAR data are processed. Then, using the atomic time provided by the BeiDou B2b signal as a unified time base, time interpolation and resampling of the LiDAR and IMU data are performed through a synchronization mechanism. In the spatial dimension, an objective function is constructed using a multi-source sensor spatial transfer calibration method based on self-supervised learning. The objective function is iteratively optimized to obtain the IMU state sequence, LiDAR→GNSS extrinsic parameters, and IMU→GNSS extrinsic parameters. A unified reference coordinate framework is constructed in the WGS84 geocentric-ground-fixed coordinate system to achieve spatial alignment. In the intelligent fusion layer, convolutional neural networks are used to identify environment types, extract semantic scene information, and adjust the prior distribution of sensor observation modeling based on the semantic scene information; variational Bayesian inference is combined to model the observation uncertainties of GNSS, LiDAR and IMU, and the confidence weights are dynamically updated; a hierarchical federated filtering architecture is used to output the full-parameter navigation solution. At the behavior control layer, global path planning is performed based on the laser SLAM map and target point. At the same time, local path optimization algorithms are used to achieve obstacle avoidance and trajectory smoothing by combining dynamic obstacle information. In the decision execution stage, a state adaptive mechanism driven by multi-source navigation accuracy is introduced. The trigger threshold and safety margin of the behavior state are dynamically adjusted according to the real-time accuracy index of the full-parameter navigation solution. A state priority switching strategy that incorporates task intent is adopted. The task objective is transformed into a weight factor, and different behavior states are prioritized and dynamically switched. Linear velocity and angular velocity control commands are output according to the behavior state to achieve autonomous navigation.

2. The autonomous navigation method for unmanned systems according to claim 1, characterized in that, The processing of input GNSS, IMU, and LiDAR data includes: for GNSS data, latitude and longitude are converted to geodetic coordinates using WGS84 parameters based on BeiDou-3 B1C / B2a dual-frequency signals and PPP-B2b corrections; for LiDAR data, road feature point cloud clusters are extracted using the NDT matching algorithm; and for IMU six-axis data, short-term high-precision attitude is calculated through temperature-compensated mechanical choreography.

3. The autonomous navigation method for unmanned systems according to claim 2, characterized in that, Spatial alignment is performed based on the processed data, achieved through a multi-source sensor spatial transfer calibration method based on self-supervised learning. First, the absolute position solution provided by GNSS, the short-time high-frequency pose integration result output by IMU, and the relative motion estimate obtained by LiDAR based on point cloud matching are obtained respectively. Under the unified reference coordinate system, the absolute position of GNSS is used as a global constraint to construct the absolute position residual between GNSS output and IMU-LiDAR estimated position. At the same time, the predicted motion trajectory of IMU is compared with the relative motion obtained by LiDAR point cloud registration to construct the relative motion residual. Subsequently, within the sliding window optimization framework, a weighted sum of the absolute position residual and the relative motion residual is jointly minimized to establish the objective function. The IMU state sequence, LiDAR→GNSS extrinsic parameters, and IMU→GNSS extrinsic parameters are used as optimization variables, and the parameters are continuously updated using nonlinear iterative optimization. The objective function is expressed as: In the formula, W represents the world coordinate system, G represents the GNSS antenna coordinate system, I represents the IMU coordinate system, and L represents the LiDAR coordinate system; J is the objective function, and minimizing J yields estimates of the IMU state sequence, LiDAR→GNSS extrinsic parameters, and IMU→GNSS extrinsic parameters; {T W←I (i)} represents the pose of the IMU relative to the world coordinate system W at time i, X G←I X represents the constant rigid body transformation from the IMU coordinate system to the GNSS antenna coordinate system. G←L This represents the constant rigid body transformation from the LiDAR coordinate system to the GNSS (BeiDou) antenna coordinate system; r i (G) This represents the absolute position residual of the GNSS. This indicates a set of time indices containing GNSS observations; W G The covariance matrix representing the GNSS residuals; ε represents the relative pose residual of the IMU. I Represents the set of IMU observation pairs; ε represents the relative pose residual of LiDAR. L Represents the set of LiDAR observation pairs; W I and W L Let i and j represent the covariance matrices of the IMU and LiDAR residuals, respectively; ij represents the relative change between time i and time j. ρ(·) represents the weighted L2 norm, and ρ(·) represents the robust kernel function. Secondly, an online verification and update mechanism is introduced: when the new observation matches well with the residual model, the extrinsic parameters remain stable; when a systematic shift in the residual is detected, the online correction of the extrinsic parameters is triggered to ensure that the multi-source spatial alignment effect can be maintained with high consistency in long-term operation and complex environments. Finally, after iterating to obtain the IMU state sequence, LiDAR→GNSS extrinsic parameters, and IMU→GNSS extrinsic parameters, a unified reference coordinate framework is constructed in the world coordinate system to achieve spatial alignment.

4. The autonomous navigation method for an unmanned system according to claim 1, characterized in that, In the intelligent fusion layer, the current environment type is identified through a convolutional neural network. The environment types include open areas, urban canyons, and tunnel / indoor environments. Semantic scene features are extracted, and the environment classification results are subjected to stability filtering using a sliding window strategy to generate a probability vector s = [s1, s2, s3] that reflects the characteristics of the current scene. s1, s2, and s3 correspond to the credibility of the three environment types: open areas, urban canyons, and tunnel / indoor environments, respectively.

5. The autonomous navigation method for an unmanned system according to claim 4, characterized in that, Based on environmental perception results, a probability distribution of the sensor observation model is established using the variational Bayesian method. The confidence weights of GNSS, lidar, and IMU are then dynamically updated using Bayesian inference, achieving fusion adaptive adjustment driven by environmental perception. L(q)=E q(x,θ) [logp(z|x,θ)]-KL(q(x,θ)||p(x,θ|s)) In the formula, L(q) represents the variational lower bound; q(x,θ) represents the variational distribution of the state variable x and the sensor parameter θ; z represents the fused observation set, including observations from GNSS, LiDAR, and IMU; p(z|x,θ) represents the observation likelihood function; p(x,θ|s) represents the joint prior distribution of state and parameters under the scene prior; KL(·) represents the Kullback-Leibler divergence; E q(x,θ) Denotes the mathematical expectation of the joint variational distribution q(x,θ); w n This represents the original confidence weight of the nth sensor. z represents the normalized weights; n Var represents the observation value of the nth sensor at the current moment. q (z n |x) represents the variance of the sensor observations estimated based on variational inference q under state variable x; θ n Var represents the observation model parameters of the nth sensor. q (θ n ) represents the parameter θ n The variational posterior distribution variance; Res n f represents the observation residual of the nth sensor; k (s) represents the scene mapping function of the k-th adjustment factor, reflecting the degree of attention to the three uncertainties under different environments, k = 1, 2, 3; N represents the total number of sensors.

6. The autonomous navigation method for an unmanned system according to claim 5, characterized in that, By combining normalized sensor confidence weights, a hierarchical federated filtering structure is used to obtain a full-parameter navigation solution that includes multi-source information; In the hierarchical federated filtering structure, the bottom layer performs fast integral prediction of attitude and position based on IMU high-frequency data, and uses the median integration method to mitigate the drift accumulation error caused by gyroscope zero bias; The middle layer introduces a residual modeling mechanism between GNSS and laser SLAM, compares the position information output by the two in real time, constructs drift observations and implements dynamic error compensation to constrain the navigation drift trend in the medium and long term; The top layer employs a federated extended Kalman filter to fuse the estimation results from each layer with sensor observations, outputting a full-parameter navigation solution including position, attitude, and velocity. In the formula, This represents the global fusion state estimation result, where N represents the total number of sensors. This represents the global prior state estimate. K represents the local state estimate of the nth sub-filter. n This represents the Kalman gain of the nth sub-filter.

7. The autonomous navigation method for an unmanned system according to claim 6, characterized in that, In the behavior control layer, based on the full-parameter navigation solution and the laser SLAM map and target point settings, the global path is planned using the HyBrid A* algorithm. Then, combined with the obstacle point cloud, the obstacle avoidance and trajectory smoothing adjustment are achieved using the dynamic window method.

8. The autonomous navigation method for an unmanned system according to claim 7, characterized in that, The execution path planning incorporates a multi-source navigation accuracy-driven state adaptive mechanism, dynamically adjusting the trigger thresholds and safety margins of the behavioral state based on the real-time accuracy metrics of the full-parameter navigation solution. In the formula, T ξ T represents the real-time trigger threshold for the behavior state ξ. ξ0 The baseline trigger threshold, k, represents the behavioral state ξ. ξ σ represents the sensitivity coefficient of the behavioral state ξ to changes in navigation accuracy. x σ y σ represents the standard deviation of the position in the X and Y directions, respectively. θ α represents the standard deviation of the attitude angle, and α represents the proportionality coefficient that converts the attitude error into the equivalent position error.

9. The autonomous navigation method for an unmanned system according to claim 8, characterized in that, During the transition between behavioral states, a state priority switching strategy incorporating task intent is introduced. The current task mode is transformed into a quantifiable weight vector, which is then combined with multi-dimensional indicators to calculate the comprehensive priority score for each behavioral state. When multiple candidate behavioral states exist, the behavioral state with the highest score is executed first. The highest-scoring behavioral state is represented as follows: In the formula, ξ * M represents the current selected action state to be executed. ξ S represents the score of behavioral state ξ in the task objective dimension. ξ E represents the score of behavioral state ξ on the security dimension. ξ This represents the score of behavioral state ξ on the efficiency dimension; w mission w represents the weight of the task objective. safety w represents the security weight. eff w represents the efficiency weight. mission +w safety +w eff =1.

Images