CN117906624A

CN117906624A - Vehicle positioning system and method based on heterologous fusion

Info

Publication number: CN117906624A
Application number: CN202311699878.4A
Authority: CN
Inventors: 柯希俊
Original assignee: SAIC Volkswagen Automotive Co Ltd
Current assignee: SAIC Volkswagen Automotive Co Ltd
Priority date: 2023-12-12
Filing date: 2023-12-12
Publication date: 2024-04-19

Abstract

The invention discloses a vehicle positioning system based on heterologous fusion, which comprises: a data acquisition module; the combined positioning module based on factor graph optimization builds a global factor graph and optimizes the global factor graph until a first convergence threshold is reached; the visual inertial positioning module is optimized based on the sliding window factor graph and performs global positioning initialization based on the factor graph optimization; detecting and tracking visual characteristic points, performing characteristic triangularization operation, visual motion restoration structure operation and visual inertia state estimation initialization based on visual camera data; then, constructing a global factor graph in a sliding time window, and optimizing the global factor graph until a second convergence threshold is reached; when GNSS signals exist and RTK service exists, the combined positioning module outputs the current global positioning pose reaching the first convergence threshold value to be used as a high-precision positioning result; when no GNSS signal exists, the apparent inertial positioning module outputs the current global positioning pose reaching the second convergence threshold as a high-precision positioning result.

Description

Vehicle positioning system and method based on heterologous fusion

Technical Field

The present disclosure relates to vehicle positioning systems and methods, and more particularly, to a vehicle positioning system and method for intelligent driving.

Background

The low-order automatic driving positioning technology used in the field of automatic driving of vehicles is mostly based on a combined positioning system (GNSS/IMU) and a high-definition Map (HD-Map), and is assisted with LiDAR or Camera perception matching positioning odometer technology.

The technical route has two problems that firstly, the technical route is too dependent on a high-precision map which is high in cost and difficult to update in time; secondly, the real-time matching result of semantic features sensed by sensors such as a laser radar (LiDAR) or a vision Camera (Camera) and semantic features from a priori map is relied on, so that the positioning requirements of the GNSS under the scenes such as lack of GNSS or lack of real-time kinematic differential (RTK) service are assisted.

Therefore, the technical solutions employing high-precision maps are increasingly being weakened by the autopilot industry. Recently, however, some solutions employ lightweight high-precision maps to assist in achieving high-precision positioning in extreme scenes.

However, the lightweight high-precision map which discards some non-key characteristic element information can reduce certain cost under a lower technical threshold, but the requirement of timely updating the map and further guaranteeing the freshness of the map is still difficult to meet.

Disclosure of Invention

The invention aims to provide a vehicle positioning system based on heterologous fusion, which has good scene self-adaption capability and robustness and can meet the requirement of high-precision positioning output of robustness when an automatic driving vehicle runs in a part of severe scenes.

In order to achieve the above object, the present invention proposes a vehicle positioning system based on heterologous fusion, comprising:

The data acquisition module acquires GNSS data, IMU data of the vehicle and vision camera data of the vehicle;

The combined positioning module is optimized based on the factor graph, and performs global positioning initialization based on the factor graph optimization based on the preprocessed GNSS data and the preprocessed IMU data; constructing a global factor graph based on the acquired GNSS factors, the IMU pre-integration factors and the prior pose factors, and optimizing the global factor graph until a first convergence threshold is reached;

The visual inertial positioning module is optimized based on the sliding window factor graph, and performs global positioning initialization based on the factor graph optimization based on the preprocessed GNSS data and the preprocessed IMU data; based on the vision camera data, detecting and tracking vision characteristic points, performing characteristic triangularization operation, performing vision motion restoration structure operation and performing vision inertial state estimation initialization; then, a global factor graph in a sliding time window is constructed based on a visual inertia state pre-estimated initialization result, the obtained visual re-projection factor, the IMU pre-integration factor and the prior pose factor, and the global factor graph in the sliding time window is optimized until a second convergence threshold is reached;

When the GNSS state bit is judged to have GNSS signals and RTK service is provided, the combined positioning module outputs the current global positioning pose reaching the first convergence threshold as a high-precision positioning result;

and when the GNSS state position is judged to be no signal, the apparent inertial positioning module outputs the current global positioning pose reaching the second convergence threshold as a high-precision positioning result.

The vehicle positioning system provided by the invention is a heterogeneous fusion positioning system based on a GNSS (global satellite positioning system), a vision camera and an inertial sensor. The positioning system at least comprises a combined positioning module (GNSS/INS module) based on factor graph optimization and a visual inertial positioning sub-module (VIO module) based on sliding window factor graph optimization. When stable GNSS signals exist and real-time kinematic (RTK) enhancement services exist, the combined positioning module can directly output six-degree-of-freedom high-precision positioning gestures in centimeter level.

In addition, the visual inertial positioning module based on sliding window factor graph optimization can independently and stably run and output six-degree-of-freedom positioning pose with higher precision without depending on the input of GNSS data after the global initialization of the visual inertial state predictor is completed with the GNSS initial positioning state quantity assistance.

Further, the vehicle positioning system further comprises a global pose diagram-based robust positioning optimization module, wherein the global pose diagram-based robust positioning optimization module:

collecting the global positioning pose output by the combined positioning module;

Judging a GNSS state position corresponding to the global positioning pose output by the combined positioning module, and when the GNSS state position is judged to have GNSS signals but not have RTK service, starting to acquire the global positioning pose output by the visual inertial positioning module by the global pose-based optimizing robust positioning module;

When the GNSS state position is continuously judged to have GNSS signals but not have RTK service, constructing a relative pose residual factor based on global positioning pose state quantities of front and rear time frames output by the combined positioning module and the visual inertial positioning module, and constructing a global pose graph based on the relative pose residual factor;

and optimizing the global pose graph until a third convergence threshold is reached, and outputting the output current global positioning pose as a high-precision positioning result.

In a preferred embodiment of the present invention, in order to further enhance the robustness of the vehicle positioning system and further improve the positioning accuracy, the vehicle positioning system further includes a robust positioning module optimized based on a global pose map. In this way, when there is a GNSS signal but no real-time motion differential service, the global positioning data from the combined positioning module and the vehicle end positioning data from the apparent inertial positioning module are coupled together to construct a pose map based on the global pose map optimization robust positioning module (GVIO module), and the high-precision positioning pose can be output by performing the large-scene pose map optimization.

Further, in the vehicle positioning system of the present invention, the combined positioning module performs optimization of the global factor graph using a nonlinear optimization solver; and/or the apparent inertial positioning module adopts a nonlinear optimization solver to execute optimization of the global factor graph in the sliding time window.

Further, in the vehicle positioning system of the present invention, the view inertial positioning module adopts a thinning strategy when constructing a global factor graph within a sliding time window.

Further, in the vehicle positioning system of the present invention, the global pose map-based optimization robust positioning module performs optimization of the global pose map using a nonlinear optimization solver.

The invention further aims to provide a vehicle positioning method based on heterologous fusion, which has good scene self-adaption capability and robustness and can meet the requirement of high-precision positioning output of robustness when an automatic driving vehicle runs in a part of severe scenes.

In order to achieve the above object, the present invention provides a vehicle positioning method based on heterologous fusion, comprising the steps of:

acquiring GNSS data, IMU data of a vehicle and vision camera data of the vehicle;

the combined positioning module based on factor graph optimization performs global positioning initialization based on factor graph optimization based on the preprocessed GNSS data and the preprocessed IMU data; the combined positioning module also constructs a global factor graph based on the acquired GNSS factors, the IMU pre-integration factors and the priori pose factors, and optimizes the global factor graph until a first convergence threshold is reached; when the GNSS state position is judged to have GNSS signals and RTK service is provided, the current global positioning pose output by the combined positioning module is output as a high-precision positioning result;

The visual inertial positioning module based on sliding window factor graph optimization performs global positioning initialization based on factor graph optimization based on the preprocessed GNSS data and IMU data; the visual inertial positioning module is also used for detecting and tracking visual characteristic points, performing characteristic triangularization operation, performing visual motion restoration structure operation and performing visual inertial state estimation initialization based on the visual camera data; then, based on visual inertia state pre-estimation initialization and the obtained visual re-projection factors, IMU pre-integration factors and priori pose factors, constructing a global factor graph in a sliding time window, and optimizing the global factor graph in the sliding time window until a second convergence threshold is reached; when the GNSS state position is judged to be no signal, the current global positioning pose output by the apparent inertial positioning module is output as a high-precision positioning result.

Further, the vehicle positioning method of the present invention further includes the steps of:

The global positioning pose output by the combined positioning module is collected by the robust positioning module based on global pose graph optimization;

When the GNSS state position is continuously judged to have GNSS signals but not have RTK service, the global pose image-based optimization robust positioning module constructs a relative pose residual factor based on global positioning pose state quantities of front and rear time frames output by the combined positioning module and the visual inertial positioning module, and constructs a global pose image based on the relative pose residual factor;

And optimizing the global pose graph until a third convergence threshold is reached, and outputting the current global positioning pose output by the global pose graph-based robust positioning module as a high-precision positioning result.

Further, in the vehicle positioning method of the present invention, a nonlinear optimization solver is adopted to perform optimization of the global factor graph; and/or performing optimization of the global factor graph within the sliding time window using a nonlinear optimization solver.

Further, in the vehicle positioning method of the present invention, a thinning strategy is adopted when constructing a global factor graph within a sliding time window.

Further, in the vehicle positioning method of the present invention, a nonlinear optimization solver is used to perform the optimization of the global pose map.

The vehicle positioning system and the method can enhance the robustness of the traditional global positioning system (GNSS/IMU), and have good positioning adaptability in severe scenes of deployment and operation of the automatic driving vehicle, such as partial urban canyon sections, partial overhead bridge sections, tunnels with small distance, boulevards and the like.

On the premise of stable GNSS signal input, the combined positioning module based on factor graph optimization can completely and independently operate. When the GNSS state bit has real-time motion differential enhancement service, the combined positioning module can directly output a centimeter-level six-degree-of-freedom high-precision positioning result.

And after the global initialization of the visual inertial state predictor assisted by the GNSS initial positioning state quantity is completed, the visual inertial positioning module based on sliding window factor graph optimization can independently and stably run without depending on the input of GNSS data and output six-degree-of-freedom positioning pose with higher precision. That is, in a harsh scenario without GNSS signals, the module has robustness of global positioning and scene adaptation capability.

In some further preferred embodiments, when there is a GNSS signal but no RTK service, the global pose map-based robust positioning module may couple global positioning data with vehicle-end local positioning data by using a map-optimized data fusion technique, so as to implement high-precision positioning output of a large scene.

Drawings

FIG. 1 shows a system architecture diagram of a vehicle positioning system according to one embodiment of the present invention.

FIG. 2 shows an architecture diagram of a factor graph-based optimized combined positioning module of a vehicle positioning system according to an embodiment of the present invention.

FIG. 3 shows a schematic diagram of a sliding window factor-based optimized view inertial positioning module of a vehicle positioning system according to an embodiment of the present invention.

FIG. 4 shows a schematic diagram of a global pose-based optimized robust positioning module for a vehicle positioning system according to an embodiment of the present invention.

Detailed Description

The vehicle positioning system and method based on heterologous fusion according to the present invention will be further explained and illustrated with reference to the drawings and the specific embodiments, however, the explanation and illustration do not unduly limit the technical solution of the present invention.

Aiming at complex scenes such as partial urban canyon road sections, partial overhead bridge road sections, tunnels with small distance, boulevard roads and the like in automatic driving deployment operation, the situations that GNSS signals are not provided with RTK service, are not provided with GNSS signals or are not provided with GNSS at times inevitably exist, and based on the positioning system, the embodiment of the invention provides a positioning system based on heterogeneous fusion of GNSS, a visual camera and an inertial sensor.

As shown in fig. 1, the vehicle positioning system includes: a combined positioning module (GNSS/INS) based on factor graph optimization, a visual inertial positioning module (VIO) based on sliding window factor graph optimization, and a robust positioning module (GVIO) based on global pose graph optimization.

The combined positioning module based on factor graph optimization inputs two types of data. The first type of data is GNSS global position quantity, position standard deviation and state bit data which are transformed into a local navigation system. The second type of data is angular velocity and linear acceleration measured by an Inertial Measurement Unit (IMU), and deviation values and noise values of the angular velocity and the linear acceleration calibrated in advance. The data are obtained by preprocessing GNSS data acquired by the data acquisition module and IMU data of the vehicle and extracting the data.

As shown in fig. 2, in some embodiments, the factor graph optimization-based combined localization module may include a factor graph optimization-based global localization initialization sub-module, a precision inertial navigation system mechanical orchestration sub-module, and a factor graph optimization sub-module (FGO).

In the beginning stage, with the assistance of the GNSS global position, the global positioning initialization sub-module based on factor graph optimization can complete the state initialization of IMU motion integration and pre-integration, and output IMU estimated initial state quantity, including position, attitude angle, linear velocity, angular velocity deviation and linear acceleration deviation, for example. And aiming at a global factor graph optimization sub-module (FGO) constructed based on the GNSS factors, the IMU pre-integration factors and the priori pose factors, global factor graph optimization is executed to reach a first convergence threshold, and when the GNSS state displays signals and has RTK service, the combined positioning module can output the high-precision global six-degree-of-freedom positioning pose.

In some more preferred embodiments, the combined positioning module may use IMU pre-integration technology to pre-integrate the IMU measurement input, thereby reducing repeated propagation multiple integration, and simultaneously greatly reducing the numerical calculation amount, and further improving the real-time performance of positioning.

Therefore, the combined positioning module based on factor graph optimization can completely and independently operate on the premise of stable GNSS signal input. When the GNSS state bit has real-time kinematic (RTK) enhancement service, the module can directly output a six-degree-of-freedom high-precision positioning result in a centimeter level.

As shown in fig. 2, the combined positioning module optimized based on the factor graph inputs two types of data. The first type of data is GNSS global position quantity, position standard deviation and state bit data which are transformed into a local navigation system. The second type of data is angular velocity and linear acceleration measured by an Inertial Measurement Unit (IMU), and deviation values and noise values of the angular velocity and the linear acceleration calibrated in advance.

With continued reference to fig. 2, the two major core modules of the combined localization module based on factor graph optimization are a global localization initialization sub-module based on factor graph optimization and a factor graph optimization sub-module (FGO).

With the assistance of the GNSS global position, the global positioning initialization based on factor graph optimization in the initial stage is to complete the state initialization of IMU motion integration and pre-integration, and output IMU estimated initial state quantity (including position, attitude angle, linear velocity, angular velocity deviation and linear acceleration deviation). The input GNSS data has been transformed into a local navigation system. The input IMU data (including angular velocity and linear acceleration) is processed by the motion integration and pre-integration algorithm to form main IMU estimated state quantities (including position, attitude angle, linear velocity, angular velocity deviation and linear acceleration deviation).

In some embodiments, the combined positioning module pre-integrates the IMU measurement input by using an IMU pre-integration technique, thereby reducing repeated propagation multiple integration and greatly reducing the numerical calculation. In addition, because the sampling frame rate of the IMU sensor and the GNSS data receiving processing frequency are inconsistent, the two types of input data need to be preprocessed. Firstly, the two types of data are required to be aligned in a time soft synchronization mode, and secondly, a binary search algorithm is required to be adopted to screen proper time stamps to determine whether the IMU data are required to be interpolated.

Next, a GNSS residual factor is constructed for the GNSS estimated state quantity and measurement value based on the filtered front and rear frame timestamps. In addition, pre-integration residual factors are also constructed for IMU pre-integration state quantities within the previous and subsequent time frames. The pre-integral residual factor may be constructed in two parts, the first part being a position quantity and a four element attitude quantity, and the second part being a velocity quantity, an angular velocity deviation quantity, and a linear acceleration deviation quantity. The factor graph constructed based on these residual factors is a maximum likelihood estimation problem that can be converted into a nonlinear least squares optimization problem.

In some embodiments, iterative optimization may be performed using a nonlinear optimization solver (Ceres) until a convergence threshold is reached, i.e., global positioning initialization based on factor graph optimization with GNSS assistance is completed.

After the GNSS assisted global positioning initialization is completed, based on the filled global initialization state quantity, an accurate Inertial Navigation System (INS) mechanical orchestration may be started, which completes the construction of IMU pre-integration residual factors, the construction of a priori pose factors, and the updating of the global state quantity.

With the advancement of the space-time history, a new thread is opened up next, and a slightly large-scale factor graph is built again based on all residual factors (including GNSS factors, IMU pre-integral factors and priori pose factors) built as described above. Likewise, for the factor graph, a nonlinear optimization solver (Ceres) may be used to perform global factor graph optimization until a first convergence threshold is reached, i.e., a pose with global six degrees of freedom may be output.

Inputs to the sliding window factor graph based visual inertial positioning module are raw data from an inertial test unit (IMU) and a monocular vision camera (camera), and the GNSS provides initial position and position standard deviation inputs only at the beginning of the IMU global initialization optimization.

As shown in fig. 3, the sliding window factor graph-based optimized view inertial localization module includes a visual feature detection sub-module, a visual feature tracking sub-module, a pure visual feature triangulating and motion restoration structure sub-module (SfM BA), a view inertial state pre-estimation initialization sub-module, and a sliding window-based Factor Graph Optimization (FGO) fusion algorithm sub-module, a global localization initialization sub-module, and a precision inertial navigation system mechanical orchestration sub-module.

In some more preferred embodiments, global factor graph optimization within the sliding time window may employ a sparsification strategy to improve the computational instantaneity and robustness of the apparent inertial positioning module. Similarly, the vision inertial positioning module can also pre-integrate the IMU measurement input by adopting an IMU pre-integration technology, so that repeated propagation multiple integration is reduced, meanwhile, the numerical calculation amount is greatly reduced, and the real-time performance of positioning is further improved.

And after the global initialization of the visual inertial state predictor is completed with the assistance of the GNSS initial positioning state quantity based on the sliding window factor graph, the visual inertial positioning module can independently and stably run without depending on the input of GNSS data and output six-degree-of-freedom positioning pose with higher precision. That is, in a harsh scenario without GNSS signals, the subsystem still has globally located scene adaptation capability.

Specifically, depending on the raw data from the inertial test unit (IMU) and monocular vision camera (camera) inputs to the inertial positioning module, the GNSS provides initial position and position standard deviation inputs only at the beginning of the IMU global initialization optimization. The subsystem can independently and stably run without depending on the input of GNSS data after the global initialization of the GNSS initial positioning state quantity auxiliary inertial state predictor is completed in the beginning (for example, about 10 seconds). The subsystem can output six-degree-of-freedom positioning pose with higher precision under severe scenes without GNSS signals (such as scenes of partial urban canyon sections, partial overhead bridge sections, tunnels with small distance, boulevees and the like).

According to the space-time advancing process, the visual inertial positioning module firstly adopts the same global positioning initialization sub-module as the combined positioning sub-system based on factor graph optimization, realizes global positioning initialization based on factor graph optimization under the assistance of GNSS, and completes global initialization of IMU state quantity. Meanwhile, for image data acquired by a monocular vision camera, detection and tracking of vision characteristic points are performed in a vision processing front end. For each new image, in some more specific embodiments, the existing feature points are tracked using a KLT sparse optical flow algorithm, while new corner features are detected, to ensure that a minimum number (e.g., 100-300) of features are kept tracked in each image frame. The detector forces a uniform feature distribution by setting a minimum pixel spacing between two adjacent features. The two-dimensional features are first de-distorted, then projected onto a unit sphere after outlier rejection, and outlier rejection is performed using a random sample consensus (RANSAC) algorithm with a base matrix model.

In addition, the apparent inertial positioning module can also select key frames in the front end of visual processing. Key frame selection may employ two criteria: the first criterion is the average disparity (average parallax) from the previous key frame. If the average disparity of the tracked feature is between the current frame and the latest key frame and exceeds a certain threshold, then the visual frame is considered a new key frame. Both translational and rotational motion cause parallax. However, the feature cannot be triangulated with only rotational motion. To avoid this, the rotational attitude can be compensated for using the attitude angle obtained by short-term integration of IMU gyroscope measurements in computing the parallax. The rotation compensation is only used for key frame selection, and does not participate in rotation attitude angle calculation in the apparent inertial fusion predictor. For this reason, even if the gyroscope contains large noise or has deviation, only poor key frame selection results are caused, and estimation quality is not directly affected. Another criterion is tracking quality. If the number of features tracked is below a certain threshold, then the visual frame is considered a new key frame, this criterion being to avoid losing feature tracking entirely.

Then, feature triangularization and visual motion restoration structure (SfM) operations are performed while constructing a local Beam Adjustment (BA) problem that estimates the pose and feature position of the camera's visual frame at the maximum scale (smax). A sliding window of a certain size may be set to only maintain a plurality of visual frames in order to reduce the amount of computation. First, the feature association between the latest frame and all previous frames is checked. If stable feature tracking (e.g., over 30 tracked features) can be found and there is sufficient disparity (e.g., over 20 pixels) between the latest frame and any other frames in the sliding window, a five-point algorithm is used to recover the relative rotation and maximum scale translation between the two frames. The scale is then arbitrarily set and all features observed in the two frames are triangulated. Based on the features that have been triangulated, a perspective n-point (PnP) method can be employed to estimate the pose of all other frames in the sliding window. Next, a local Beam Adjustment (BA) problem is constructed based on minimizing the sum of the re-projection residuals of all features observed at all key frame camera poses within the sliding window. Finally, a nonlinear least squares optimization algorithm can be used to solve the local Beam Adjustment (BA) problem constructed herein, thereby optimizing and adjusting the pose of the relative key frame and the position quantities of all features relative to the first frame of the sliding window, and these quantities constitute the initialized state quantities of the pure visual motion restoration structure (SfM).

And after global positioning initialization based on factor graph optimization and pure visual motion restoration structure initialization prediction under GNSS assistance are respectively completed, visual inertia state prediction initialization is carried out. Firstly, based on the IMU estimated global initial pose and external parameters calibrated in advance between the camera and the IMU, the visual key frame pose and the characteristic position state quantity are transformed to a world coordinate system, so that the space alignment of the visual inertial pose is realized. Secondly, setting a small-scale sliding window in two continuous time frames, and performing cross-multiplication linearization operation on the four-element attitude angle increment obtained by IMU angular velocity pre-integration and the accumulated relative rotation angle component obtained by the pure visual motion recovery structure to construct a nonlinear least square optimization problem. After solving the problem, the angular velocity deviation value of the IMU gyroscope can be corrected again. The IMU pre-integral factor is then re-propagated using the corrected angular velocity deviation value. And thirdly, after the correction and initialization of the angular velocity deviation value of the IMU gyroscope, initializing and assigning states of the velocity, the gravity acceleration vector and the visual monocular depth scale. Similarly, in two continuous time frames, a small-scale sliding window is set, and difference linearization operation is carried out on the position increment and the speed increment obtained by IMU position and speed pre-integration and the accumulated relative translation component obtained by the pure vision motion recovery structure, so that the nonlinear least square optimization problem is constructed. After solving the problem, the carrier speed, the gravity acceleration vector and the visual monocular depth scale under the world coordinate system can be obtained. Thus, the initialization of the visual inertial state estimation global state quantity is completed.

After the global positioning initialization and the apparent inertial state estimation initialization assisted by the GNSS are completed, based on the filled global initialization state quantity and the corrected angular velocity deviation value, the accurate Inertial Navigation System (INS) mechanical arrangement can be started, and the arrangement completes the construction of the IMU pre-integration residual error factor, the construction of the priori pose factor and the update of the global state quantity.

With the advancement of the space-time course, a new thread is opened up next, a larger-scale sliding time window is set, and a larger-scale factor graph is built again based on initial state quantity (including position quantity, attitude quaternion, speed, linear acceleration deviation, angular speed deviation, monocular vision scale and gravity vector) obtained by pre-estimating initialization of the vision inertia state and all residual factors (including vision re-projection factors, IMU pre-integration factors and priori pose factors) constructed above.

To limit the computational complexity of the factor graph-based optimized view inertial positioning module, a sparsification (or marginalization) strategy may be employed within the sliding time window. Specifically, in some embodiments, first, the IMU state quantities and the feature position quantities at a certain moment may be selectively marginalized from the sliding window, while the measured values corresponding to these marginalized state quantities are converted into a priori pose factor. When the next to last latest frame in the sliding window is a key frame, the last to last latest frame in the sliding window is reserved in the window, the oldest frame and the corresponding vision and inertia measured values are marginalized, and the marginalized measured values are used for constructing the prior pose factors. Otherwise, if the next to last latest frame within the sliding window is a non-key frame, that frame and all its corresponding visual measurements will simply be deleted, but the IMU pre-integration inertial measurements associated with that non-key frame will remain, and the IMU pre-integration process will proceed to the next frame. In order to maintain the sparsity and robustness of the sliding window inside view inertial positioning module, the measurement values of all non-key frames are not marginalized.

The sparsification strategy aims to keep spatially separated keyframes in a sliding window, which ensures that adequate parallax is provided for feature triangulation and maximizes the probability of maintaining acceleration measurements with greater excitation.

In some more specific embodiments, the sparsification strategy may operate with a schulr substitution elimination (Schur Complement). First, all the marginalized measurements corresponding to the deleted state quantity will be used to construct a new a priori factor. Further, the a priori factors will be added to existing a priori factors.

Factor graphs within a dynamic sliding window maintained based on a sparsification strategy will have robust and efficient properties. Likewise, for the dynamic sliding window factor graph, a nonlinear optimization solver (Ceres) may be used to perform global factor graph optimization until a second convergence threshold is reached, i.e., a pose with global six degrees of freedom may be output.

In a preferred embodiment of the invention, the vehicle localization system further comprises an optimized robust localization module (GVIO) based on the global pose map. In order to enhance the robustness of the positioning system and further improve the positioning accuracy, the global pose diagram-based robust positioning module is designed in a data post-fusion stage. The method is always subscribed to global positioning pose output by a combined positioning module (GNSS/INS) based on factor graph optimization, and the GNSS state position corresponding to the global positioning pose state quantity is judged. When the GNSS state bit displays GNSS signals but no RTK service, the global pose-based optimizing robust positioning module is triggered to subscribe to the visual inertial odometer pose output by the visual inertial positioning module optimized based on the sliding window factor graph. When the GNSS state position is continuously displayed with GNSS signals but without RTK service, the global pose-based optimizing robust positioning module constructs relative pose residual factors based on global positioning pose state quantities of front and rear time frames input by the front two modules respectively, then a large-scene global pose graph can be constructed based on the two groups of relative pose residual factors, and the pose graph optimization is executed, so that the positioning pose with high precision can be output.

The invention also provides a vehicle positioning method based on heterologous fusion, which comprises preprocessing (or called preprocessing) of sensor data, pre-fusion of data of a combined positioning module and an apparent inertial positioning module, post-fusion of data and post-processing from the time advancing process of data flow.

Specifically, the vehicle positioning method based on heterologous fusion comprises the following steps:

100: GNSS data, IMU data of the vehicle, and vision camera data (e.g., monocular vision camera data) of the vehicle are acquired.

200: Preprocessing of sensor data:

Before the preprocessing of the sensor data starts, a series of coordinate systems, such as a local navigation coordinate system, a world coordinate system (w), a carrier system (b), a camera coordinate system, is defined first. The local navigation coordinate system here employs the northeast day (ENU) coordinate system. The world coordinate system is defined at the initial position of the navigation coordinate system, and the direction is consistent with the navigation system. The IMU body coordinate system is the same coordinate system as the carrier system, and is based on the vehicle-defined direction as the front right upper (RFU) coordinate system. The visual camera coordinate system is defined as the lower right front (RDF) coordinate system relative to the vehicle axis. The positioning state quantity (position, attitude quaternion and velocity) is an expression of the IMU body coordinate system relative to the world coordinate system.

In some embodiments, an altitude filter may be used to filter the altitude data for the raw GNSS data and to detect and verify the status for the GNSS. In addition, the lever arm position parameters from the GNSS receiving antenna to the IMU body coordinate system can be measured and calibrated in advance, so that the geographic data transformation library (GeographicLib) can be conveniently adopted to transform the original GNSS data (longitude and latitude height and position standard deviation) under the WGS84 coordinate system to the local navigation coordinate system (northeast day (ENU)) so as to obtain the global initial position of the carrier.

In addition, for the data input to the visual inertial positioning module, the external parameters of the monocular vision camera relative to the IMU body coordinate system, the internal parameters of the camera and the parameters of the IMU noise model can be calibrated in advance. For example, the original data of the two types of sensors can be acquired in advance, and the classical kalibr open source algorithm is adopted to perform offline calibration on the internal parameters of the camera, the external parameters of the camera relative to the IMU, and the deviation term and the noise term of the angular velocity and the linear acceleration of the IMU.

300: Front fusion of data of the combined positioning module and the vision inertial positioning module:

As shown in fig. 2, the combined localization module optimized based on the factor graph can completely independently run the input of two types of sensing data. The first type of data is GNSS global position quantity, position standard deviation and state bit data which are transformed into a local navigation system. The second type of data is angular velocity and linear acceleration measured by an Inertial Measurement Unit (IMU), and deviation values and noise values of the angular velocity and the linear acceleration calibrated in advance. The two core modules of the subsystem are a global positioning initialization sub-module based on factor graph optimization and a factor graph optimization sub-module.

Under the assistance of a GNSS global position, a global positioning initialization submodule based on factor graph optimization in the beginning stage completes the state initialization of IMU motion integration and pre-integration, and outputs IMU estimated initial state quantity (position, attitude angle, linear velocity, angular velocity deviation and linear acceleration deviation). The input GNSS data has been transformed into a local navigation system. The input IMU data (including angular velocity and linear acceleration) is processed by the motion integration and pre-integration algorithm to form main IMU estimated state quantities (including position, attitude angle, linear velocity, angular velocity deviation and linear acceleration deviation). Because the sampling frame rate of the IMU sensor and the GNSS data receiving processing frequency are inconsistent, the two types of input data need to be preprocessed. Firstly, the two types of data are required to be aligned in a time soft synchronization mode, and secondly, a binary search algorithm is required to be adopted to screen proper time stamps to determine whether the IMU data are required to be interpolated.

In some embodiments, iterative optimization may be performed using an advanced nonlinear optimization solver (Ceres) until a convergence threshold is reached, i.e., global positioning initialization based on factor graph optimization with GNSS assistance is completed.

After the global positioning initialization assisted by the GNSS is completed, based on the filled global initialization state quantity, the accurate Inertial Navigation System (INS) mechanical arrangement can be performed, and the arrangement completes the construction of the IMU pre-integral residual factor, the construction of the priori pose factor and the update of the global state quantity.

In addition, after the data post-fusion thread is started, the GNSS state position corresponding to the position quantity of the pose with the global six degrees of freedom output by the combined positioning module can be judged. If the GNSS state display is effective and has RTK service, the data post-fusion thread directly judges that the global pose state quantity acquired at the moment is a high-precision optimized value and directly outputs the current high-precision positioning result.

As shown in fig. 3, the inputs to the view inertial positioning module optimized based on the sliding window factor graph come from raw data of an inertial test unit (IMU) and a monocular vision camera (camera), and the GNSS provides initial position and position standard deviation inputs only at the IMU global initialization optimization start stage.

Specifically, according to the space-time advancing process, the apparent inertial positioning module firstly adopts the same global positioning initialization sub-module as the combined positioning module based on factor graph optimization, and realizes the global positioning initialization based on factor graph optimization under the assistance of GNSS, thereby completing the global initialization of the IMU state quantity. Meanwhile, for image data acquired by a monocular vision camera, detection and tracking of vision characteristic points are performed in a vision processing front end.

In addition, the selection of key frames may also be performed in the vision processing front-end. Key frame selection may employ two criteria: the first criterion is the average disparity (average parallax) from the previous key frame. If the average disparity of the tracked feature is between the current frame and the latest key frame and exceeds a certain threshold, then the visual frame is considered a new key frame. Another criterion is tracking quality. If the number of features tracked is below a certain threshold, then the visual frame is considered a new key frame, this criterion being to avoid losing feature tracking entirely. Since both translational and rotational motion can cause visual parallax, short-term integrated attitude angles of IMU gyroscope measurements can be introduced to compensate for rotational components for subsequent triangulating of visual feature point landmarks.

Then, feature triangularization and visual motion restoration structure (SfM) operations are performed while constructing a local Beam Adjustment (BA) problem that estimates the pose and feature position of the camera's visual frame at the maximum scale (smax). And solving the constructed local Beam Adjustment (BA) problem by adopting a nonlinear least square optimization algorithm, thereby obtaining the pose of a relative key frame relative to the first frame of the sliding window and the position quantity of all features after optimization adjustment, and forming the initialized state quantity of the pure visual motion recovery structure (SfM).

And after global positioning initialization based on factor graph optimization and pure visual motion restoration structure (SfM, BA) initialization prediction under GNSS assistance are respectively completed, visual inertial state prediction initialization is carried out. The method comprises the steps of realizing space alignment of the apparent inertial pose, correcting the angular velocity deviation value of the IMU gyroscope, updating the IMU pre-integral factor by using the correction value, obtaining the carrier velocity, the gravity acceleration vector and the visual monocular depth scale under the world coordinate system, and thus completing initialization of the apparent inertial state estimated global state quantity.

After the global positioning initialization and the apparent inertial state estimation initialization assisted by the GNSS are completed, based on the filled global initialization state quantity and the corrected angular velocity deviation value, an accurate Inertial Navigation System (INS) can be mechanically arranged, and the arrangement completes the construction of an IMU pre-integral residual error factor, the construction of an priori pose factor and the update of the global state quantity.

In some preferred embodiments, to limit the computational complexity of the factor graph-based optimized inertial navigation module (VIO), a sparsification (or marginalization) strategy may be employed within the sliding time window. The sparsification strategy aims to keep spatially separated keyframes in a sliding window, which ensures that adequate parallax is provided for feature triangulation and maximizes the probability of maintaining acceleration measurements with greater excitation.

And after the subsequent data post-fusion thread is started, judging the GNSS state position corresponding to the position quantity of the global six-degree-of-freedom pose output by the visual inertial positioning module. If the GNSS state is invalid (or called no signal), the data post-fusion thread directly judges that the global pose state quantity acquired at the moment is a high-precision optimized value and directly outputs the current high-precision positioning result.

In some preferred embodiments, the vehicle positioning method according to the present invention further includes step 400: and (5) fusing the data.

The main purpose of post-data fusion is three. First, the state of the raw GNSS data is continuously detected and checked. Second, it is determined whether to trigger execution of a global pose map optimization robust positioning module designed at this stage based on GNSS state bits (GVIO). Thirdly, judging which positioning fusion module to finally select and output the high-precision positioning pose generated by the positioning fusion module based on the GNSS state position to output as a final positioning result of the whole positioning system. Thus, the basic logic steps of this post-data fusion stage are three steps. First, when stable GNSS signals exist and real-time kinematic (RTK) enhancement service exists, the combined positioning module based on factor graph optimization directly outputs six-degree-of-freedom high-precision positioning pose in centimeter level. And secondly, when the GNSS signals are not stable or sometimes are not stable at the beginning, the visual inertial positioning module based on sliding window factor graph optimization can independently and stably run and output six-degree-of-freedom positioning pose with higher precision without depending on the input of GNSS data after the global initialization of the visual inertial state predictor assisted by the initial positioning state quantity of the GNSS is completed. And thirdly, when GNSS signals exist and RTK service is not available, the global positioning data from the combined positioning module and the vehicle end positioning data from the visual inertial positioning module are coupled together to construct a pose graph based on the global pose graph optimization robust positioning module, and the high-precision positioning pose can be output by executing the large-scene pose graph optimization. By means of the data post-fusion design, the robustness and the self-adaption capability of the whole positioning system can be enhanced while high-precision positioning output is guaranteed.

Therefore, the vehicle positioning method and system provided by the invention do not depend on a priori high-precision map and high-cost sensor hardware, and have certain scene self-adaptation capability and robustness for severe automatic driving vehicle deployment operation scenes (such as scenes of partial urban canyon sections, partial overhead bridge sections, tunnels and boulevards with small distances, and the like). The technology solves the need of light-weight, low-cost, robust and high-precision positioning output when the automatic driving vehicle runs in a part of severe scenes to a certain extent.

It should be noted that the prior art in the protection scope of the present application is not limited to the embodiments given in the present application document, and all the prior art that does not contradict the scheme of the present application, including but not limited to the prior patent document, the prior publication, the prior disclosure, etc. can be included in the protection scope of the present application.

In addition, the combination of the features described in the present application is not limited to the combination described in the claims or the combination described in the embodiments, and all the features described in the present application may be freely combined or combined in any manner unless contradiction occurs between them.

It should also be noted that the above-recited embodiments are merely specific examples of the present invention. It is apparent that the present invention is not limited to the above embodiments, and similar changes or modifications will be apparent to those skilled in the art from the present disclosure, and it is intended to be within the scope of the present invention.

Claims

1. A vehicle locating system based on heterofusion, comprising:

When the GNSS state position is judged to have GNSS signals and has real-time motion differential service, the combined positioning module outputs the current global positioning pose reaching a first convergence threshold value as a high-precision positioning result;

2. The vehicle localization system of claim 1, further comprising a global-pose-map-based optimized robust localization module that:

judging a GNSS state position corresponding to the global positioning pose output by the combined positioning module, and when the GNSS state position is judged to have GNSS signals but not have real-time motion differential service, starting to acquire the global positioning pose output by the visual inertial positioning module by the global pose-based optimal robust positioning module;

When the GNSS state position is continuously judged to have GNSS signals but no real-time motion differential service, constructing a relative pose residual factor based on global positioning pose state quantities of front and rear time frames output by the combined positioning module and the visual inertial positioning module, and constructing a global pose graph based on the relative pose residual factor;

3. The vehicle localization system of claim 1, wherein the combined localization module performs optimization of the global factor graph using a nonlinear optimization solver; and/or the apparent inertial positioning module adopts a nonlinear optimization solver to execute optimization of the global factor graph in the sliding time window.

4. The vehicle positioning system of claim 1, wherein the apparent inertial positioning module employs a sparsification strategy in constructing a global factor graph within a sliding time window.

5. The vehicle localization system of claim 2, wherein the global pose map-based optimization robust localization module performs optimization of the global pose map using a nonlinear optimization solver.

6. The vehicle positioning method based on the heterologous fusion is characterized by comprising the following steps of:

The combined positioning module based on factor graph optimization performs global positioning initialization based on factor graph optimization based on the preprocessed GNSS data and the preprocessed IMU data; the combined positioning module also constructs a global factor graph based on the acquired GNSS factors, the IMU pre-integration factors and the priori pose factors, and optimizes the global factor graph until a first convergence threshold is reached; when the GNSS state position is judged to have GNSS signals and has real-time motion differential service, the current global positioning pose output by the combined positioning module is output as a high-precision positioning result;

7. The vehicle positioning method according to claim 6, characterized by further comprising the step of:

When the GNSS state position is continuously judged to have GNSS signals but no real-time motion differential service exists, the global pose image-based optimization robust positioning module constructs a relative pose residual factor based on global positioning pose state quantities of front and rear time frames output by the combined positioning module and the visual inertial positioning module, and constructs a global pose image based on the relative pose residual factor;

8. The vehicle localization method of claim 6, in which the optimization of the global factor graph is performed using a nonlinear optimization solver; and/or performing optimization of the global factor graph within the sliding time window using a nonlinear optimization solver.

9. The vehicle localization method of claim 6, in which a sparsification strategy is employed in constructing the global factor graph within the sliding time window.

10. The vehicle localization method of claim 7, in which the optimization of the global pose map is performed using a nonlinear optimization solver.