A neural network assisted robust multi-sensor factor graph fusion positioning method

Abstract

The application discloses a neural network assisted robust multi-sensor factor graph fusion positioning method. The application comprises the following steps: firstly, a sensor data set is acquired; then, a neural network assisted laser radar sensing module obtains a GNSS weight factor, and an IMU-wheel speed pre-integration model is established; then, a measurement model corresponding to each sensor is constructed, a sensor residual error model is established by using the measurement model of each sensor, a corresponding constraint relationship, a weight factor and the IMU-wheel speed pre-integration model; finally, a target function to be optimized is constructed according to the sensor residual error model, and after solving, the final navigation information of the carrier is obtained. The application reduces the measurement proportion of GNSS in the fault period. The IMU-wheel speed pre-integration model reduces the drift of the IMU, reduces the calculation time and improves the real-time performance of the algorithm. The measurement information of all sensors is used, the adverse effects of GNSS failure on the navigation system are effectively reduced, and the navigation precision is improved.

Description

Technical Field

[0001] This invention relates to a positioning method in the field of vehicle-mounted integrated navigation technology, and more particularly to a neural network-assisted robust multi-sensor factor graph fusion positioning method. Background Technology

[0002] Integrated inertial navigation systems typically use an inertial navigation system as the main system, with other navigation systems, such as celestial navigation, satellite navigation, and visual odometry, serving as auxiliary systems. These systems optimize the state variables of the integrated system to obtain high-precision navigation results. With the development of UAVs and unmanned vehicles, multi-source fusion navigation technology has compensated for the deficiencies of single navigation sensors, such as insufficient or unavailable information. Providing a more reliable, accurate, and adaptable integrated navigation system by combining information from multiple sensors is a research hotspot. Due to the inherent characteristics of sensors, external sensors such as Global Navigation Satellite Systems (GNSS) are prone to rejection or interruption due to multipath and non-line-of-sight interference, thus affecting the final navigation accuracy. Currently, most multi-sensor fusion technologies are based on the Extended Kalman Filter (EKF) method, which directly removes GNSS information in case of GNSS failure, thus discarding some navigation information. Furthermore, filtering-based methods can only iterate step-by-step due to the first-order Markov assumption, thus failing to fully utilize historical information. Factor graphs have been widely validated in the SLAM field due to their advantages such as multiple iterations, relinearization, and utilization of historical information. However, their application in navigation and positioning is limited, and existing systems are mostly vision-centric, failing to fully leverage the role of inertial navigation. Inertial measurement units (IMUs), due to their autonomous navigation capabilities, exhibit higher stability than other sensors. Furthermore, with the rapid development of MEMS-IMUs, the accuracy of the sensors themselves has significantly improved. Therefore, traditional multi-source fusion algorithms that do not consider Earth's rotation and Coriolis forces waste the inherent accuracy of the sensors.

[0003] Compared to the EKF single-iteration algorithm, the factor graph algorithm utilizes multiple measurement information from historical moments to simultaneously estimate state variables and obtain the global optimal solution. Its multiple iterations and relinearization properties effectively improve the accuracy of solving nonlinear problems, and its plug-and-play feature makes adding or removing sensor factors very convenient. However, traditional factor graph models are very time-consuming to solve using IMU update frequencies, and fault detection using hierarchical factor graphs involves a large computational burden. Summary of the Invention

[0004] For vehicle navigation systems, GNSS signals are easily blocked by tall buildings, trees, tunnels, and elevated roads, which can cause inaccurate positioning and affect the final navigation results. The purpose of this invention is to provide a factor graph fusion method with high real-time performance, high navigation accuracy, and strong robustness to overcome the shortcomings of existing methods and effectively solve the problems of decreased navigation accuracy and poor real-time performance caused by sensor failure. The method proposed in this invention has high reliability and strong scalability.

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

[0006] 1) The position, three-axis angular velocity, three-axis acceleration, wheel speed and point cloud information are obtained by the multiple sensors loaded in the carrier, and are recorded as sensor dataset;

[0007] 2) After feature extraction and matching of point cloud information, key frame pose information is obtained. The target neural network is selected according to the environment of the carrier. The key frame pose information is input into the target neural network and the GNSS fault probability is obtained and recorded as the weight factor W.

[0008] 3) Based on the triaxial angular velocity, triaxial acceleration and wheel speed, after compensating for Earth rotation and Coriolis force on the pre-integral model, an IMU-wheel speed pre-integral model considering Earth rotation and Coriolis force is obtained.

[0009] 4) Construct measurement models for each sensor, and use the measurement models of each sensor, corresponding constraint relationships, weighting factors W, and IMU-wheel speed pre-integration models considering Earth's rotation and Coriolis force to establish sensor residual models.

[0010] 5) Construct the objective function to be optimized based on the sensor residual model. After solving the objective function, obtain the navigation information of the carrier, thereby realizing the fusion positioning of the carrier.

[0011] In step 1), the position is obtained through the Global Navigation Satellite System, the three-axis angular velocity and three-axis acceleration are acquired by the Inertial Measurement Unit (IMU), the wheel speed is acquired by the wheel speed odometer, and the point cloud information is acquired by the sensing module.

[0012] The sensing module is one or more of lidar and millimeter-wave radar.

[0013] The formula for the IMU-wheel speed pre-integral model that considers Earth's rotation and Coriolis force is as follows:

[0014]

[0015]

[0016]

[0017] in, Let the rotation quaternions be at time k-1 and time k, respectively. Indicates the Kronecker product. Let t be the Earth's rotation term at time k-1. k Let k be the time. Represents the velocity of the carrier in the world coordinate system w. Let g be the velocity at time k-1 in the world coordinate system w, g be the local gravity, and Δt be the velocity. k-1，k The time increment from k-1 to k. Let w be the position of the carrier in the world coordinate system at time k. Let w be the position of the carrier in the world coordinate system at time k-1. These are the Gorbachev terms for velocity and position, respectively. Let be the rotational attitude matrix from the carrier coordinate system b to the world coordinate system w at time k-1. For attitude pre-integration, velocity pre-integration, and position pre-integration.

[0018] The sensor residual model includes prior residuals, pre-integrated residuals, GNSS residuals, and LiDAR residuals, specifically:

[0019] a priori residual e p Satisfy e p =r p -H p X, where r p H represents the prior factor residual. p Let X be the prior factor measurement matrix, and X be the navigation state variable;

[0020] Pre-integral residual e Pre The formula is as follows:

[0021]

[0022]

[0023] Where, r Pre () represents the pre-integration factor residual. The value is the pre-integrated measurement, and T is the transpose. Pre-integration for position The first-order Taylor expansion result, attitude pre-integration The first-order Taylor expansion result, Pre-integration of velocity The first-order Taylor expansion result, 2[ v The equivalent rotation vector for converting a quaternion at a small angle; The gyroscope zero biases at times k and k-1 are respectively. The zero bias of the table addition at times k and k-1 are respectively, s ok s ok-1 These are the wheel speed scaling factors at times k and k-1, respectively;

[0024] GNSS residual e GNSS The formula is as follows:

[0025]

[0026]

[0027] in, For antenna mast arm, The values ​​are measured latitude and longitude, and W is a weighting factor. r represents the rotation matrix from the world coordinate system w to the Earth coordinate system e. GNSS () represents the GNSS residual. These are GNSS measurements. For the predicted location;

[0028] LIDAR residual e LIDAR The formula is as follows:

[0029]

[0030]

[0031] Where, r LIDAR () represents the lidar residual. These are lidar measurement values. Represents planar feature points; This represents the three points that match the planar feature point and are closest to it, and these three points are not on the same plane; || represents the absolute value.

[0032] In step 5), the objective function to be optimized is constructed based on the sensor residual model. The formula for the objective function to be optimized is as follows:

[0033]

[0034] Where |||| is the squared Mahalanobis distance. The covariance is The squared Mahalanobis distance, The covariance is The squared Mahalanobis distance, The covariance is The squared Mahalanobis distance, where n is the number of pre-integration factors, m is the number of GNSS positioning factors, and l is the number of lidar factors.

[0035] In step 5), the LM nonlinear optimization method is used to solve the objective function to be optimized to obtain the navigation information of the carrier; sensor datasets are continuously acquired, and the objective function to be optimized is updated and solved based on the sensor datasets. The latest navigation information of the carrier is used as the final navigation information of the carrier to achieve the fusion positioning of the carrier.

[0036] The navigation information of the carrier includes time, carrier position, carrier speed, carrier attitude, gyroscope zero bias and accelerator zero bias, and wheel speed proportional factor.

[0037] The beneficial effects of this invention are:

[0038] This invention proposes a robust factor graph method for multi-sensor fusion of IMU-OD, GNSS, and LiDAR data suitable for vehicle-mounted applications. It improves the robustness of the algorithm by using the probability of GNSS faults obtained from CNN-assisted LiDAR perception as a weighting factor to adjust the proportion of GNSS measurements. A pre-integration model eliminates the need for the system to update at every IMU measurement moment, thus reducing the computation frequency and improving real-time performance. Introducing an NHC-constrained wheel speed model into the IMU pre-integration reduces IMU drift and improves computation accuracy. Based on the factor graph model, GNSS, IMU-OD, and LiDAR data are effectively fused, while avoiding the navigation information reduction problem caused by directly isolating fault factors.

[0039] This invention first proposes a robust factor graph method for multi-sensor fusion of IMU-OD, GNSS, and LIDAR in vehicle-mounted applications, which effectively solves the problems of poor real-time performance, large computational load, and accuracy loss in existing methods.

[0040] With the development of autonomous driving technology, data fusion positioning using multiple types of sensors such as GNSS, LiDAR, and inertial navigation has become possible. This invention introduces wheel speed data into the traditional model, uses NHC-constrained inertial navigation, and reduces computational load through a pre-integration model. Furthermore, in cases of GNSS rejection or obstruction, it utilizes LiDAR perception and neural network-assisted intelligent adjustment of the GNSS measurement ratio to reduce the adverse impact of malfunctions on navigation performance. Simultaneously, this neural network-assisted model exhibits excellent generalization capabilities. This invention primarily targets scenarios such as high-rise buildings, tree-lined areas, tunnels, and elevated roads, and can further generalize and expand inputs and classification labels to adapt to more complex environments.

[0041] It should be noted that not all technical solutions of this invention can achieve all the above-mentioned effects. Attached Figure Description

[0042] Figure 1 This is a diagram illustrating the overall framework of a neural network-assisted robust factor graph multi-sensor fusion method according to the present invention.

[0043] Figure 2 This is a flowchart of the steps of a neural network-assisted robust factor graph multi-sensor fusion method according to the present invention. Detailed Implementation

[0044] The accompanying drawings will be briefly introduced below in conjunction with the objectives and technical solutions of this invention, and the invention will be further described in detail with reference to specific embodiments and related formula derivations.

[0045] like Figure 1 As shown, this embodiment provides a neural network-assisted robust factor graph multi-sensor fusion positioning method. The overall framework is divided into a data input part, a CNN-assisted lidar acquisition of GNSS fault weight factors part, an IMU-OD pre-integration part, a factor graph construction and optimization solution part, and a navigation result output part.

[0046] The data input section primarily acquires data from various sensors, including IMU data from a three-axis gyroscope and a three-axis accelerometer, wheel speed data (output velocity), GNSS data (output position information), and LIDAR data (output point cloud information). In practice, system noise and system state variables all require initialization. Navigation system state variables include: vehicle position, vehicle velocity, vehicle attitude, gyroscope and accelerometer zero bias, and wheel speed scaling factor. Compared to traditional methods, this invention incorporates the wheel speed scaling factor into the navigation information to achieve more comprehensive model parameter estimation. System noise is selected based on the sensor characteristics. Vehicle position information, the latitude and longitude of the vehicle's geographical location, is initialized to the initial GNSS latitude and longitude values; velocity information, including eastward, northward, and azimuth velocities, is initialized to 0 m / s; attitude information, including pitch, roll, and yaw angles, is initialized to 0. The IMU output is incremental information, consisting of gyroscope increments and acceleration increments. Wheel speed is only measured using forward velocity, which leverages the NHC constraint. The NHC constraint assumes that lateral and vertical velocities are zero during vehicle movement, effectively suppressing inertial navigation divergence. The LIDAR point cloud information contains distance and angle information. This processed information can be used not only for subsequent pose fusion but also to reflect the surrounding environment of the moving vehicle, such as distance to a tall building or the height of trees.

[0047] The part of CNN-assisted LiDAR acquisition of GNSS fault weight factors mainly utilizes the above-mentioned processing of LiDAR point cloud information. The point cloud information is used to obtain pose information through feature extraction and matching. Through the perception of the surrounding environment, it is divided into two categories: 1. in the shaded area and high-rise building area; 2. in the tunnel and elevated area. According to different environmental classifications, the pose information is input into two different CNN networks for training, and finally outputs the GNSS fault probability as the weight factor W.

[0048] The IMU-OD pre-integration section employs a model that compensates for Earth's rotation and Coriolis forces, making good use of the advantages of inertial navigation and preserving its accuracy to the greatest extent possible. Introducing wheel speed data into the pre-integration process suppresses inertial navigation drift, further improving navigation accuracy.

[0049] The inertial motion model considering Earth's rotation and Coriolis force is shown in the following equation:

[0050]

[0051]

[0052]

[0053] in, Indicates the location of the carrier. Indicates the speed of the carrier. The derivative of the carrier velocity, Let f be the rotation matrix from the carrier coordinate system b to the world coordinate system w. b Indicates acceleration. Indicates attitude differential, The attitude is represented by w, the geographic coordinate system by e, and the inertial coordinate system by i. g is the gravitational vector in the geographic coordinate system. It is the projection of the world coordinate system w relative to the carrier coordinate system b in the carrier coordinate system b. It is the Earth's rotation rate in the w coordinate system.

[0054] The wheel speed model is as follows:

[0055]

[0056] in,, This indicates the actual wheel speed output. This represents the rotation of the carrier coordinate system b from time i-1 to time i. This represents the installation angle matrix between the inertial navigation system and wheel speed. For wheel speed lever arm, The output is the wheel speed in the carrier coordinate system b.

[0057] When a new IMU measurement arrives, the motion state is updated based on the discretized results of the above model. However, each measurement requires re-linearization, resulting in a large computational burden. To avoid repeated integration, pre-integration is introduced. Simultaneously, to suppress inertial navigation drift, wheel speed is incorporated into the pre-integration. Therefore, the formula for the IMU-wheel speed pre-integration model considering Earth's rotation and Coriolis force is as follows:

[0058]

[0059]

[0060]

[0061] in, Let the rotation quaternions be at time k-1 and time k, respectively. Indicates the Kronecker product. Let t be the Earth's rotation term at time k-1. k Let k be the time. Represents the velocity of the carrier in the world coordinate system w. Let g be the velocity at time k-1 in the world coordinate system w, g be the local gravity, and Δt be the velocity. k-1，k The time increment from k-1 to k. Let w be the position of the carrier in the world coordinate system at time k. Let w be the position of the carrier in the world coordinate system at time k-1. These are the Gorbachev terms for velocity and position, respectively. Let be the rotational attitude matrix from the carrier coordinate system b to the world coordinate system w at time k-1. For attitude pre-integration, velocity pre-integration, and position pre-integration.

[0062] The factor graph construction and optimization section mainly constructs sensor factors based on the sensor models and constraints, including prior factors, pre-integration factors, GNSS factors, and LiDRA factors. When establishing the factor graph model, IMU bias and wheel speed proportional factors are also added to the model according to the system state to improve navigation accuracy. Each sensor factor and its corresponding residual equation are established based on the measurement model and constraints.

[0063] The carrier's navigation information includes time, carrier position, carrier velocity, carrier attitude, gyroscope zero bias and accumulator zero bias, and wheel speed scaling factor. Carrier position information includes the latitude, longitude, and altitude of the carrier's geographical location; the carrier velocity is initialized to 0 m / s; and the carrier attitude represents the rotation from the IMU coordinate system to the geographic coordinate system, expressed as a quaternion.

[0064] Navigation information is represented as:

[0065] X = [x0, x1, ..., x] n ]

[0066] Where w represents the geographic coordinate system, b represents the carrier coordinate system, and coordinate transformation is the transformation from subscript to superscript. k Represents the real-time state at time k, including position. speed attitude Gyroscope zero bias b a Add zero bias b to the table g and wheel speed proportional factor so .

[0067] The sensor residual model includes prior residuals, pre-integrated residuals, GNSS residuals, and LiDAR residuals, specifically:

[0068] a priori residual e p Satisfy e p =r p -H p X, where r p H represents the prior factor residual. p Let X be the prior factor measurement matrix, and X be the navigation state variable;

[0069] Pre-integral residual e Pre The formula is as follows:

[0070]

[0071]

[0072] Where, r Pre () represents the pre-integration factor residual. The value is the pre-integrated measurement, and T is the transpose. Pre-integration for position The first-order Taylor expansion result, attitude pre-integration The first-order Taylor expansion result, Pre-integration of velocity The first-order Taylor expansion result, 2[ v The equivalent rotation vector for converting a quaternion at a small angle; The gyroscope zero biases at times k and k-1 are respectively. The zero bias of the table addition at times k and k-1 are respectively, s ok s ok-1 These are the wheel speed scaling factors at times k and k-1, respectively;

[0073] GNSS residual e GNSS The formula is as follows:

[0074]

[0075]

[0076] in, For antenna mast arm, The values ​​are measured latitude and longitude, and W is a weighting factor. r represents the rotation matrix from the world coordinate system w to the Earth coordinate system e. GNSS () represents the GNSS residual. These are GNSS measurements. For the predicted location;

[0077] LIDAR residual e LIDAR The formula is as follows:

[0078]

[0079]

[0080] Where, r LIDAR () represents the lidar residual. These are lidar measurement values. Represents planar feature points; This represents the three points that match the planar feature point and are closest to it, and these three points are not on the same plane; and Construct vector ji, Construct plane normal vector The dot product of || is the absolute value.

[0081] This embodiment involves specific steps as follows: Figure 2 As shown, the steps are as follows:

[0082] 1) The position, three-axis angular velocity, three-axis acceleration, wheel speed and point cloud information are obtained by the multiple sensors loaded on the carrier, and are recorded as sensor dataset; in specific implementation, the carrier is a vehicle.

[0083] In step 1), position is obtained through the Global Navigation Satellite System (GNSS). Three-axis angular velocity and three-axis acceleration are acquired through an Inertial Measurement Unit (IMU), which includes a three-axis gyroscope and a three-axis accelerometer, outputting the three-axis angular velocity and acceleration respectively. Wheel speed is acquired through a wheel speed-oddscopy system. Point cloud information is acquired through a sensing module, containing distance and angle information.

[0084] The sensing module is one or more of lidar and millimeter-wave radar.

[0085] 2) After feature extraction and matching of the point cloud information, keyframe pose information is obtained. A target neural network is selected based on the environment of the carrier. In practice, different pre-trained neural networks are set up for different environments. The input of the neural network is the keyframe pose information, and the output is the probability of GNSS failure. The neural network is specifically a CNN. It is divided into two categories according to different environments: 1. High-rise buildings and tree-lined areas; 2. Tunnels and elevated roads. The keyframe pose information is input into the target neural network, and the output is the GNSS failure probability, which is denoted as the weight factor W.

[0086] 3) Based on triaxial angular velocity, triaxial acceleration, and wheel speed, the pre-integral model is compensated for Earth rotation and Coriolis force to obtain an IMU-wheel speed pre-integral model that considers Earth rotation and Coriolis force. Angular velocity, acceleration, and velocity (i.e., IMU-wheel speed data) are used to obtain navigation information through the pre-integral model. Adding wheel speed to the pre-integral model can reduce IMU drift. Adding only forward wheel speed considers non-holonomic constraints (NHC constraints). Using pre-integral model can reduce the IMU update frequency and improve system real-time performance. At the same time, the model considers compensation for Earth rotation and Coriolis force, which is particularly important for high-precision inertial navigation.

[0087] 4) Construct measurement models for each sensor, and use the measurement models of each sensor, corresponding constraint relationships, weighting factors W, and IMU-wheel speed pre-integration models considering Earth's rotation and Coriolis force to establish sensor residual models.

[0088] 5) Construct the objective function to be optimized based on the sensor residual model. After solving the objective function, obtain the final navigation information of the carrier, thereby realizing the fusion positioning of the carrier.

[0089] 5) Specifically:

[0090] First, the objective function to be optimized is constructed based on the sensor residual model, as shown in the following formula:

[0091]

[0092] Where |||| is the squared Mahalanobis distance. The covariance is The squared Mahalanobis distance, The covariance is The squared Mahalanobis distance, The covariance is The squared Mahalanobis distance, where n is the number of pre-integration factors, m is the number of GNSS factors, and l is the number of lidar factors;

[0093] Then, the Levenberg-Marquardt nonlinear optimization method (LM nonlinear optimization method) is used to solve the objective function to obtain the vehicle's navigation information. Next, the sensor data set of the next time step is continuously acquired, and steps 2)-4) are repeated until the end to obtain the final navigation information of the vehicle, thus realizing the vehicle's fusion positioning.

[0094] The above description is only a preferred embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any changes or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in the present invention should be included within the scope of protection of the present invention.

Claims

1. A neural network-assisted robust multi-sensor factor map fusion localization method, characterized in that, Includes the following steps: 1) The position, three-axis angular velocity, three-axis acceleration, wheel speed and point cloud information are obtained by the multiple sensors loaded in the carrier, and are recorded as sensor dataset; 2) After feature extraction and matching of point cloud information, key frame pose information is obtained. The target neural network is selected according to the environment of the carrier. The key frame pose information is input into the target neural network and the GNSS fault probability is obtained and recorded as the weight factor W. 3) Based on the triaxial angular velocity, triaxial acceleration and wheel speed, after compensating for Earth rotation and Coriolis force on the pre-integral model, an IMU-wheel speed pre-integral model considering Earth rotation and Coriolis force is obtained. The formula for the IMU-wheel speed pre-integral model that considers Earth's rotation and Coriolis force is as follows: in, , Let the rotation quaternions be at time k-1 and time k, respectively. Indicates the Kronecker product. The Earth's rotation term at time k-1. Let k be the time. The velocity of the carrier at time k represents the velocity in the world coordinate system w. Let the velocity at time k-1 be the velocity in the world coordinate system w. Due to local gravity, The time increment from k-1 to k. Let w be the position of the carrier in the world coordinate system at time k. Let w be the position of the carrier in the world coordinate system at time k-1. , These are the Gorbachev terms for velocity and position, respectively. Let be the rotational attitude matrix from the carrier coordinate system b to the world coordinate system w at time k-1. , , For attitude pre-integration, velocity pre-integration, and position pre-integration; 4) Construct measurement models for each sensor, and use the measurement models of each sensor, corresponding constraint relationships, weighting factors W, and IMU-wheel speed pre-integration models considering Earth's rotation and Coriolis force to establish sensor residual models. 5) Construct the objective function to be optimized based on the sensor residual model. After solving the objective function, obtain the navigation information of the carrier, thereby realizing the fusion positioning of the carrier.

2. The neural network-assisted robust multi-sensor factor map fusion localization method according to claim 1, characterized in that, In step 1), the position is obtained through the Global Navigation Satellite System, the three-axis angular velocity and three-axis acceleration are acquired by the Inertial Measurement Unit (IMU), the wheel speed is acquired by the wheel speed odometer, and the point cloud information is acquired by the sensing module.

3. The neural network-assisted robust multi-sensor factor map fusion localization method according to claim 2, characterized in that, The sensing module is one or more of lidar and millimeter-wave radar.

4. The neural network-assisted robust multi-sensor factor map fusion localization method according to claim 1, characterized in that, The sensor residual model includes prior residuals, pre-integrated residuals, GNSS residuals, and LiDAR residuals, specifically: a priori residuals satisfy ,in, For the prior factor residuals, For the prior factor measurement matrix, For navigation state variables; Pre-integral residual The formula is as follows: in, For the pre-integral factor residual, For pre-integrated measurements, For transpose, Pre-integration for position The first-order Taylor expansion result, attitude pre-integration The first-order Taylor expansion result, Pre-integration of velocity The first-order Taylor expansion result, 2 The equivalent rotation vector for converting a quaternion at a small angle; The gyroscope zero biases at times k and k-1 are respectively. The zero bias of the table addition at times k and k-1 are respectively. These are the wheel speed scaling factors at times k and k-1, respectively; Let be the rotation quaternion at time k-1. Indicates the Kronecker product. The Earth's rotation term at time k-1. The velocity of the carrier at time k-1 in the world coordinate system w is represented. Let w be the position of the carrier in the world coordinate system at time k. Let w be the position of the carrier in the world coordinate system at time k-1. and Let be the velocities at times k and k-1 in the world coordinate system w, respectively. Due to local gravity, The time increment from k-1 to k. , These are the Gorbachev terms for velocity and position, respectively. Let be the rotational attitude matrix from the carrier coordinate system b to the world coordinate system w at time k-1; GNSS residuals The formula is as follows: in, For antenna mast arm, The values ​​are measured latitude and longitude, and W is a weighting factor. This represents the rotation matrix from the world coordinate system w to the Earth coordinate system e. For GNSS residuals, These are GNSS measurements. For the predicted location; LIDAR residuals The formula is as follows: in, For lidar residuals, These are lidar measurement values. Represents planar feature points; This represents the three points that match the planar feature point and are closest to it, and these three points are not on the same plane; To take the absolute value.

5. The neural network-assisted robust multi-sensor factor map fusion localization method according to claim 1, characterized in that, In step 5), the objective function to be optimized is constructed based on the sensor residual model. The formula for the objective function to be optimized is as follows: in, For the prior factor residuals, For the prior factor measurement matrix, For navigation state variables, The squared Mahalanobis distance, For the pre-integral factor residual, For pre-integrated measurements, The covariance is The squared Mahalanobis distance, For GNSS residuals, These are GNSS measurements. The covariance is The squared Mahalanobis distance, For lidar residuals, These are lidar measurement values. The covariance is The squared Mahalanobis distance, The number of pre-integral factors. The number of GNSS positioning factors. This represents the number of lidar factors.

6. The neural network-assisted robust multi-sensor factor map fusion localization method according to claim 1, characterized in that, In step 5), the LM nonlinear optimization method is used to solve the objective function to be optimized to obtain the navigation information of the carrier; the sensor dataset at the next moment is continuously acquired, and the objective function to be optimized is updated and solved based on the sensor dataset. The latest navigation information of the carrier is used as the final navigation information of the carrier to achieve the fusion positioning of the carrier.

7. The neural network-assisted robust multi-sensor factor map fusion localization method according to claim 1, characterized in that, The navigation information of the carrier includes time, carrier position, carrier speed, carrier attitude, gyroscope zero bias and accelerator zero bias, and wheel speed proportional factor.

Images