IMU-centered multi-sensor fusion automatic driving positioning method

Abstract

The invention provides an IMU (inertial measurement unit)-centered multi-sensor fusion automatic driving positioning method, which comprises the following steps of: acquiring a GNSS (global navigation satellite system) signal, a laser radar point cloud, image data and an IMU state of a vehicle through a receiver, a camera, a laser radar and an IMU of a global navigation satellite system (GNSS), and calculating the position, the speed and the attitude of the vehicle in real time; an IMU state, a camera pose and a laser radar frame are used as nodes, IMU pre-integration, a visual re-projection error, a laser radar matching error and a GNSS measurement value are used as factors, and a factor graph is constructed; taking the weighted residual sum of all minimized factors as a target function, taking the state of the IMU as a core variable, solving the target function by adopting a sliding window mechanism and a variable elimination algorithm, and updating nodes in the factor graph; the updated nodes are fed back to the IMU, the IMU state is updated, the steps are repeated, and automatic driving positioning of the vehicle is achieved.

Description

Technical Field

[0001] The present invention relates to the technical field of autonomous driving positioning and navigation, and particularly refers to a multi-sensor fusion autonomous driving positioning method centered on an IMU. Background Art

[0002] In the field of autonomous driving vehicles in complex environments, continuous, robust, and accurate positioning is crucial. In recent years, with the development of sensors such as cameras and lidar, multi-sensor fusion SLAM technology based on vision and lidar has become the main idea for solving the positioning of autonomous driving vehicles. Currently, due to the low cost of cameras, rich image information, and strong perception and recognition capabilities, vision-based SLAM solutions are widely used. However, the image quality output by vision sensors is overly dependent on lighting conditions, and images lack depth information. Although multi-sensor fusion SLAM technology dominated by lidar is not affected by lighting conditions, and lidar points contain accurate distance information of objects, these characteristics can reduce the computational burden on the processor and greatly improve the positioning accuracy. However, in scenarios with few environmental features or in structured scenarios, the laser-based SLAM algorithm will fail to position due to lack of sufficient geometric features, such as in long and narrow corridors or tunnel environments.

[0003] Therefore, in multi-sensor fusion algorithms, such SLAM systems based on perception sensors all have natural defects, which greatly limit their application scenarios. The lidar-based SLAM system can achieve high accuracy in structured scenarios, but usually fails in unstructured scenarios such as long corridors or flat fields. Vision-based SLAM systems perform well in environments with rich textures, but the performance of such SLAM systems is very sensitive to lighting changes, fast movements, and initialization. The Global Navigation Satellite System (GNSS) is prone to a serious decline in positioning accuracy when in areas where satellite signals are blocked or reflected. Summary of the Invention

[0004] The present invention proposes a multi-sensor fusion autonomous driving positioning method centered on an IMU, which solves the problem that the positioning accuracy of the Global Navigation Satellite System will seriously decline when in areas where satellite signals are blocked or reflected.

[0005] To solve the above technical problems, the present invention provides a multi-sensor fusion autonomous driving positioning method centered on an IMU, including the following steps:

[0006] Step S1: Obtain the GNSS measurements, lidar frames, camera poses, and IMU states of the vehicle through a receiver of the Global Navigation Satellite System (GNSS), a camera, a lidar, and an Inertial Measurement Unit (IMU). Calculate the position, speed, and attitude of the vehicle in real time according to the IMU states.

[0007] Step S2: Use the IMU states, camera poses, and lidar frames as nodes, and use IMU pre-integration, visual reprojection error, lidar matching error, and GNSS measurements as factors to construct a factor graph.

[0008] Step S3: Take minimizing the weighted residual sum of all factors as the objective function, use the IMU states as the core variables, and adopt a sliding window mechanism and a variable elimination algorithm to solve the objective function, and update the nodes in the factor graph.

[0009] Step S4: Feed the updated nodes back to the IMU to update the IMU states, and repeat Steps S1 to S3 to achieve autonomous driving positioning of the vehicle.

[0010] Preferably, in Step S1, the global position and speed information of the vehicle are received through the GNSS receiver to correct the accelerometer bias and gyroscope bias of the IMU.

[0011] Preferably, in Step S2, the observation constraint between the landmark node and the camera pose node is used as the visual reprojection error. The steps for obtaining the landmark node include the following:

[0012] Step S201: Divide the image captured by the camera into several grids of the same size, and separately extract the visual features in each grid.

[0013] Step S202: Use the optical flow algorithm to track the visual features of each grid, calculate the average parallax between the current frame and the previous frame. If the average parallax is greater than the set threshold, the current frame is used as a key frame.

[0014] Step S203: Use the prior attitude information provided by the IMU to perform triangulation on the matching visual features in the current key frame and the previous key frame, determine the depth of the landmark in the key frame, and use the landmark with the depth within the set landmark depth range as the landmark node.

[0015] Preferably, in Step S2, the matching error between the lidar frame and the local map is used as the lidar matching error. The steps for matching the lidar frame and the local map include the following:

[0016] Step S211: Extract the edge features and plane features in the lidar point cloud through curvature evaluation to form a lidar frame.

[0017] Step S212: When the vehicle pose changes by more than a threshold, set the corresponding radar frame as a key frame, and construct a local voxel map using the key frame;

[0018] Step S213: Based on the initial pose of the IMU, match the newly received radar frame with the local voxel map.

[0019] Preferably, the expression for IMU pre-integration in step S2 is:

[0020]

[0021] In the formula, is the IMU pre-integration; is the rotation matrix, representing the rotation from the b coordinate system to the w coordinate system at time i, where the b coordinate system is the IMU coordinate system and the w coordinate system is the world coordinate system; are the positions of the IMU coordinate system corresponding to time i and time j in the world coordinate system respectively; are the velocities of the IMU coordinate system corresponding to time i and time j in the world coordinate system respectively; are the rotation quaternions from the b coordinate system to the w coordinate system at time i and time j respectively; and are the Coriolis correction terms for position and velocity pre-integration respectively; and are the measurement values of position, velocity and attitude pre-integration respectively; the quaternion is the rotation caused by the earth's rotation; Δt ij represents the time interval between time i and time j; represents quaternion multiplication; [·] v represents the imaginary part of the quaternion; g w is the gravity vector in the w coordinate system; are the accelerometer biases at time i and time j respectively; are the gyroscope biases at time i and time j respectively.

[0022] Preferably, the expression for the visual reprojection error in step S2 is:

[0023]

[0024] In the formula, e v is the visual reprojection error; is the pose of the feature point in the camera coordinate system; is the rear camera projection function, which back-projects the feature point on the pixel plane of the key frame to the camera coordinate system using the camera internal parameters; b 1 and b 2 are Two orthogonal bases of the tangent plane; rotation matrix is the rotation from the IMU coordinate system b to the camera coordinate system c; rotation matrix is the rotation from the c coordinate system to the b coordinate system; and represent the rotation between the world coordinate system and the IMU coordinate system at time j; δ l is the depth of the landmark l; is the translation vector from the b coordinate system to the c coordinate system; and represent the positions of the IMU coordinate systems corresponding to the i-th and j-th frames in the world coordinate system.

[0025] Preferably, the expression of the objective function in step S3 is:

[0026]

[0027] where E is the objective function; E prior is the marginalized prior residual; B represents the set of all key frames within the sliding window; are the relative attitude factors of the visual-inertial odometer VIO and the lidar odometer LIO, respectively; is the covariance matrix, used to measure the confidence of the VIO, LIO, and GNSS measurement values in the IMU pre-integration constraint; is the residual of the GNSS measurement value.

[0028] Preferably, the expression of the non-linear optimization problem of the visual-inertial odometer is:

[0029]

[0030] where T i+1 = [R t] is the optimal transformation matrix of the feature points of adjacent frames; i ∈ obs(i) represents the set of all tracked visual feature points i; (i, i+1) ∈ C is the set of all visual nodes connected by the IMU pre-integration factor; e v is the visual reprojection error; e imu is the IMU pre-integration; e m is the marginalization factor; e prior is the attitude prior of GNSS and IMU; W v and W imu are the covariance matrices of the visual reprojection error and the IMU pre-integration, respectively.

[0031] Preferably, the expression of the minimization non-linear optimization problem of the lidar odometer is:

[0032]

[0033] where Ti+1 = [R t] is the optimal transformation matrix of adjacent frame feature points; e e is the residual of the distance from a point to a line; e p is the residual of the distance from a point to a plane; and are respectively an edge feature point and a plane feature point of the (i + 1)-th key frame; and are respectively the set of all edge features and the set of plane features of the (i + 1)-th key frame; W i is the lidar information matrix; e prior is the attitude prior of GNSS and IMU; e imu is the IMU pre-integration.

[0034] Preferably, in step S3, a sliding window and a variable elimination solving algorithm are used to transform the factor graph into a Bayesian network, including the following steps:

[0035] Step S31: Take the IMU state as the core variable and preferentially retain it in the separator S j in, delay its elimination, and the conditional distribution of the Bayesian network is:

[0036]

[0038] In the formula, p(X) is the conditional distribution of the Bayesian network; x imu represents the state variable of the IMU; S imu represents the separator of the IMU; p(x imu |S imu ) represents the conditional probability of the IMU state variable; S j represents the separator related to the variable x j ; the elimination of other sensor variables is conditional on the IMU state; p(x j |S j ) is the conditional probability under all other state variables; j is all non-IMU related state variables;

[0039] Step S32: Assign a higher weight to the IMU pre-integration factor to reflect its dominant position. For the IMU pre-integration factor:

[0040]

[0041] In the formula, φ imu (X imu ) represents the IMU pre-integration factor under the state X imu ; X imu represents the IMU state; A imu and b imuMatrices and vectors related to the IMU, which describe the linear relationship between the state and the observation; Σ imu is the covariance matrix;

[0042] Step S33: When visual features are missing, by increasing the confidence of the IMU pre-integration factor, the noise effects of other sensors are suppressed. At this time, the conditional probability of the Bayesian network is:

[0043]

[0044] In the formula, Ψ(x j ,S j ) is the conditional probability of the Bayesian network; is the block matrix obtained by combining all matrices; x j is the state variable of the vehicle; is the combined term of all vectors; R j is the measurement matrix related to the state variable x j ; T j is the transfer matrix from the sensor to the IMU; d j is the measurement residual; is the information matrix of the IMU; is the offset vector of the IMU.

[0045] The beneficial effects of the present invention at least include:

[0046] 1. By combining data from multiple sensors such as GNSS, lidar, camera, and IMU, the advantages of each sensor can be fully utilized to make up for the deficiencies of a single sensor; even if a certain sensor fails or the data quality deteriorates, such as GNSS signals being blocked or the camera failing in a textureless area, other sensors can still provide sufficient information to maintain the positioning function;

[0047] 2. By constructing a factor graph and minimizing the weighted residual sum of all factors, the sensor data can be globally optimized to further improve the accuracy and consistency of positioning; through the sliding window mechanism and the variable elimination algorithm, the IMU state can be updated and corrected in real time, and the cumulative error can be corrected in time to enhance the robustness of the system in complex environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0048] Figure 1 is a schematic flowchart of the method according to an embodiment of the present invention;

[0049] Figure 2 is the vehicle equipment device according to an embodiment of the present invention;

[0050] Figure 3 is a schematic diagram of the system framework according to an embodiment of the present invention;

[0051] Figure 4Schematic diagram of variable elimination solution for factor graph in embodiments of the present invention. Detailed implementation manners

[0052] The following will clearly and completely describe the technical solutions in the embodiments of the present invention with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall within the protection scope of the present invention.

[0053] As Figure 1 shown, a multi-sensor fusion autonomous driving positioning method centered on an IMU proposed in an embodiment of the present invention includes the following steps:

[0054] Step 1: As Figure 2 shown, mount a GNSS satellite receiving device, a camera, a lidar, and an IMU module on the vehicle device, and collect GNSS signals, image data, point cloud data, and IMU data during movement as the input of the entire positioning method.

[0055] Step 2: Use an accurate IMU mechanization algorithm to compensate for the earth's rotation and Coriolis acceleration. In order to obtain the acceleration and angular velocity of the vehicle's own movement, it is necessary to compensate for gravity and the earth's rotation from the IMU measurement values. At the same time, the kinematic model of inertial navigation is defined as follows:

[0056]

[0057] Where are the position and velocity of the b coordinate system in the w coordinate system respectively, where the b coordinate system is the IMU coordinate system and the w coordinate system is the world coordinate system; the quaternion and the rotation matrix represent the rotation of the b coordinate system relative to the w coordinate system; the w coordinate system is defined at the initial position of the navigation frame or the local ground downward coordinate system, and the navigation frame is defined as the n coordinate system; the IMU coordinate system is defined as the body coordinate; g w and are the gravity vector and the earth's rotation rate in the w coordinate system respectively; is the compensated gyroscope angular velocity; represents the quaternion product. Using the above kinematic model, an accurate IMU mechanization model can be established. The IMU attitude is directly used for real-time navigation and provides assistance for the visual process.

[0058] Step 3: Integrate GNSS and IMU to initialize the IMU, as Figure 3As shown in the red part. GNSS-aided IMU initialization is a process that uses the global position and velocity information provided by GNSS, combined with the acceleration and angular velocity measured by the IMU, to quickly determine the initial state of the navigation system, namely position, velocity, and attitude, and to calibrate sensor biases. First, the latitude, longitude, and altitude data provided by GNSS are converted through coordinate transformation to obtain the initial position, and the velocity information is aligned through coordinates to determine the initial velocity. Then, the accelerometer of the IMU measures the direction of gravity to estimate the pitch angle and roll angle; the horizontal velocity direction of GNSS is used to assist in estimating the heading angle, thus completing the attitude initialization. At the same time, during the stationary phase, the initial bias of the gyroscope is estimated by assuming that the angular velocity is zero, and during dynamic operation, the bias calibration is further optimized through the velocity change of GNSS.

[0059] Step 4: Use the prior attitude of the IMU to assist the entire vision processing process. As Figure 3 shown in the green part, the vision processing process of the visual inertial subsystem includes feature tracking and landmark triangulation, and the steps are as follows:

[0060] Step 401: First, in the visual front-end, the Shi-Tomasi corner detection algorithm is used for feature extraction. The image is divided into several grids of the same size, and feature detection is performed separately in each grid, and the minimum interval between two adjacent pixels is set to maintain the uniform distribution of features. To improve the detection efficiency, multi-thread parallel technology is used.

[0061] Then, the Lukas-Kanade optical flow algorithm is used to track the features. For features without initial depth information, the initial optical flow estimate is obtained by compensating for rotation, and the outliers are removed by combining with the RANSAC algorithm. For features with depth information, the initial optical flow estimate is obtained by projecting the depth information onto the image plane; to further improve the matching accuracy, the reverse direction is implemented, that is, the features are tracked from the current frame to the previous frame, and the features with tracking failure are deleted. After feature tracking, key frame selection is performed. Calculate the average parallax between the current frame and the previous frame. If the average parallax is greater than a fixed threshold, select the current frame as the new key frame.

[0062] Step 402: After selecting a new key frame, use the matching features in the current key frame and the previous key frame for triangulation to determine the initial depth of the landmark. To ensure the accuracy of triangulation, a strict outlier rejection algorithm is adopted. First, calculate the parallax between the features in the current key frame and the corresponding features in the key frame where they were first observed. If the parallax is too small, continue to track the feature until the preset parallax threshold is reached. Subsequently, use the prior pose information provided by the IMU for triangulation of the landmark to obtain the depth of the landmark in the key frame where it was first observed. Finally, check all depths, and only add the landmark to the landmark queue if the depth is within the preset valid range; otherwise, it is regarded as an outlier.

[0063] Step 5: Use the prior pose of the IMU to assist the entire lidar processing process. As Figure 3 shown in the yellow part, the process of the lidar inertial subsystem includes feature extraction and feature matching, and the steps are as follows:

[0064] Step 501: When new lidar scan data is received, extract edge and plane features by evaluating the curvature of points in a local area of the point cloud. Points with larger curvature are classified as edge features, and points with smaller curvature are classified as plane features, which respectively form an edge feature set and a plane feature set. These feature points together form a lidar frame, expressed in the lidar coordinate system. If the change in the vehicle pose relative to the previous state exceeds the preset threshold, the corresponding lidar frame is set as a key frame and added to the factor graph, and adjacent frames are discarded to maintain the sparse map and optimization efficiency.

[0065] Step 502: Use the nearest key frame to construct a local voxel map. Extract a fixed number of key frames and transform them to the world coordinate system, and fuse them to generate a local voxel map containing edge and plane features. Subsequently, downsample the feature point set to remove duplicate points falling in the same voxel grid to improve efficiency. Use the initial pose provided by the GNSS / IMU integrated navigation to transform the new lidar frame to the world coordinate system, and then perform scan matching. Through the initial transformation relationship estimated by the IMU, transform the lidar feature points from the lidar coordinate system to the world coordinate system, and match the edge or plane features in the voxel map to complete the registration of the lidar frame and the local voxel map. Determine the real-time estimation of the vehicle's position and pose through registration, providing constraint conditions for backend optimization.

[0066] Step 6: As Figure 4 shown, use a sliding window optimizer to tightly fuse all measurements within the factor graph optimization framework. When a new key frame is selected or new GNSS-RTK measurement data is valid, a new time node is inserted into the sliding window and factor graph optimization is performed. Only when the GNSS-RTK system successfully calculates a high-precision position and meets the preset accuracy requirements, the relevant data is regarded as valid.

[0067] Step 601: The state vector x in the sliding window of this method k can be defined as:

[0068]

[0069] where the k-th vehicle state x k consists of the position centered on the IMU velocity orientation and the accelerometer bias b a and the gyroscope bias b g .

[0070] The proposed factor graph optimization consists of three modules: GNSS / IMU, VIO, and LIO. This method uses the IMU as the mainstay, which uses the observations provided by VIO and LIO to constrain the accelerometer bias b a and the gyroscope bias b g . In return, the GNSS / IMU integrated navigation provides predictions for VIO and LIO, thus recovering the motion in a coarse-to-fine manner.

[0071] Step 602: Based on the GNSS / IMU integrated navigation in Step 601, the IMU pre-integration factor is defined as:

[0072]

[0073] where is the IMU pre-integration; is the rotation matrix, representing the rotation from the b coordinate system to the w coordinate system at time i, where the b coordinate system is the IMU coordinate system and the w coordinate system is the world coordinate system; are the positions of the IMU coordinate system corresponding to time i and time j in the world coordinate system respectively; are the velocities of the IMU coordinate system corresponding to time i and time j in the world coordinate system respectively; are the rotation quaternions from the b coordinate system to the w coordinate system at time i and time j respectively; and are the Coriolis correction terms for position and velocity pre-integration respectively; and are the measurement values of position, velocity, and attitude pre-integration; the quaternion is the rotation caused by the Earth's rotation; Δt ij represents the time interval between time i and time j; represents the quaternion product; [·] v represents the imaginary part of the quaternion; g w is the gravity vector in the w coordinate system; The accelerometer biases at time i and time j respectively; The gyroscope biases at time i and time j respectively.

[0074] By considering the GNSS lever arm in the b coordinate system The residual of the GNSS-RTK measurement is:

[0075]

[0076] Relative attitude factor: Since the camera and lidar are not globally referenced, only their relative attitudes are used where and The position and rotation between time i and time j are used as local constraints to constrain the position and rotation Relative attitude factor is derived as:

[0077]

[0078] For each frame, an optimization problem is minimized, which consists of the relative attitude factor GNSS-RTK residual and the marginalized prior residual E prior The optimization problem formed is expressed as follows:

[0079]

[0080] where B represents the set of all key frames within the sliding window; the covariance matrix can be calculated according to the reliability of the observations measures the confidence of the VIO, LIO, and GNSS measurements in the IMU pre-integration constraints.

[0081] Step 603: In the visual inertial subsystem, the IMU pre-integration factor provides the motion information of adjacent frames, and introducing the constrained IMU odometry factor can provide the absolute pose information relative to the starting time. Combining GNSS and IMU pre-integration can obtain more accurate absolute prior information, which helps to improve the robustness and accuracy of the system.

[0082] The features observed in the pixel plane can be represented as p p . For a landmark l with a depth of δ l in the first observed key frame i and another observed key frame j, the observation constraint between the landmark node and the camera pose node is used as the visual reprojection factor, and the visual reprojection residual is:

[0083]

[0084] Among them, e v is the visual reprojection error; is the pose of the feature point in the camera coordinate system; is the posterior camera projection function, which uses the camera intrinsic parameters to back-project the feature point on the pixel plane of the key frame to the camera coordinate system; b 1 and b 2 are two orthogonal bases spanning the tangent plane; the rotation matrix is the rotation from the IMU coordinate system b to the camera coordinate system c; the rotation matrix is the rotation from the c coordinate system to the b coordinate system; and represent the rotation between the world coordinate system and the IMU coordinate system at time j; δ l is the depth of the landmark l; is the translation vector from the b coordinate system to the c coordinate system; and are the positions of the IMU coordinate systems corresponding to time i and time j in the world coordinate system, respectively.

[0085] The nonlinear optimization problem minimized by VIO consists of the visual reprojection factor e v , the IMU pre-integration factor e imu , the marginalization factor e m and the attitude prior e prior from GNSS / IMU. Its expression is as follows:

[0086]

[0087] Among them, i∈obs(i) represents the set of all tracked visual feature points i; (i, i + 1)∈C is the set of all visual nodes connected by the IMU pre-integration factor; W v and W imu are the covariance matrices of the visual reprojection factor and the IMU pre-integration factor, respectively; T i+1 = [R t] represents the optimal transformation of the feature points between adjacent frames.

[0088] Step 604: Based on the lidar inertial odometry, the minimized nonlinear optimization problem consists of the lidar factor, the IMU pre-integration factor e imu , and the attitude prior e prior from GNSS / IMU. Its expression is as follows:

[0089]

[0090] Among them, T i+1= [R t] is the optimal transformation matrix of adjacent frame feature points; e e is the residual of the distance from a point to a line; e p is the residual of the distance from a point to a plane; and are respectively an edge feature point and a plane feature point of the (i + 1)-th key frame; and are respectively the set of all edge features and the set of plane features of the (i + 1)-th key frame; W i is the lidar information matrix; e prior is the attitude prior of GNSS and IMU; e imu is the IMU pre-integration.

[0091] Step 7: Use the sliding window and variable elimination solution algorithm to transform the factor graph into a Bayesian network, as Figure 3 shown in purple.

[0092] Step 701: In traditional multi-sensor fusion methods, the mutual relationship between sensors is established through direct conditional dependencies. However, in an IMU-centered framework, the IMU, as the core sensor of the system, its state becomes the basis for the state estimation of all other sensors such as vision, lidar, and GNSS. In this way, the observation information of other sensors not only directly affects their own state variables, but also indirectly affects through the state of the IMU.

[0093] Take the IMU state variable as the core variable and preferentially retain it in the separator S j and delay its elimination. The conditional distribution of the Bayesian network is:

[0094]

[0095] where p(X) is the conditional distribution of the Bayesian network; x imu represents the state variable of the IMU; S imu represents the separator of the IMU; p(x imu |S imu ) represents the conditional probability of the IMU state variable; S j represents the separator related to the variable x j , and the elimination of other sensor variables is conditional on the IMU state; p(x j |S j ) is the conditional probability under all other state variables; j is all non-IMU related state variables.

[0096] Due to the IMU-centered, the information of other sensors affects the estimation of the IMU state. In the SLAM least squares problem, the form of the IMU pre-integration factor is:

[0097]

[0098] Among them, X imu represents the state of the IMU, A imu and b imu are matrices and vectors related to the IMU, which describe the linear relationship between the state and the observation; the inverse of the covariance matrix Σ imu , that is, the information matrix, is significantly larger than other sensors, ensuring that the optimization is centered around the IMU.

[0099] When the perception degrades, such as the absence of visual features, by increasing the confidence of the IMU factor, the noise influence of other sensors is suppressed. At this time, the conditional probability of the Bayesian network is modified as:

[0100]

[0101] Among them, is a larger block matrix combined by all matrices A j , and is also the combination of all vectors b j ; T j is the transfer matrix from the sensor to the IMU, and other sensor factors are dynamically removed during degradation; mainly represents the error terms of the poses and observations of other sensors; is mainly used to constrain the IMU state. By taking the IMU as the central node in multi-sensor fusion, it can not only provide high-precision and high-frequency state estimation, but also enhance the robustness of the system, especially in a degraded perception environment. The introduction of the IMU enables the observations of each sensor to be fused based on the IMU state, thus optimizing the efficiency and accuracy of the overall state estimation; x j is the state variable of the vehicle; is the combined term of all vectors; R j is the measurement matrix related to the state variable x j ; T j is the transfer matrix from the sensor to the IMU; d j is the measurement residual; is the information matrix of the IMU; is the offset vector of the IMU.

[0102] Step 702: In the traditional factor graph algorithm, the information at the previous moment is closely related to the current state estimation. However, the marginalization operation will cause a large amount of historical state information to be discarded, thus weakening the robustness of the system. To solve this problem, the embodiment of the present invention proposes a solution algorithm based on variable elimination. This algorithm sorts the state quantities within a sliding window and marginalizes the historical state information at the same time until a new state enters the sliding window. Its specific process is as Figure 4As shown, the algorithm introduces a sliding window in the factor graph and eliminates variables in the order of state variables x 3 、x 4 、x 5 in turn, and transforms it into a Bayesian network. First, eliminate x 3 , convert the information of the historical state x 2 into a prior and pass it to x 3 , and at the same time use sparse Cholesky decomposition to generate a new factor node τ(S 3 ). Then, marginalize x 3 and eliminate x 4 , and generate a new factor node τ(S 4 ) through sparse Cholesky decomposition. Finally, marginalize x 4 to obtain the maximum a posteriori probability estimate of x 5 . Compared with the method of directly calculating x 3 , x 4 , x 5 , this scheme effectively avoids the problem of high-dimensional matrix inverse operation.

[0103] Step 8: Based on the state estimated in Step 5, feedback it to the IMU mechanization module to update the latest state of the IMU.

[0104] The technical features of the above embodiments can be combined arbitrarily. For the sake of concise description, not all possible combinations of the technical features in the above embodiments are described. Only the preferred embodiments of the present invention are expressed. The description is relatively specific and detailed, but it should not be construed as a limitation to the scope of the present invention. As long as the combinations of these technical features do not conflict, they should be considered as the scope recorded in this specification.

[0105] It should be noted that for those of ordinary skill in the art, without departing from the concept of the present invention, several deformations and improvements can be made, and these all belong to the protection scope of the present invention. Therefore, the protection scope of the present invention should be subject to the appended claims.

Claims

1. An IMU-centered multi-sensor fusion autonomous driving positioning method, characterized in that: The following steps are involved: Step S1: Obtain the vehicle's GNSS measurement value, LiDAR frame, camera pose and IMU state through the receiver of the global navigation satellite system GNSS, camera, LiDAR and inertial sensor IMU, and calculate the vehicle's position, speed and attitude in real time according to the IMU state; Step S2: construct a factor graph by taking IMU state, camera pose, and lidar frame as nodes, and IMU pre-integration, visual reprojection error, lidar matching error, and GNSS measurement value as factors; Step S3: Taking the weighted residual sum of all factors minimized as the objective function, taking the IMU state as the core variable, using the sliding window mechanism and variable elimination algorithm to solve the objective function, and updating the nodes in the factor graph; Step S4: Feedback the updated node to the IMU, update the IMU state, and repeat steps S1 to S3 to achieve autonomous driving positioning of the vehicle.

2. The IMU-centered multi-sensor fusion autonomous driving positioning method according to claim 1, characterized in that: In step S1, the global position and velocity information of the vehicle is received through a GNSS receiver, and the accelerometer bias and gyroscope bias of the IMU are corrected.

3. The IMU-centered multi-sensor fusion autonomous driving positioning method according to claim 1, characterized in that: In step S2, the observation constraint between the landmark node and the camera pose node is used as the visual reprojection error, and obtaining the landmark node includes the following steps: Step S201: Divide the image captured by the camera into a number of grids of the same size, and extract visual features in each grid separately; Step S202: using an optical flow algorithm to track the visual features of each grid, calculating the average disparity between the current frame and the previous frame, and if the average disparity is greater than a set threshold, taking the current frame as a key frame; Step S203: triangulate the matching visual features in the current key frame and the previous key frame using the prior attitude information provided by the IMU to determine the depth of the landmark in the key frame, and use the landmark whose depth is within the set landmark depth range as the landmark node.

4. The IMU-centered multi-sensor fusion autonomous driving positioning method according to claim 1, characterized in that: In step S2, the matching error between the laser radar frame and the local map is used as the laser radar matching error. Matching the laser radar frame with the local map includes the following steps: Step S211: extracting edge features and plane features in the laser radar point cloud through curvature evaluation to form a radar frame; Step S212: when the vehicle posture change exceeds a threshold, the corresponding radar frame is set as a key frame, and a local voxel map is constructed through the key frame; Step S213: Based on the initial posture of the IMU, the newly received radar frame is matched with the local voxel map.

5. The IMU-centered multi-sensor fusion autonomous driving positioning method according to claim 1, characterized in that: The expression of the IMU pre-integration in step S2 is: In the formula, Pre-integration for IMU; is the rotation matrix, which represents the rotation from the b coordinate system to the w coordinate system at time i, where the b coordinate system is the IMU coordinate system and the w coordinate system is the world coordinate system; are the positions of the IMU coordinate system corresponding to time i and time j in the world coordinate system respectively; are the velocities of the IMU coordinate system corresponding to time i and time j in the world coordinate system respectively; The rotation quaternion from the b coordinate system to the w coordinate system at time i and time j respectively; and are the Coriolis correction terms for position and velocity pre-integration respectively; and They are the pre-integrated measurements of position, velocity and attitude; quaternion is the rotation caused by the rotation of the Earth; Δt ij Represents the time interval between time i and time j; represents quaternion product; [·] v Represents the imaginary part of the quaternion; g w is the gravity vector in the w coordinate system; are the accelerometer biases at time i and time j respectively; are the gyroscope biases at time i and time j respectively.

6. The IMU-centered multi-sensor fusion autonomous driving positioning method according to claim 1, characterized in that: The expression of the visual reprojection error in step S2 is: In the formula, e v is the visual reprojection error; is the pose of the feature point in the camera coordinate system; is the rear camera projection function, which uses the camera internal parameters to transform the feature points on the pixel plane of the key frame Back-projected to the camera coordinate system; b1 and b2 are spanning Two orthogonal bases for the tangent plane; rotation matrices is the rotation from the IMU coordinate system b to the camera coordinate system c; rotation matrix is the rotation from c coordinate system to b coordinate system; and represents the rotation between the world coordinate system and the IMU coordinate system at time j; δ l is the depth of landmark l; is the translation vector from the b coordinate system to the c coordinate system; and They are the positions of the IMU coordinate system corresponding to time i and time j in the world coordinate system respectively.

7. The IMU-centered multi-sensor fusion autonomous driving positioning method according to claim 1, characterized in that: The expression of the objective function in step S3 is: Where E is the objective function; E prior is the marginalized prior residual; B represents the set of all key frames in the sliding window; They are the relative attitude factors of the visual inertial odometry VIO and the lidar odometry LIO respectively; is the covariance matrix, which is used to measure the confidence of VIO, LIO and GNSS measurements on the IMU pre-integration constraints; is the residual of the GNSS measurement.

8. The IMU-centered multi-sensor fusion autonomous driving positioning method according to claim 7, characterized in that: The expression of the nonlinear optimization problem of visual inertial odometry is: Where, T i+1 =[R t] is the optimal transformation matrix of feature points in adjacent frames; i∈obs(i) represents the set of all tracked visual feature points i; (i,i+1)∈C is the set of all visual nodes connected by the IMU pre-integration factor; e v is the visual reprojection error; e imu is the IMU pre-integration; e m is the marginalization factor; e prior is the attitude prior of GNSS and IMU; W v and W imu are the covariance matrices of visual reprojection error and IMU pre-integration, respectively.

9. The IMU-centered multi-sensor fusion autonomous driving positioning method according to claim 7, characterized in that: The expression of the minimization nonlinear optimization problem of the lidar odometer is: Among them, T i+1 =[R t] is the optimal transformation matrix of feature points in adjacent frames; e e is the residual of the distance from the point to the line; e p is the residual of the distance from point to surface; and They are an edge feature point and a plane feature point of the i+1th key frame respectively; and are all edge feature sets and plane feature sets of the i+1th key frame respectively; W i is the laser radar information matrix; e prior is the attitude prior of GNSS and IMU; e imu Pre-integration for IMU.

10. The IMU-centered multi-sensor fusion autonomous driving positioning method according to claim 1, characterized in that: In step S3, a sliding window and variable elimination algorithm are used to transform the factor graph into a Bayesian network, including the following steps: Step S31: The IMU state is used as the core variable and is preferentially retained in the separator S j In the case of delaying elimination, the conditional distribution of the Bayesian network is: Where p(X) is the conditional distribution of the Bayesian network; x imu Represents the state variable of IMU; S imu represents the separator of IMU; p(x imu |S imu ) represents the conditional probability of the IMU state variables; S j Represents the variable x j Related separators, elimination of other sensor variables is conditioned on the IMU state; p(x j |S j ) is the conditional probability under all other state variables; j is all non-IMU related state variables; Step S32: Give a higher weight to the IMU pre-integration factor to reflect its dominant position. For the IMU pre-integration factor: In the formula, φ imu (X imu ) means in state X imu IMU pre-integration factor under X imu Indicates the IMU status; A imu and b imu are matrices and vectors related to IMU, describing the linear relationship between state and observation; Σ imu is the covariance matrix; Step S33: When visual features are missing, the noise influence of other sensors is suppressed by increasing the confidence of the IMU pre-integration factor. At this time, the conditional probability of the Bayesian network is: In the formula, Ψ(x j ,S j ) is the conditional probability of the Bayesian network; is the block matrix obtained by merging all matrices; x j is the state variable of the vehicle; is the combined term of all vectors; R j is the state variable x j The associated measurement matrix; T j is the transfer matrix from sensor to IMU; d j is the measurement residual; is the information matrix of IMU; is the offset vector of the IMU.

Images