Multi-source fusion navigation positioning method and system for unmanned vehicle in complex terrain

Abstract

The invention discloses a multi-source fusion navigation positioning method and system for an unmanned vehicle in a complex terrain, and relates to the technical field of high-precision navigation positioning, and the method comprises the steps: building a local coordinate system according to the initial pose of the unmanned vehicle, and carrying out the coordinate alignment and IMU initial pose alignment and course alignment through differential satellite positioning; angular velocity and specific force data of IMU initial attitude alignment and course alignment are extracted, strapdown inertial analysis is carried out, and a continuous prediction state under a local coordinate system is generated to serve as a dominant state quantity; and constructing an observation factor graph according to multi-source joint observation, carrying out global joint optimization by adopting an incremental factor elimination method, and outputting six-degree-of-freedom pose estimation under a local coordinate system. According to the method, cross-modal screening is carried out on the pose increment matched with the local map based on the time-varying observation confidence, multi-source joint observation is generated, automatic optimization of high-consistency observation is achieved, and the stability and continuity of high-precision positioning under the complex terrain are enhanced.

Description

Technical Field

[0001] This invention relates to the field of high-precision navigation and positioning technology, and in particular to a multi-source fusion navigation and positioning method and system for unmanned vehicles in complex terrain. Background Technology

[0002] In recent years, the rapid development of high-precision positioning for unmanned vehicles in complex terrain environments has led to multi-source fusion methods gradually becoming the mainstream research direction. Based on differential satellite positioning, IMU, LiDAR, and visual sensors, pose calculation is achieved through a state estimation and graph optimization framework, achieving success in structured roads and some unstructured scenarios. A loosely coupled and tightly coupled fusion structure is employed, along with backend optimization using observation factor graphs, improving overall robustness and accuracy.

[0003] However, existing methods still struggle to effectively address the issue of drastic fluctuations in confidence levels from multiple sources under dynamic and complex terrain conditions. In scenarios with occlusion, dust, weak texture, and strong motion interference, differences in the observation quality of various sensors and positioning failures prevent the real-time reflection of observation reliability, leading to pose estimation drift and divergence, which severely impacts navigation continuity and positioning stability. Summary of the Invention

[0004] In view of the aforementioned existing problems, the present invention is proposed.

[0005] Therefore, this invention provides a multi-source fusion navigation and positioning method for unmanned vehicles in complex terrain to solve the problems of high-precision positioning failure and pose estimation drift caused by severe fluctuations.

[0006] To solve the above-mentioned technical problems, the present invention provides the following technical solution: In a first aspect, the present invention provides a multi-source fusion navigation and positioning method for unmanned vehicles in complex terrain. The method includes: establishing a local coordinate system based on the initial pose of the unmanned vehicle, and using differential satellite positioning for coordinate alignment and IMU initial attitude and heading alignment; extracting angular velocity and specific force data from the IMU initial attitude and heading alignment, and performing strapdown inertial analysis to generate a continuous predicted state in the local coordinate system as the dominant state variable; based on the time synchronization reference of the dominant state variable, acquiring raw observations from laser, vision, and differential satellites, and extracting signal quality indicators, environmental semantic labels, and data integrity rate to form an auxiliary observation representation; using a multi-dimensional dynamic coupling quantization method to perform nonlinear mapping on the auxiliary observation representation to generate a time-varying observation confidence level; based on the time-varying observation confidence level, performing cross-modal screening on the pose increment matching the local map to generate multi-source joint observations; constructing an observation factor map based on the multi-source joint observations, and using an incremental factor elimination method for global joint optimization to output a six-degree-of-freedom pose estimate in the local coordinate system.

[0007] As a preferred embodiment of the multi-source fusion navigation and positioning method for unmanned vehicles in complex terrain described in this invention, the steps of establishing a local coordinate system based on the initial pose of the unmanned vehicle and performing coordinate alignment and IMU initial attitude and heading alignment using differential satellite positioning are as follows: Based on the initial stationary state of the unmanned vehicle, and based on the position and initial heading information output by differential satellite positioning, the on-board IMU is initially aligned to obtain the initial attitude parameters and heading reference. Based on the initial attitude parameters and heading reference, and combined with the initial position output by differential satellite positioning, a local coordinate system is constructed for the vehicle's initial pose, obtaining a local coordinate system with the vehicle's initial position as the origin and the initial heading as the x-axis.

[0008] As a preferred embodiment of the multi-source fusion navigation and positioning method for unmanned vehicles in complex terrain described in this invention, the angular velocity and force data refer to the raw observation values ​​of the three-axis angular velocity and three-axis force continuously output by the on-board IMU from the start time.

[0009] As a preferred embodiment of the multi-source fusion navigation and positioning method for unmanned vehicles in complex terrain described in this invention, the step of generating continuous predicted states in a local coordinate system as the dominant state variable is as follows: Based on the local coordinate system established by the initial pose of the unmanned vehicle, the initial attitude and heading information of the IMU are used to perform initial attitude alignment and heading alignment of the IMU to obtain the initial attitude parameters of the aligned IMU. Based on the initial attitude parameters of the aligned IMU, strapdown inertial analysis is performed on the angular velocity and specific force data output by the IMU to obtain the continuous predicted state in the local coordinate system, which is then used as the dominant state variable.

[0010] As a preferred embodiment of the multi-source fusion navigation and positioning method for unmanned vehicles in complex terrain described in this invention, the specific steps for forming auxiliary observation representations are as follows: Based on the time synchronization reference of the dominant state variables, the raw observation data of lidar, vision and differential satellite are time-aligned to obtain time-aligned multi-source raw observation sequences. Based on time-aligned multi-source raw observation sequences, signal quality indicators, corresponding environmental semantic labels, and data integrity rates of each sensor are extracted, and multi-source feature fusion is performed to form a unified auxiliary observation representation.

[0011] As a preferred embodiment of the multi-source fusion navigation and positioning method for unmanned vehicles in complex terrain described in this invention, the specific steps for generating time-varying observation confidence scores are as follows: Based on signal quality indicators, environmental semantic labels and data integrity rate in auxiliary observation representation, a multidimensional dynamic coupling quantization method is used to perform nonlinear mapping fusion to obtain a preliminary time-varying confidence score. Based on the preliminary time-varying confidence score, and combined with the dynamic change characteristics of the current environmental semantic labels and the short-term consistency trend of signal quality, confidence level calibration and correction are performed to generate time-varying observation confidence.

[0012] As a preferred embodiment of the multi-source fusion navigation and positioning method for unmanned vehicles in complex terrain described in this invention, the specific steps for generating multi-source joint observations are as follows: Based on the confidence level of time-varying observations, the pose increments generated by matching the lidar point cloud and visual image with the local map in the local coordinate system are compared spatially to obtain cross-modal residual evaluation. Based on cross-modal residual assessment and time-varying observation confidence, cross-modal screening of laser and visual observations is performed to generate multi-source joint observations.

[0013] As a preferred embodiment of the multi-source fusion navigation and positioning method for unmanned vehicles in complex terrain described in this invention, the observation factor graph is a graph optimization structure formed by constructing observation constraint factors based on six-degree-of-freedom poses in a local coordinate system as nodes and multi-source joint observations.

[0014] As a preferred embodiment of the multi-source fusion navigation and positioning method for unmanned vehicles in complex terrain described in this invention, the specific steps for outputting the six-degree-of-freedom pose estimation in the local coordinate system are as follows: Based on the observation factor map, incremental factor elimination is used to marginalize and sparsify the local subgraph containing new observations to obtain the optimized pose state. Based on the optimized pose state, the six-DOF pose in the local coordinate system is iteratively corrected, and the six-DOF pose estimate is output.

[0015] Secondly, this invention provides a multi-source fusion navigation and positioning system for unmanned vehicles in complex terrain, comprising: a calibration and alignment module, which establishes a local coordinate system based on the initial pose of the unmanned vehicle and performs coordinate alignment and IMU initial attitude and heading alignment using differential satellite positioning; an extraction and analysis module, which extracts angular velocity and specific force data of the IMU initial attitude and heading alignment and performs strapdown inertial analysis to generate continuous predicted states in the local coordinate system as the dominant state variables; an observation and characterization module, which collects raw observations from lidar, vision, and differential satellites based on the time synchronization reference of the dominant state variables and extracts signal quality indicators, environmental semantic labels, and data integrity rate to form auxiliary observation characterizations; a confidence mapping module, which uses a multi-dimensional dynamic coupling quantization method to perform nonlinear mapping on the auxiliary observation characterizations to generate time-varying observation confidence; a multi-source constraint module, which performs cross-modal geometric consistency constraints on the time-varying observation confidence using lidar and vision to generate multi-source joint observations; and an incremental optimization module, which constructs an observation factor map based on the multi-source joint observations, performs global joint optimization using an incremental factor elimination method, and outputs a six-degree-of-freedom pose estimate in the local coordinate system.

[0016] The beneficial effects of this invention are as follows: by using a multidimensional dynamic coupling quantization method to perform nonlinear mapping on the auxiliary observation characterization, time-varying observation confidence is generated, which realizes the characterization of the reliability of multi-source observations and provides a dynamic basis for cross-modal screening; by performing cross-modal screening on the pose increment matching the local map based on the time-varying observation confidence, multi-source joint observations are generated, which realizes the automatic selection of highly consistent observations and enhances the stability and continuity of high-precision positioning under complex terrain. Attached Figure Description

[0017] To more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings used in the following description of the embodiments will be briefly introduced. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0018] Figure 1 This is a flowchart of a multi-source fusion navigation and positioning method for unmanned vehicles used in complex terrain.

[0019] Figure 2 This is a schematic diagram of a multi-source fusion navigation and positioning system for unmanned vehicles used in complex terrain.

[0020] Figure 3 This is a flowchart of the local coordinate system and initial alignment.

[0021] Figure 4 This is a flowchart for extracting characterizations from multi-source observations. Detailed Implementation

[0022] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, the specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings.

[0023] Many specific details are set forth in the following description in order to provide a full understanding of the invention. However, the invention may also be practiced in other ways different from those described herein, and those skilled in the art can make similar extensions without departing from the spirit of the invention. Therefore, the invention is not limited to the specific embodiments disclosed below.

[0024] Secondly, the term "one embodiment" or "embodiment" as used herein refers to a specific feature, structure, or characteristic that may be included in at least one implementation of the present invention. The phrase "in one embodiment" appearing in different places in this specification does not necessarily refer to the same embodiment, nor is it a single or selective embodiment that is mutually exclusive with other embodiments.

[0025] Reference Figures 1-4 This is one embodiment of the present invention, which provides a multi-source fusion navigation and positioning method for unmanned vehicles in complex terrain, including the following steps: S1: Establish a local coordinate system based on the initial pose of the unmanned vehicle, and use differential satellite positioning to perform coordinate alignment and IMU initial attitude alignment and heading alignment.

[0026] S1.1: Based on the initial stationary state of the unmanned vehicle, and based on the position and initial heading information output by differential satellite positioning, perform initial attitude alignment on the onboard IMU to obtain initial attitude parameters and heading reference.

[0027] Furthermore, the initial position of the unmanned vehicle in the geographic coordinate system is obtained based on the position output by differential satellite positioning. At the same time, the initial heading direction of the unmanned vehicle is obtained based on the initial heading information output by differential satellite positioning. When the unmanned vehicle is stationary, the direction of the gravity component in the original three-axis specific force observation value output by the onboard IMU is used, combined with the initial heading direction, to obtain the initial attitude parameters of the onboard IMU in the local geographic coordinate system. The initial attitude alignment of the onboard IMU is completed, and the initial attitude parameters and heading reference are obtained.

[0028] It should be noted that differential satellite positioning refers to the process of differentially processing satellite observation data between the base station and the rover to eliminate and reduce common errors such as satellite orbital errors, clock errors, and ionospheric and tropospheric delays. The position output by differential satellite positioning is used to obtain the initial position of the unmanned vehicle in the geographic coordinate system. The output initial heading information is obtained through continuous high-precision position changes and serves as the initial heading direction of the unmanned vehicle, which is used to perform initial attitude alignment and heading alignment of the onboard IMU.

[0029] Differential satellite positioning uses a dual-antenna configuration. The absolute heading angle is calculated by the phase difference of the carrier waves received by the two antennas. The absolute heading angle does not depend on position changes and directly outputs the initial heading information when the vehicle is stationary.

[0030] Initial heading information refers to the absolute heading angle obtained by differential satellite positioning through dual-antenna carrier phase difference calculation when the unmanned vehicle is in an initial stationary state, and is used to perform initial attitude alignment and heading alignment on the vehicle-mounted IMU.

[0031] IMU initial attitude alignment includes IMU heading alignment. Attitude alignment refers to determining the IMU's initial three-dimensional attitude (including pitch, roll, and heading) in the local geographic coordinate system. Heading alignment refers to aligning the IMU's heading angle with the initial travel direction output by differential satellite positioning, and is a sub-step in attitude alignment.

[0032] Differential satellite positioning involves receiving satellite signals from a base station and analyzing common errors (including orbital errors, clock errors, and ionospheric and tropospheric delays). The error correction information is then sent to the rover on the unmanned vehicle. The rover combines its own observation data to obtain a high-precision position and derives the initial heading based on continuous high-precision position changes. The high-precision position determines the starting position of the unmanned vehicle in the geographic coordinate system, and the initial heading determines the initial heading direction of the unmanned vehicle. This is then combined with the direction of the gravity component in the three-axis specific force output by the IMU in the initial stationary state to obtain the initial attitude parameter alignment of the onboard IMU in the local geographic coordinate system.

[0033] S1.2: Based on the initial attitude parameters and heading reference, and combined with the initial position output by differential satellite positioning, a local coordinate system is constructed for the initial pose of the vehicle to obtain a local coordinate system with the initial position of the unmanned vehicle as the origin and the initial heading as the x-axis.

[0034] Furthermore, the initial position output by differential satellite positioning is used as the origin of the coordinate system. The initial heading direction of the unmanned vehicle, obtained from the initial attitude parameters and the heading reference, is set as the positive x-axis direction of the local coordinate system. The y-axis and z-axis are obtained according to the right-hand coordinate system, thus establishing a local coordinate system with the starting position of the unmanned vehicle as the origin and the initial heading as the x-axis direction.

[0035] The local coordinate system is a right-handed rectangular coordinate system with the starting position of the unmanned vehicle as the origin and the initial heading as the x-axis. The transformation relationship with the geographic coordinate system is obtained by the initial position (translation of the origin) and initial heading information (rotation around the vertical axis) provided by differential satellite positioning.

[0036] S2: Extract the angular velocity and specific force data of the initial attitude alignment and heading alignment of the IMU, and perform strapdown inertial analysis to generate continuous predicted states in the local coordinate system as the dominant state variables.

[0037] S2.1: Angular velocity and force data refer to the raw observation values ​​of the three-axis angular velocity and three-axis force continuously output by the on-board IMU from the start time.

[0038] Based on the local coordinate system established by the initial pose of the unmanned vehicle, the initial attitude and heading information of the IMU are used to perform initial attitude alignment and heading alignment, and the initial attitude parameters of the aligned IMU are obtained.

[0039] Furthermore, when the unmanned vehicle is initially stationary, the onboard IMU is initially aligned based on the position and initial heading information output by differential satellite positioning to obtain the initial attitude parameters and heading reference of the IMU. Based on the initial attitude parameters and heading reference, and combined with the initial position output by differential satellite positioning, a local coordinate system is established with the starting position of the unmanned vehicle as the origin and the initial heading as the x-axis. Under this local coordinate system, the initial position and initial heading information provided by differential satellite positioning are used as reference references to complete the alignment of the initial attitude parameters of the IMU, and the aligned initial attitude parameters of the IMU are obtained.

[0040] It should be noted that the unit of the raw triaxial force measurement is meters per second squared (m / s²), representing the combined force of non-gravitational acceleration and gravitational acceleration sensed by the IMU in its own coordinate system. The unit of triaxial angular velocity is radians per second (rad / s), representing the instantaneous angular rate of the IMU's rotation around its own three axes. All data are assumed to be synchronously output at the IMU sampling time and are unfiltered and unprocessed.

[0041] S2.2: Based on the initial attitude parameters of the aligned IMU, perform strapdown inertial analysis on the angular velocity and specific force data output by the IMU to obtain the continuous predicted state in the local coordinate system, and use it as the dominant state variable.

[0042] Furthermore, using the local coordinate system established by the initial pose of the autonomous vehicle as a reference, the original observation values ​​of the three-axis angular velocity and three-axis specific force output by the IMU are transformed from the IMU coordinate system to the local coordinate system. The attitude is updated based on the transformed angular velocity to obtain continuous attitude changes. The velocity and position are recursively integrated by combining the specific force data. In the analysis process at each moment, the pose state in the local coordinate system at the previous moment and the angular velocity and specific force data after coordinate transformation at the current moment are used to sequentially complete the attitude, velocity and position updates according to the strapdown inertial analysis process, generating continuous predicted states in the local coordinate system, which serve as the dominant state variables.

[0043] The position prediction formula for strapdown inertial analysis is: ; in, It is the first The position vector of the autonomous vehicle in the local coordinate system at any given time (unit: m). It is the first The velocity vector in the local coordinate system at any given time is obtained by aligning the IMU specific force and compensating for gravity (unit: m / s). It is the first The time length of each inertial navigation update cycle (i.e., from time t) arrive (Time interval, unit: s) It is a circular index. It is the velocity vector of the autonomous vehicle in the local coordinate system. It's time to update. It refers to the length of the navigation update cycle. It is the navigation update step index at the current moment.

[0044] It should be noted that, Indicates location, in meters. It is speed (in meters per second) and The sum of products (in seconds), with each term in meters, and the summation is still in meters. The dimensions of the position prediction formula for strapdown inertial analysis are consistent.

[0045] Specifically, strapdown inertial analysis refers to performing strapdown analysis on the three-axis angular velocity and three-axis specific force data output by the on-board IMU in the initial stationary state based on the initial attitude parameters of the aligned IMU. The pose state of the unmanned vehicle is then recursively obtained in the local coordinate system through inertial navigation, generating a continuous predicted state in the local coordinate system, which is then used as the dominant state variable.

[0046] The continuously predicted state consists of position, attitude, velocity, and optional IMU zero bias. The timestamp is determined by the IMU sampling time. The observations of each sensor are uniformly interpolated and resampled to this timestamp sequence, which serves as the time synchronization reference for multi-source fusion.

[0047] Strapdown inertial analysis refers to the process of transforming the original observations of the three-axis angular velocity and three-axis specific force output by the IMU from the IMU coordinate system to a local coordinate system with the initial position of the unmanned vehicle as the origin and the initial heading as the x-axis, based on the initial attitude parameters of the aligned IMU. In the local coordinate system, continuous attitude changes are obtained based on the transformed angular velocity, and combined with the specific force data, the velocity and position are recursively integrated at each moment based on the pose state of the previous moment.

[0048] Specifically, the initial attitude parameters are obtained based on the direction of the gravity component and the initial heading information in the initial static state. The current specific force observation value is aligned with the coordinates, the influence of gravity is removed to obtain the true acceleration, and the velocity is obtained by accumulating the true acceleration over time. The position is obtained by accumulating the velocity over time. By recursively updating the attitude, velocity and position in sequence, a continuous predicted state in the local coordinate system is generated as the dominant state variable.

[0049] Attitude update based on the transformed angular velocity refers to, in the local coordinate system, starting from the initial attitude parameters jointly determined by the direction of the gravity component in the three-axis specific force of the IMU in the initial static state and the initial heading information provided by differential satellite positioning, accumulating the raw observation values ​​of the three-axis angular velocity continuously output by the IMU at each moment to obtain the angular displacement relative to the initial attitude. The angular displacement, combined with the initial attitude parameters, is the continuous attitude change of the unmanned vehicle in the local coordinate system from the initial moment to the current moment.

[0050] The time accumulation of angular velocity is obtained by multiplying the angular velocity between adjacent sampling times by the corresponding time interval and summing them up, and the attitude at the current time is obtained by recursion, thus transforming the force data from the IMU coordinate system to the local coordinate system.

[0051] S3: Based on the time synchronization benchmark of the dominant state variables, raw observations from laser, visual and differential satellites are collected, and signal quality indicators, environmental semantic labels and data integrity are extracted to form auxiliary observation characterization.

[0052] S3.1: Based on the time synchronization reference of the dominant state variables, the raw observation data of lidar, vision and differential satellites are time-aligned to obtain a time-aligned multi-source raw observation sequence.

[0053] Furthermore, using the continuous predicted state in the local coordinate system generated by strapdown inertial analysis as the time reference, the point cloud frames of the lidar, the image frames of the vision system, and the position output of the differential satellite positioning are resampled using the corresponding timestamp sequence in the continuous predicted state. The original observation data of lidar, vision and differential satellite are aligned to the time node of the dominant state quantity on the time axis to form a time-consistent multi-source original observation sequence.

[0054] It should be noted that a point cloud frame of a lidar refers to a set of three-dimensional spatial points with timestamps output by the lidar within a single scanning cycle.

[0055] A visual image frame refers to a two-dimensional pixel image with a timestamp output by an onboard camera within a single exposure cycle.

[0056] Time alignment processing refers to aligning the point cloud frames of lidar, image frames of vision, and position outputs of differential satellite positioning with the time nodes of the dominant state variables based on the timestamp sequence provided by the dominant state variables, thereby forming time-synchronized multi-source observation data.

[0057] S3.2: Based on the time-aligned multi-source original observation sequences, extract the signal quality indicators, corresponding environmental semantic labels, and data integrity rates of each sensor, and perform multi-source feature fusion to form a unified auxiliary observation representation.

[0058] Furthermore, based on the time-aligned multi-source original observation sequence, signal quality indicators, corresponding environmental semantic labels, and data integrity rates are extracted from LiDAR, visual, and differential satellite observations, respectively. The signal quality indicators are obtained through LiDAR point cloud density, visual image gradient response intensity, and differential satellite positioning. Environmental semantic labels are obtained through semantic matching with the local map. The data integrity rate is obtained based on the ratio of the number of observation frames to the theoretical number of frames for each sensor within the time window. The indicators are aligned according to timestamps to form a vector with a consistent structure, and multi-source feature fusion is performed to form a unified auxiliary observation representation.

[0059] The formula for characterizing the data integrity fusion of multiple sensors is: ; in, It is the first The total observation time of multiple source sensors within a single synchronous observation window. It is the first Within a time window, the sensor Number of output frames, It is a sensor The theoretical time interval of a single frame It is a sensor modal index. It is the sequence number of the synchronous observation window.

[0060] It should be noted that, It is the total observation time, in seconds. Each term is the number of frames (dimensionless) multiplied by The time interval between single frames (seconds) and the result unit is seconds. The sum of these units is seconds. Therefore, the units of the formula for characterizing the data integrity fusion of multiple sensors are unified.

[0061] Specifically, signal quality indicators refer to the quantitative indicators of the current observations of each sensor extracted from lidar point cloud density, visual image gradient response intensity, and differential satellite positioning solution quality.

[0062] On the lidar side, the point cloud density (unit: points / square meter) within the scanning period indicates that the higher the value, the clearer the environmental geometry and the higher the observation quality.

[0063] On the visual side, the average gradient response intensity of a single frame image within a sliding time window (e.g., 5 frames) is dimensionless, ranging from 0 to 255. The larger the value, the richer the texture information and the stronger the feature extraction capability.

[0064] The differential satellite positioning side includes the positioning solution status (fixed solution is 1, floating solution is 0), the number of visible satellites (number of satellites, ≥6 is effective), and the position accuracy attenuation factor PDOP (dimensionless, ≤2 is preferred). The larger the solution status and the number of satellites, the better, and the smaller the PDOP, the better.

[0065] The semantic label of the corresponding environment refers to the environmental category identifier that represents the current terrain and scene category by semantically matching LiDAR point cloud and visual image with local map.

[0066] Data integrity rate refers to the ratio of the number of observation frames output by each sensor to the theoretically required number of frames within the time window provided by the dominant state variable.

[0067] S4: The multidimensional dynamic coupling quantization method is used to perform nonlinear mapping on the auxiliary observation characterization to generate time-varying observation confidence.

[0068] S4.1: Based on the signal quality index, environmental semantic label and data integrity rate in the auxiliary observation representation, a multi-dimensional dynamic coupling quantization method is used to perform nonlinear mapping fusion to obtain a preliminary time-varying confidence score.

[0069] Furthermore, signal quality indicators, environmental semantic labels, and data integrity rate in the auxiliary observation representation are used as multi-dimensional input variables. A multi-dimensional dynamic coupling quantization method is used for nonlinear mapping to dynamically fuse and map the coupling characteristics of each variable in the time dimension. By jointly representing the interactive changes of each variable over time, a preliminary time-varying confidence score corresponding to the current moment is generated.

[0070] Specifically, the multidimensional dynamic coupling quantization method uses the timestamp sequence of the dominant state variable as a benchmark and aligns the signal quality index, environmental semantic label and data integrity rate through a sliding time window. The window length covers multiple consecutive time nodes, and the update step size is consistent with the update frequency of the dominant state variable.

[0071] The multidimensional dynamic coupling quantization method uses a sliding time window to align signal quality indicators, environmental semantic labels, and data integrity rate in a time sequence, thereby obtaining a nonlinear interaction relationship in the time dimension. It maps the multidimensional features at each time step into a single confidence score. The input is the observable feature sequence of each sensor within the window, and the output is the preliminary time-varying confidence score at the current time step.

[0072] Dynamic fusion mapping takes the multi-source observation feature sequence aligned within the time window as input, and uses signal quality indicators, environmental semantic labels, and data integrity rate as time-series variables. It captures the co-evolution law of signal quality indicators, environmental semantic labels, and data integrity rate in the time dimension.

[0073] For example, when the environmental semantic label undergoes a sudden change (such as switching from a structured road to an unstructured field), if the signal quality index decreases simultaneously and the data integrity rate fluctuates more, it is judged as a high uncertainty scenario, and the confidence score is reduced accordingly; if the signal quality index, environmental semantic label and data integrity rate remain stable and synergistically enhance in continuous time steps, the confidence score is increased.

[0074] Multidimensional heterogeneous observation features include signal quality indicators, environmental semantic labels, and data integrity rate. Multidimensional means that the input contains multiple feature dimensions with different physical meanings and origins. Dynamic means that the mapping changes with the evolution trend of features within the time window. Coupling means that the feature dimensions are related to each other in the time series and are not isolated from each other. Quantization means that the output is a confidence score that participates in observation screening and optimization.

[0075] Nonlinear mapping fusion refers to the process of converting the input quantities of signal quality indicators, environmental semantic labels, and data integrity rate into a unified output quantity, namely a preliminary time-varying confidence score, through a multi-dimensional dynamic coupling quantization method. Instead of using fixed rule combinations, it obtains a fused representation of the multi-source heterogeneous observation characteristics based on the evolution direction and rate of the values ​​of signal quality indicators, environmental semantic labels, and data integrity rate within a continuous time window, including the temporal behavior of maintaining stability, monotonically increasing, abruptly decreasing, and periodically fluctuating.

[0076] Multidimensional heterogeneous observation features refer to the set of observation features extracted from lidar, vision, and differential satellites after time alignment processing, including signal quality indicators, environmental semantic labels, and data integrity rate.

[0077] S4.2: Based on the preliminary time-varying confidence score, and combined with the dynamic change characteristics of the current environmental semantic labels and the short-term consistency trend of signal quality, confidence calibration and correction are performed to generate time-varying observation confidence.

[0078] Furthermore, based on the preliminary time-varying confidence score, dynamic change characteristics are identified by the semantic category shift of environmental semantic labels within a continuous time window. At the same time, short-term consistency trends are analyzed for the fluctuations of signal quality indicators between adjacent time steps. Combining the dynamic change characteristics of environmental semantic labels with the short-term consistency trends of signal quality, the preliminary time-varying confidence score is calibrated and corrected through the nonlinear mapping structure of the multidimensional dynamic coupling quantization method to generate time-varying observation confidence.

[0079] It should be noted that dynamic change characteristics refer to the semantic category shifts of environmental semantic labels within a continuous time window, including the temporal evolution of category switching, switching frequency, and semantic complexity.

[0080] Fluctuation refers to the magnitude and direction of numerical changes in signal quality indicators between adjacent time steps, representing the stability and mutability of the indicator over a short period of time.

[0081] Short-term consistency trend refers to the stable and smooth evolution of signal quality indicators between adjacent time steps, maintaining similar levels and following the predicted direction of change in a short period of time.

[0082] The calibration correction is a nonlinear mapping of the multidimensional dynamic coupling quantization method. It takes the initial time-varying confidence score, the dynamic change characteristics of the environmental semantic label, and the short-term consistency trend of the signal quality as inputs, and corrects them according to the coupling relationship (nonlinear interaction) between them, and outputs the corrected time-varying observation confidence.

[0083] S5: Based on the confidence level of time-varying observations, perform cross-modal screening on the pose increments that match the local map to generate multi-source joint observations.

[0084] S5.1: Based on time-varying observation confidence, the pose increment generated by the local map is matched with the lidar point cloud and the visual image in the local coordinate system, and spatial consistency comparison is performed to obtain cross-modal residual evaluation.

[0085] Furthermore, the LiDAR point cloud is matched with the local map in the local coordinate system through point cloud registration to generate the LiDAR pose increment. The visual image is matched with the local map in the local coordinate system through image features to generate the visual pose increment. The LiDAR pose increment and the visual pose increment are compared in a six-degree-of-freedom space to obtain the differences in position and attitude dimensions, forming a cross-modal residual evaluation.

[0086] Specifically, cross-modal residual evaluation is the degree of spatial consistency between the pose increments obtained by comparing the lidar point cloud and the visual image for the same environmental structure in the current local coordinate system, and serves as the basis for cross-modal screening.

[0087] Pose increment refers to the six-degree-of-freedom pose correction output after the LiDAR point cloud and visual image are matched with the local map in the current local coordinate system. It is a pose correction vector obtained by optimizing point cloud registration and image feature matching based on the current pose predicted by the dominant state variables.

[0088] Point cloud registration refers to the process of aligning the LiDAR point cloud with the local map in a local coordinate system to generate LiDAR pose increments.

[0089] Image features refer to identifiable visual information extracted from visual images and used to match them with local maps to generate visual pose increments.

[0090] S5.2: Based on cross-modal residual assessment and time-varying observation confidence, cross-modal screening of laser and visual observations is performed to generate multi-source joint observations.

[0091] Furthermore, based on cross-modal residual assessment and time-varying observation confidence, spatial consistency comparison is performed on the pose increments generated by matching the local map with the lidar point cloud and visual image in the local coordinate system to obtain cross-modal residual assessment. By using the degree of geometric deviation between pose increments of each modality in the cross-modal residual assessment, combined with the nonlinear mapping information of signal quality indicators, environmental semantic labels and data integrity reflected by time-varying observation confidence, cross-modal screening is performed on the pose increments generated by lidar point cloud and visual image respectively. On the basis of screening, the pose increments of lidar point cloud and visual image with spatial consistency and corresponding time-varying observation confidence are retained, and the pose increments that do not meet the conditions are eliminated to generate multi-source joint observation.

[0092] It should be noted that the degree of geometric deviation refers to the degree of inconsistency in spatial position and orientation between the pose increment generated by matching the lidar point cloud and the visual image with the local map in the local coordinate system. By comparing the spatial coordinates, the relative deviation in the six-degree-of-freedom space is obtained, and the consistency and conflict of different modal observation information in geometric structure are obtained.

[0093] For example, when the pose increment (A) generated by matching the lidar point cloud with the local map indicates that the autonomous vehicle has moved 0.8 meters to the right front and rotated 3 degrees clockwise, while the pose increment (B) generated by matching the visual image with the local map indicates that the autonomous vehicle has moved 1.1 meters to the right front and rotated 7 degrees clockwise, a spatial comparison of pose increments A and B in the local coordinate system reveals significant deviations in translation direction and rotation angle. These deviations represent the degree of geometric bias and reflect the consistency of the observation information between the lidar point cloud and the visual image in spatial geometry over a given time period.

[0094] S6: Construct an observation factor map based on multi-source joint observations, and use incremental factor elimination method for global joint optimization to output a six-degree-of-freedom pose estimate in the local coordinate system.

[0095] S6.1: The observation factor graph is a graph optimization structure that uses six-degree-of-freedom poses in a local coordinate system as nodes and constructs observation constraint factors based on multi-source joint observations.

[0096] Based on the observation factor map, incremental factor elimination is used to marginalize and sparsify the local subgraph containing new observations to obtain the optimized pose state.

[0097] Furthermore, using the six-degree-of-freedom pose in the local coordinate system as nodes, an observation factor graph is formed based on the observation constraint factors constructed from multi-source joint observations. When a new observation is added, a local subgraph consisting of the current six-degree-of-freedom pose node and adjacent observation constraint factors is extracted. An incremental factor elimination method is used on the local subgraph to perform algebraic elimination on non-six-degree-of-freedom pose nodes in sequence, retaining the six-degree-of-freedom pose nodes in the current state. The sparse structure of the factor graph is used to compress the information bandwidth. During the elimination process, the state of the retained six-degree-of-freedom pose nodes is updated according to the observation constraint factors of the observation factor graph to obtain the optimized pose state.

[0098] Specifically, incremental factor elimination is an incremental state estimation method used for factor graph optimization. It performs local elimination only on the local subgraph of newly observed data, rather than resolving the entire factor graph.

[0099] Upon receiving a new observation constraint factor constructed from multi-source joint observations, the system identifies neighboring six-degree-of-freedom pose nodes directly connected to the current six-degree-of-freedom pose node and their corresponding observation constraint factors, forming a local subgraph. Algebraic elimination is performed on the six-degree-of-freedom pose nodes in the local subgraph that are not in the current time step, following the temporal and graph topological order. Prior information generated during elimination is retained as marginalization factors and injected into the optimization problem of the remaining six-degree-of-freedom pose nodes. Incremental factor elimination is performed on the local subgraph. Each six-degree-of-freedom pose node in the observation factor graph of the factor graph is only connected to pose nodes and their corresponding multi-source joint observation constraint factors in a few adjacent time steps. Observation constraint factors directly connected to the current six-degree-of-freedom pose node are retained. Algebraic elimination is performed on pose nodes that are not in the current time step, and prior information generated during elimination is injected into the remaining nodes as marginalization factors. This reduces complexity while optimizing the process, and the optimized pose state is output.

[0100] Marginalization and sparsification refers to the process of removing the six-DOF pose nodes in the local subgraph from the currently optimized subgraph through algebraic elimination during the incremental factor elimination method, and transforming the constraint information into marginalization factors for the retained six-DOF pose nodes. By utilizing the sparsity of the factor graph structure itself, only the connection between the six-DOF pose nodes and the observation constraint factors is retained, thus performing information marginalization and sparsification.

[0101] S6.2: Iteratively correct the six-degree-of-freedom pose in the local coordinate system based on the optimized pose state, and output the six-degree-of-freedom pose estimate.

[0102] Furthermore, the optimized pose state is used as the initial pose value for the iteration step and substituted back into the observation factor graph for factor residual linearization. The pose node state is updated based on the observation constraint factors constructed by multi-source joint observation. The elimination and optimization process in the incremental factor elimination method is repeated until the pose state converges, and the six-degree-of-freedom pose estimate in the local coordinate system is output.

[0103] Specifically, factor residual linearization refers to the process in the observation factor map where, based on the six-degree-of-freedom pose estimation in the local coordinate system, the deviation between the observation constraint factors and the current pose state is locally approximated for the observation constraint factors constructed by multi-source joint observations. This transforms the nonlinearity into linearity with respect to the six-degree-of-freedom pose increment. Based on the pose increment generated when matching the lidar point cloud and visual image with the local map, as well as the observation constraints provided by differential satellite positioning, the trend of the changing observation constraint factors is obtained in each iteration, centered on the optimized pose state. This trend is then subjected to marginalization and sparsification processing in the incremental factor elimination method to update the six-degree-of-freedom pose estimation.

[0104] Local approximation refers to linearizing the nonlinearity between the observation constraint factor and the six-degree-of-freedom pose near the current optimized pose state, and then performing iterative optimization using incremental factor elimination.

[0105] Nonlinearity refers to the geometric and observational constraints between the pose increment generated by multi-source joint observation (including lidar point clouds, visual images and differential satellite positioning) and the six-degree-of-freedom pose in the local coordinate system during the matching process with the local map.

[0106] Linearity refers to the expression obtained by locally approximating the nonlinear observation constraints around the current optimized pose state when using incremental factor elimination for optimization.

[0107] This embodiment also provides a multi-source fusion navigation and positioning system for unmanned vehicles in complex terrain, including: a calibration and alignment module, which establishes a local coordinate system based on the initial pose of the unmanned vehicle and performs coordinate alignment and IMU initial attitude and heading alignment using differential satellite positioning; an extraction and estimation module, which extracts angular velocity and specific force data for IMU initial attitude and heading alignment and performs strapdown inertial analysis to generate continuous predicted states in the local coordinate system as the dominant state variables; an observation characterization module, which collects raw observations from lidar, vision, and differential satellites based on the time synchronization reference of the dominant state variables and extracts signal quality indicators, environmental semantic labels, and data integrity rate to form auxiliary observation characterizations; a confidence mapping module, which uses a multi-dimensional dynamic coupling quantization method to perform nonlinear mapping on the auxiliary observation characterizations to generate time-varying observation confidence; a multi-source constraint module, which performs cross-modal geometric consistency constraints on the time-varying observation confidence using lidar and vision to generate multi-source joint observations; and an incremental optimization module, which constructs an observation factor map based on the multi-source joint observations, performs global joint optimization using an incremental factor elimination method, and outputs a six-degree-of-freedom pose estimate in the local coordinate system.

[0108] In summary, this invention achieves a characterization of the reliability of multi-source observations by employing a multi-dimensional dynamic coupling quantization method to perform nonlinear mapping on the auxiliary observation representation and generating time-varying observation confidence scores, thus providing a dynamic basis for cross-modal screening. Furthermore, by performing cross-modal screening on the pose increments matching the local map based on the time-varying observation confidence scores, multi-source joint observations are generated, enabling automatic optimization of highly consistent observations and enhancing the stability and continuity of high-precision positioning under complex terrain.

[0109] 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 technical solutions 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. A multi-source fusion navigation and positioning method for unmanned vehicles in complex terrain, characterized in that: include, A local coordinate system is established based on the initial pose of the unmanned vehicle, and differential satellite positioning is used for coordinate alignment and IMU initial attitude and heading alignment. Extract the angular velocity and specific force data of the initial attitude alignment and heading alignment of the IMU, and perform strapdown inertial analysis to generate continuous predicted states in the local coordinate system as the dominant state variables. Based on the time synchronization benchmark of the dominant state variables, raw observations from laser, visual and differential satellites are collected, and signal quality indicators, environmental semantic labels and data integrity are extracted to form auxiliary observation characterization. A multidimensional dynamic coupling quantization method is used to perform nonlinear mapping on the auxiliary observation characterization to generate time-varying observation confidence. Based on the confidence level of time-varying observations, cross-modal screening is performed on the pose increments that match the local map to generate multi-source joint observations; An observation factor map is constructed based on multi-source joint observations. An incremental factor elimination method is used for global joint optimization, and a six-degree-of-freedom pose estimate in the local coordinate system is output.

2. The multi-source fusion navigation and positioning method for unmanned vehicles in complex terrain as described in claim 1, characterized in that: The process of establishing a local coordinate system based on the initial pose of the unmanned vehicle, and using differential satellite positioning for coordinate alignment and IMU initial attitude and heading alignment, involves the following specific steps. Based on the initial stationary state of the unmanned vehicle, and based on the position and initial heading information output by differential satellite positioning, the on-board IMU is initially aligned to obtain the initial attitude parameters and heading reference. Based on the initial attitude parameters and heading reference, and combined with the initial position output by differential satellite positioning, a local coordinate system is constructed for the vehicle's initial pose, obtaining a local coordinate system with the vehicle's initial position as the origin and the initial heading as the x-axis.

3. The multi-source fusion navigation and positioning method for unmanned vehicles in complex terrain as described in claim 2, characterized in that: The angular velocity and force data refer to the raw observation values ​​of the three-axis angular velocity and three-axis force continuously output by the on-board IMU from the start time.

4. The multi-source fusion navigation and positioning method for unmanned vehicles in complex terrain as described in claim 3, characterized in that: The process of generating continuous predicted states in the local coordinate system as the dominant state variables involves the following steps. Based on the local coordinate system established by the initial pose of the unmanned vehicle, the initial position and heading information of differential satellite positioning are used to perform initial attitude alignment and heading alignment of the IMU, and the initial attitude parameters of the aligned IMU are obtained. Based on the initial attitude parameters of the aligned IMU, strapdown inertial analysis is performed on the angular velocity and specific force data output by the IMU to obtain the continuous predicted state in the local coordinate system, which is then used as the dominant state variable.

5. The multi-source fusion navigation and positioning method for unmanned vehicles in complex terrain as described in claim 4, characterized in that: The specific steps for forming the auxiliary observation characterization are as follows: Based on the time synchronization reference of the dominant state variables, the raw observation data of lidar, vision and differential satellite are time-aligned to obtain time-aligned multi-source raw observation sequences. Based on time-aligned multi-source raw observation sequences, signal quality indicators, corresponding environmental semantic labels, and data integrity rates of each sensor are extracted, and multi-source feature fusion is performed to form a unified auxiliary observation representation.

6. The multi-source fusion navigation and positioning method for unmanned vehicles in complex terrain as described in claim 5, characterized in that: The specific steps for generating the time-varying observation confidence score are as follows: Based on signal quality indicators, environmental semantic labels and data integrity rate in auxiliary observation representation, a multidimensional dynamic coupling quantization method is used to perform nonlinear mapping fusion to obtain a preliminary time-varying confidence score. Based on the preliminary time-varying confidence score, and combined with the dynamic change characteristics of the current environmental semantic labels and the short-term consistency trend of signal quality, confidence level calibration and correction are performed to generate time-varying observation confidence.

7. The multi-source fusion navigation and positioning method for unmanned vehicles in complex terrain as described in claim 6, characterized in that: The specific steps for generating multi-source joint observations are as follows: Based on the confidence level of time-varying observations, the pose increments generated by matching the lidar point cloud and visual image with the local map in the local coordinate system are compared spatially to obtain cross-modal residual evaluation. Based on cross-modal residual assessment and time-varying observation confidence, cross-modal screening of laser and visual observations is performed to generate multi-source joint observations.

8. The multi-source fusion navigation and positioning method for unmanned vehicles in complex terrain as described in claim 7, characterized in that: The observation factor graph is a graph optimization structure that uses six-degree-of-freedom poses in a local coordinate system as nodes and constructs observation constraint factors based on multi-source joint observations.

9. The multi-source fusion navigation and positioning method for unmanned vehicles in complex terrain as described in claim 8, characterized in that: The specific steps for the six-DOF pose estimation in the output local coordinate system are as follows. Based on the observation factor map, incremental factor elimination is used to marginalize and sparsify the local subgraph containing new observations to obtain the optimized pose state. Based on the optimized pose state, the six-DOF pose in the local coordinate system is iteratively corrected, and the six-DOF pose estimate is output.

10. A multi-source fusion navigation and positioning system for unmanned vehicles in complex terrain, based on the multi-source fusion navigation and positioning method for unmanned vehicles in complex terrain as described in any one of claims 1 to 9, characterized in that: include, The calibration and alignment module establishes a local coordinate system based on the initial pose of the unmanned vehicle, and uses differential satellite positioning to perform coordinate alignment and IMU initial attitude alignment and heading alignment. The extraction and analysis module extracts the angular velocity and specific force data of the IMU's initial attitude alignment and heading alignment, and performs strapdown inertial analysis to generate continuous predicted states in the local coordinate system as the dominant state variables. The observation characterization module, based on the time synchronization benchmark of the dominant state variables, collects raw observations from lidar, vision and differential satellites, and extracts signal quality indicators, environmental semantic tags and data integrity rate to form auxiliary observation characterization. The confidence mapping module uses a multidimensional dynamic coupling quantization method to perform nonlinear mapping on the auxiliary observation representation and generate time-varying observation confidence. The multi-source constraint module applies cross-modal geometric consistency constraints on the confidence of time-varying observations using laser and vision methods to generate multi-source joint observations. The incremental optimization module constructs an observation factor map based on multi-source joint observations, and uses an incremental factor elimination method to perform global joint optimization, outputting a six-degree-of-freedom pose estimate in the local coordinate system.

Images