Factor graph optimization vehicle positioning method and system considering SINS (Strapdown Inertial Navigation System) mechanical arrangement

Abstract

The invention discloses a factor graph optimization vehicle positioning method and system considering SINS (Strapdown Inertial Navigation System) mechanical arrangement, which effectively reduces the error accumulation of IMU (Inertial Measurement Unit) and ODO (Optical Distribution Object) data through a fine SINS mechanical arrangement error correction model, and can provide a stable and high-precision positioning result especially in the environment of long-time operation and weak GPS (Global Positioning System) signal. Compared with other multi-source information fusion methods such as federated filtering, the factor graph fusion method can process asynchronous heterogeneous sensor data, the plug and play function of the sensor in the system is met, and the flexibility of integrated navigation is improved. The method can be flexibly fused with other sensors, such as a visual sensor, a laser radar and the like, and is suitable for vehicle positioning requirements in various dynamic environments.

Description

Technical Field

[0001] The present invention relates to the technical field of vehicle navigation and positioning, and in particular to a factor graph optimization vehicle positioning method and system considering SINS mechanical arrangement. Background Art

[0002] With the rapid development of smart transportation and smart vehicles, the requirements for the accuracy of motion carrier state estimation are getting higher and higher. A single sensor can no longer meet the existing navigation needs. By fusing the information of various navigation source sensors, the advantages of sensors can be complemented, and the accuracy, fault tolerance and reliability of the integrated navigation system can be improved. According to the structure of the fusion algorithm, the multi-source information fusion algorithm can be roughly divided into three categories: centralized fusion method, parallel fusion method and sequential fusion method. Among them, the most mature and widely used centralized fusion method in integrated navigation is Kalman Filter (KF), which is an optimal estimate under the assumption that the observed quantity is linear and conforms to the Gaussian distribution. In practical applications, the observed quantity is usually nonlinear and non-Gaussian, so nonlinear filtering methods such as Extended Kalman Filter (EKF), Unscented Kalman Filter (UKF) and Particle Filter (PF) have emerged. Although these methods solve the nonlinear and non-Gaussian problems, the algorithm complexity increases, resulting in reduced fusion efficiency. Some scholars have also introduced neural networks into the fusion algorithm to learn and train the system state information, but this method requires a large number of training samples and has poor real-time performance. The representative parallel fusion method is the Federated Kalman Filter (FKF) proposed by Carlson. This algorithm is a decentralized KF, consisting of multiple sub-filters and a main filter. The global optimal estimate is obtained by fusion of the main filter and each sub-filter. This method has the advantages of good real-time performance and high calculation accuracy, but the output of each filter is inconsistent, which is easy to introduce global estimation errors, and it is overly dependent on observation information and has poor fault tolerance. The sequential fusion method is the main fusion method to solve asynchronous heterogeneous navigation source information, including dynamic and static filtering methods, interactive adaptive methods, etc. A relatively new method is the factor graph method proposed by Kschischang FR et al. in the 1990s. The factor graph is a probabilistic graph model that encodes the observation information of each navigation source, builds a multi-source information fusion factor graph framework for all sensor observation factors through node connections, and performs message transmission and iteration according to certain rules to complete parameter estimation and data fusion. This method can fuse non-co-frequency and non-synchronous sensor observation information, where the information of each navigation source is independent of each other, and is fused in sequence and passed downward. When adding or deleting sensors, only the corresponding nodes need to be added or deleted in the factor graph, meeting the plug-and-play system requirements.

[0003] However, although IMU can provide continuous pose estimation, the errors of IMU (such as zero bias and noise of gyroscope and accelerometer) will accumulate over time, and ODO will also have certain installation errors and scale factor errors, resulting in a gradual decrease in positioning accuracy. At present, most factor graph multi-source information fusion methods use IMU pre-integration method to process IMU data, ignoring the particularity of IMU mechanical arrangement and accurate modeling of IMU errors.

[0004] Based on the above problems, a factor graph fusion method considering the mechanical arrangement of SINS is proposed to reduce the error between IMU factors and ODO factors before factor graph fusion and improve the accuracy of the fusion algorithm. This method is based on the IMU and ODO update method of SINS mechanical arrangement, accurately models IMU errors and ODO errors, combines GNSS factors, and uses the factor graph optimization framework to perform multi-source information fusion to achieve high-precision and robust vehicle positioning. Summary of the invention

[0005] The purpose of the present invention is to solve the above-mentioned problems and to provide a method and system for optimizing vehicle positioning by factor graph considering SINS mechanical arrangement.

[0006] To achieve the above purpose, the technical solution adopted by the present invention is: the specific steps are as follows:

[0007] Step 1: Obtain sensor measurement information, including the angular velocity and specific force measurement information output by the gyroscope and accelerometer in the inertial measurement unit, the latitude and longitude measurement information output by the GNSS, and the speed information output by the odometer.

[0008] Step 2: Analyze the real-time motion information data and synchronize the data in time and space;

[0009] Step 3: Based on the collected real-time motion information data, the state space modeling of the GNSS system and SINS system data is carried out. On this basis, a nonlinear state space model about the factor graph is constructed so that the trajectory data can be corrected later using the factor graph model;

[0010] Step 4: Use IMU sensor data to update attitude, speed, and position;

[0011] Step 5: Introduce the error model to correct the error of the updated posture, speed, and position, improve the data accuracy before the factor graph multi-source information fusion, and improve the vehicle positioning accuracy;

[0012] Step 6: Perform dead reckoning based on ODO data and attitude information;

[0013] Step 7: Calculate the dead reckoning error and perform error correction;

[0014] Step 8: Construct a factor graph model. Using the prior probability P(X) of state X, based on the Bayesian formula, and the obtained posterior probability P(X|Z), the posterior probability is converted into the maximum posterior estimation probability, and the maximum posterior estimation probability is simplified through marginalization.

[0015] Step 9: Based on the constructed factor graph model, rewrite the simplified maximum a posteriori estimated probability in step 8 into a function decomposition formula, perform nonlinear least squares processing, perform nonlinear linearization processing as needed, and transform it into a standard least squares problem. By introducing the Gauss-Newton iteration method, the trajectory of the vehicle combination positioning is corrected;

[0016] Step 10: Perform similar error correction on the data of each node to obtain the optimal estimate of the state of each node in turn, thereby achieving high-precision positioning of the vehicle.

[0017] A factor graph optimized vehicle positioning system considering SINS mechanical arrangement, used to execute the method of claim 1, the system comprises the following parts:

[0018] External measurement antenna module, the external measurement antenna module adopts dual antenna satellite acquisition unit

[0019] GNSS receiver module, the GNSS receiver module adopts multi-frequency multi-system high-precision GNSS receiver with a sampling frequency of 1Hz

[0020] IMU acquisition module, IMU acquisition module uses MEMS, the sampling frequency is 125Hz

[0021] ODO acquisition module, ODO acquisition module uses a direct-plug wheel odometer with a sampling frequency of 20Hz

[0022] Data fusion module: The data fusion module includes: acquisition board and fusion board. The acquisition board is used to collect GNSS, IMU, and ODO data, and the fusion board is responsible for integrating and calculating various data;

[0023] The power module is used to supply power.

[0024] Compared with the prior art, the present invention has the following beneficial effects:

[0025] The present invention effectively reduces the error accumulation of IMU and ODO data through a sophisticated SINS mechanical arrangement error correction model, and can provide stable and high-precision positioning results, especially in long-term operation and weak GPS signal environments.

[0026] Through the factor graph optimization method, the fusion of IMU, GNSS and ODO data can suppress the errors between sensors. Especially when the GNSS signal is lost or the ODO error is large, the system can still maintain a high robustness. At the same time, the factor graph optimization method can efficiently solve the global optimal solution, process multi-source data and optimize the weight relationship between sensors, thereby improving positioning accuracy and ensuring the real-time performance of the algorithm.

[0027] Compared with other multi-source information fusion methods such as federated filtering, the factor graph fusion method adopted in the present invention can process asynchronous heterogeneous sensor data, meet the plug-and-play function of sensors in the system, and improve the flexibility of combined navigation. This method can be flexibly fused with other sensors, such as visual sensors, lidar, etc., and is suitable for vehicle positioning needs in various dynamic environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0028] Figure 1 It is the overall technical roadmap of the present invention;

[0029] Figure 2 It is a hardware structure block diagram of the present invention;

[0030] Figure 3 This is a flowchart of the key improvement method of the present invention;

[0031] Figure 4 is a system flow chart of the present invention;

[0032] Figure 5 Schematic diagram of the factor graph algorithm of the present invention;

[0033] Figure 6 It is a schematic diagram of the improved factor graph positioning of the present invention. DETAILED DESCRIPTION

[0034] In order to make the technical means, creative features, objectives and effects achieved by the present invention easy to understand, the present invention is further explained below in conjunction with specific implementation methods.

[0035] A factor graph fusion method considering the mechanical arrangement of SINS is proposed. The specific steps are as follows:

[0036] Step 1: Obtain sensor measurement information, including the angular velocity and specific force measurement information output by the gyroscope and accelerometer in the inertial measurement unit, the latitude and longitude measurement information output by the GNSS, and the speed information output by the odometer.

[0037] In step 1, the “east-north-sky” is used as the navigation coordinate system, and the IMU outputs the information of the three-axis accelerometer and three-axis gyroscope of the vehicle-mounted system; the data output by the GNSS receiver includes: the three-dimensional velocity (v E -vN -v U ), longitude (λ), latitude (L), and altitude (h); ODO outputs the forward velocity in the vehicle carrier coordinate system, and the right and sky velocities are both zero;

[0038] Step 2: Analyze the real-time motion information data and synchronize the data in time and space;

[0039] In step 2, a preliminary solution is performed on the GNSS raw data to obtain the corresponding carrier position information and speed information in the GNSS data, and the obtained data is converted into a coordinate system to achieve spatial synchronization between the GNSS data and the SINS data. Combined with the time of data output, the data is synchronized in time.

[0040] Step 3: Based on the collected real-time motion information data, the state space modeling of the GNSS system and SINS system data is carried out. On this basis, a nonlinear state space model about the factor graph is constructed so that the trajectory data can be corrected later using the factor graph model;

[0041] In step 3, the "east-north-sky" three-dimensional geographic coordinate system (ie, the ENU geographic coordinate system) is selected as the solution coordinate system to construct the vehicle combination positioning system and determine the state equation and measurement equation of the vehicle combination positioning system. The state equation and measurement equation are:

[0042]

[0043] Among them, W k-1 is the system process noise, W k-1 ~N(0,Q k-1 );Γ k|k-1 is the process noise distribution matrix; Φ k|k-1 t k-1 to k The system state transfer matrix at time Z, the subscript k-1 represents the k-1 time; k is the measurement vector, H k is the measurement matrix, V k is the measurement noise vector, V k ~N(0,R k );

[0044] State variables Where φ=[φ E φ N φ U ] is the platform misalignment angle in the three directions of “east-north-sky”, ε=[ε E ε N ε U ] is the zero bias error of the three-axis gyroscope, is the random walk error of the triaxial accelerometer; δV = [δV E δV N δV U ] is the velocity error in the three directions of “east-north-sky”, Respectively represent the errors of longitude, latitude and altitude; δp D =[δp E δp N δp U ] is the three-way dead reckoning error;

[0045] Step 4: Use IMU sensor data to update attitude, speed, and position;

[0046] In step 4, the attitude, velocity, and position are updated using the IMU sensor data and SINS mechanical choreography;

[0047] (1) Attitude solution update. In the SINS system, the carrier's attitude differential equation is:

[0048]

[0049] In the formula, is the angular velocity of the carrier system rotating relative to the navigation system; is the posture matrix; Represented by The antisymmetric matrix is ​​established.

[0050] (2) Speed ​​solution update. The specific force equation of the inertial navigation system is:

[0051]

[0052] in, is the specific force measured by the accelerometer; is the Coriolis acceleration caused by the carrier motion and the Earth's rotation; is the centripetal acceleration caused by the carrier motion; g n is the acceleration due to gravity.

[0053] Expand the above formula to get the velocity expressions of the three axes of the northeast sky:

[0054]

[0055] In the formula, λ, h is the current geographical location of the carrier, including longitude, latitude and altitude; is the radius of curvature on the earth meridian at that point, is the radius of curvature on the Earth's circumference, R eIt represents the radius of the earth in the earth ellipsoid model, and e is the first eccentricity of the earth.

[0056] (3) Position solution update: The position (longitude, latitude and altitude) differential equation of the strapdown inertial navigation system is:

[0057]

[0058]

[0059]

[0060] Step 5: Introduce the error model to correct the error of the updated posture, speed, and position, improve data accuracy before performing factor graph multi-source information fusion, and improve vehicle positioning accuracy;

[0061] In step 5, the error model is introduced to perform error correction on the updated posture, speed, and position, so as to improve data accuracy and vehicle positioning accuracy before performing factor graph multi-source information fusion.

[0062] (1) Attitude error equation. The ideal error-free strapdown inertial navigation attitude matrix from the navigation coordinate system (n system) to the carrier coordinate system (b system) is: The posture matrix given by the solution is and The corresponding navigation coordinate system is the computational navigation coordinate system, abbreviated as n′ system, and the computational attitude matrix is ​​recorded as and The deviation between is the deviation between the n′ system and the n system.

[0063] According to the matrix chain multiplication rule, we have

[0064]

[0065] Taking the n system as the reference coordinate system, the equivalent rotation vector from the n system to the n′ system is φ nn′ (hereinafter referred to as φ), which is often called the misalignment angle error. Assuming φ is a small quantity, according to the relationship between the equivalent rotation vector and the direction cosine matrix, we can approximately get

[0066]

[0067] Transposing the above formula, we have

[0068]

[0069] Substituting formula (10) into formula (8), we can get

[0070]

[0071] The formula for solving the ideal posture matrix is ​​as follows:

[0072]

[0073] In actual calculation, each quantity contains errors, which can be expressed as

[0074]

[0075] in,

[0076]

[0077]

[0078] is the gyro measurement error; Calculate the error for the navigation system.

[0079] Differentiate both sides of equation (11) at the same time, and its right side should be exactly equal to the right side of equation (13), that is,

[0080]

[0081] Substituting equations (8), (12), (14) and (15) into equation (16), we can obtain

[0082]

[0083] Multiply both sides of formula (17) by Expanding and ignoring the second-order small quantity about the error, we get

[0084]

[0085] In the first term on the right side of equation (18), the formula (V1×)(V2×)-(V2×)(V1×)=[(V1×V2)×] is used, and in the third term, the similarity transformation of the antisymmetric matrix is ​​used. Then equation (18) can be simplified to

[0086]

[0087] Thus there is

[0088]

[0089] That is, the strapdown inertial navigation attitude error differential equation is obtained, which reflects the variation law of the misalignment angle φ of the computational navigation system (n′ system) relative to the ideal navigation system (n system).

[0090] (2) Speed ​​error equation. Speed ​​error is the deviation between the calculated speed and the ideal speed in the inertial navigation system. The calculated speed is expressed as Abbreviated as The velocity error is defined as

[0091]

[0092] Differentiating both sides of equation (21) yields

[0093]

[0094] The theoretical formula of the specific force equation is as follows:

[0095]

[0096] In actual calculation, it is expressed as

[0097]

[0098] in,

[0099]

[0100]

[0101]

[0102]

[0103] is the accelerometer measurement error; δg n They are the calculation error of the earth's rotation angular velocity, the calculation error of the navigation system rotation and the gravity error.

[0104] Subtracting equation (23) from equation (24), we get

[0105]

[0106] Substitute equation (11), equation (25) to equation (28) into equation (29), expand and ignore the second-order small quantity related to the error, and we get

[0107]

[0108] The above equation is the strapdown inertial navigation velocity error differential equation.

[0109] (3) Position error equation. The deviation of the strapdown inertial navigation position (longitude, latitude and altitude) is calculated using the differential equations (5 to 7), where R M and R N In a short period of time, the change is very small and is regarded as a constant value.

[0110]

[0111]

[0112]

[0113] Among them, δλ, and δh represent the longitude error, latitude error and height error respectively, and the IMU velocity component V n =[V E V N V U ] T , velocity error component δV n =[δV E δV N δV U ] T ;

[0114] Step 6: Perform dead reckoning based on ODO data and attitude information;

[0115] In step 6, the attitude, heading and mileage information are used to estimate the relative position of the vehicle relative to the starting point.

[0116] The odometer coordinate system is a "right-front-up" right-handed rectangular coordinate system fixed to the vehicle body. The speed output of the odometer can be expressed in the odometer coordinate system as

[0117]

[0118] Among them, v D It is the forward speed measured by the odometer; the right and skyward speeds are both zero, which is regarded as the speed constraint condition when the vehicle is driving normally.

[0119] Speed ​​by odometer The differential equation for dead reckoning positioning solution is as follows:

[0120]

[0121]

[0122]

[0123] The above three equations are written in vector form, which is

[0124]

[0125] in, λ D and h D are the geographical latitude, longitude and altitude of dead reckoning respectively; R MhD =R MD +h D , R NhD =RND +h D , R MD and R ND are the principal curvature radii of the meridian and meridian calculated using the dead reckoning geographic location; And there is

[0126]

[0127] In the inertial navigation attitude differential equation, the velocity is calculated using ODO Replace SINS speed V n , and calculate the latitude by dead reckoning Replace strapdown inertial navigation to calculate latitude The attitude matrix differential equation for dead reckoning is as follows:

[0128]

[0129] in, is the angular velocity information of the three gyro components, and

[0130]

[0131]

[0132] Equation (38) and equation (39) together constitute the dead reckoning algorithm.

[0133] Step 7: Calculate the dead reckoning error and perform error correction;

[0134] In the actual system, it is difficult to ensure that the strapdown inertial unit is completely parallel to each coordinate axis when installed on the vehicle. Assume that there is a small installation deviation angle from the vehicle body coordinate system (m system) to the carrier coordinate system (b system), that is, around the vehicle body horizontal axis ox m 、Vertical axisoy m And vertical axis oz m There are pitch angles α θ 、Rolling angle α γ and azimuth angle α ψ , let the deviation angle vector α = [α θ α γ α ψ ] T , then similar to formula (9), the transformation matrix can be obtained

[0135]

[0136] In addition, there may be a scale factor error δK in the actual odometer measurement. D , the output speed The theoretical speed v D The relationship between

[0137]

[0138] Formula (41) is expressed as a vector

[0139]

[0140] Therefore, the actual speed output of the odometer in the navigation coordinate system is

[0141]

[0142] Among them, φ D is the attitude misalignment angle calculated by dead reckoning. and Substituting the third and fourth terms on the right side of equation (46), we can obtain

[0143]

[0144] Formula (47) shows that the rolling angle α γ It does not affect the speed measurement value of the odometer. Further simplifying equation (47), we have

[0145]

[0146] Among them,

[0147]

[0148] The odometer speed error equation can be obtained from equation (48):

[0149]

[0150] By calculating the deviation on both sides of the dead reckoning position equation (38), we can obtain

[0151]

[0152] in,

[0153]

[0154]

[0155] Substituting equation (50) into equation (51), we get the dead reckoning position error equation:

[0156]

[0157] in,

[0158]

[0159] Similar to the attitude error equation (20) of strapdown inertial navigation, it is not difficult to obtain

[0160]

[0161] in,

[0162]

[0163]

[0164] Substituting equation (50) into equation (56), we get the attitude error equation for dead reckoning:

[0165]

[0166] in,

[0167]

[0168] Step 8: Construct a factor graph model. Using the prior probability of the state, based on the Bayesian formula, and using the obtained posterior probability, convert the posterior probability into the maximum posterior estimation probability, and simplify the maximum posterior estimation probability through marginalization;

[0169] In step 8, a factor graph model is constructed. The factor graph is a bipartite graph model that represents the joint probability distribution of random variables and can decompose the global function of multivariate variables into the product of multiple local function factors.

[0170] The factor graph model includes two types of nodes: variable nodes, which represent variables of global multivariate functions, and factor nodes, which represent decomposition factors. Variable nodes are connected to factor nodes through edges, so the factor graph is defined as:

[0171] G=(F,X,E) (61)

[0172] In formula (61), is a set of variable nodes, m is the number of variable nodes; F = {f1(X1),f2(X2),...,f n (X n )} represents a group of factor nodes, n represents the number of factor nodes; edge E = (e ij ) consists of variable nodes and factor nodes connected by lines.

[0173] Rewrite the factor graph definition into a function decomposition form:

[0174]

[0175] Introduce a specific factor graph instance, such as Figure 5As shown in the example. Let g(x1,x2,x3,x4) be a global function containing 4 variables. According to the above formula This function can be expressed as:

[0176] g(x1,x2,x3,x4)=f1(x1)f2(x1)f3(x1,x2,x3)f4(x2)f5(x3,x4)f6(x4) (63)

[0177] Where X={x1,x2,……,x m}, and f i (X i ) has many forms. Each variable x corresponds to a variable node, and each local function f corresponds to a factor node. If and only if the variable x is the independent variable of the local function f, there is an edge connecting the two nodes between the corresponding variable node and the factor node.

[0178] Step 9: Based on the constructed factor graph model, rewrite the simplified maximum a posteriori estimated probability in step 8 into a function decomposition formula, perform nonlinear least squares processing, perform nonlinear linearization processing as needed, and transform it into a standard least squares problem. By introducing the Gauss-Newton iteration method, the trajectory of the vehicle combination positioning is corrected;

[0179] In step 9, the factor graph model is estimated. According to Bayesian estimation, the joint probability density p(X k |Z k ) can be decomposed into:

[0180]

[0181] And the global conditional probability density function is proportional to the likelihood probability density in the numerator and the prior probability of state transition:

[0182]

[0183] According to the maximum a posteriori estimation criterion, the state variable with the maximum a posteriori probability density is regarded as the estimator. Therefore, the calculation of the factor graph is essentially to solve the joint probability distribution p(X k |Z k ) is estimated through the maximum a posteriori probability, and the maximum a posteriori probability expression is:

[0184]

[0185] To calculate the maximum a posteriori probability estimate The right side of equation (65) p(Z i |X i )p(X i |X i-1 ) will be maximized.

[0186] For Gaussian distribution, p(Z i |X i ) can be expressed as:

[0187]

[0188] In formula (67), W is the variance matrix, h i (X i ) is the measurement function, Z i is the measured value.

[0189] Set the cost function to:

[0190] J=[Z i -h i (X i )] T W -1 [Z i -h i (X i )]=min (68)

[0191] Therefore, let the cost function take the minimum value and solve the optimal estimate. For linear quantity measurement, the optimal estimate can be solved by the extreme value method. For nonlinear quantity measurement, Newton iteration method can be used to solve. When a new factor node Z is added to the graph k When the optimal solution is calculated, the measurements of different sensors are used.

[0192] Similarly, p(X i |X i-1 ) can be calculated in the same way:

[0193]

[0194] In formula (69), P is the variance matrix.

[0195] Set the cost function to:

[0196]

[0197] Let the cost function of the error function take the minimum value, the optimal state estimator Multi-source information fusion can be completed.

[0198] Step 10: Perform similar error correction on the data of each node, and obtain the optimal estimate of the state of each node in turn to achieve high-precision positioning of the vehicle;

[0199] In step 10, the measurement information output by IMU, GNSS, and ODO is defined as the factor node of the factor graph, and the state information of the navigation system is defined as the variable node of the factor graph, so as to construct a system framework diagram of multi-source information fusion based on the factor graph, as shown in FIG. Figure 6 Therefore, the multi-source information fusion method based on factor graph has quite good flexibility and can realize the plug-and-play of sensors.

[0200] The system for executing the above method includes the following contents:

[0201] External measurement antenna module, GNSS receiver module, IMU acquisition module, ODO acquisition module, data fusion module, power module; the external measurement antenna module adopts a dual-antenna satellite acquisition unit; the GNSS receiver module adopts a multi-frequency and multi-system high-precision GNSS receiver with a sampling frequency of 1Hz; the IMU acquisition module adopts MEMS with a sampling frequency of 125Hz; the ODO acquisition module adopts a plug-in wheel odometer with a sampling frequency of 20Hz; the data fusion module includes: an acquisition board for collecting GNSS, IMU, and ODO data; a fusion board for integrating and calculating various data; the system hardware structure is as follows: Figure 2 As shown;

[0202] It will be apparent to those skilled in the art that the invention is not limited to the details of the exemplary embodiments described above and that the invention can be implemented in other specific forms without departing from the spirit or essential features of the invention. Therefore, the embodiments should be considered exemplary and non-limiting in all respects, and the scope of the invention is defined by the appended claims rather than the foregoing description, and it is intended that all variations falling within the meaning and scope of the equivalent elements of the claims be included in the invention. Any reference numeral in a claim should not be considered as limiting the claim to which it relates.

[0203] In addition, it should be understood that although the present specification is described according to implementation modes, not every implementation mode contains only one independent technical solution. This description of the specification is only for the sake of clarity. Those skilled in the art should regard the specification as a whole. The technical solutions in each embodiment may also be appropriately combined to form other implementation modes that can be understood by those skilled in the art.

Claims

1. A factor graph optimization vehicle positioning method considering SINS mechanical arrangement, characterized in that: The specific steps are as follows: Step 1: Obtain sensor measurement information, including the angular velocity and specific force measurement information output by the gyroscope and accelerometer in the inertial measurement unit, the longitude and latitude measurement information output by the GNSS, and the speed information output by the odometer; Step 2: Analyze the real-time motion information data and synchronize the data in time and space; Step 3: Based on the collected real-time motion information data, the state space modeling of the GNSS system and SINS system data is carried out. On this basis, a nonlinear state space model about the factor graph is constructed so that the trajectory data can be corrected later using the factor graph model; Step 4: Use IMU sensor data to update attitude, speed, and position; Step 5: Introduce the error model to correct the error of the updated posture, speed, and position, improve the data accuracy before the factor graph multi-source information fusion, and improve the vehicle positioning accuracy; Step 6: Perform dead reckoning based on ODO data and attitude information; Step 7: Calculate the dead reckoning error and perform error correction; Step 8: Build a factor graph model using state The prior probability , based on the Bayesian formula, using the obtained posterior probability , convert the posterior probability into the maximum a posteriori estimation probability, and simplify the maximum a posteriori estimation probability through marginalization; Step 9: Based on the constructed factor graph model, rewrite the simplified maximum a posteriori estimated probability in step 8 into a function decomposition formula, perform nonlinear least squares processing, perform nonlinear linearization processing as needed, and transform it into a standard least squares problem. By introducing the Gauss-Newton iteration method, the trajectory of the vehicle combination positioning is corrected; Step 10: Perform similar error correction on the data of each node to obtain the optimal estimate of the state of each node in turn, thereby achieving high-precision positioning of the vehicle.

2. A factor graph optimization vehicle positioning system considering SINS mechanical arrangement, characterized by: For executing the method according to claim 1, the system comprises the following parts: External measurement antenna module, the external measurement antenna module adopts dual antenna satellite acquisition unit GNSS receiver module, the GNSS receiver module adopts multi-frequency multi-system high-precision GNSS receiver with a sampling frequency of 1Hz IMU acquisition module, IMU acquisition module uses MEMS, the sampling frequency is 125Hz ODO acquisition module, ODO acquisition module uses a direct-plug wheel odometer with a sampling frequency of 20Hz Data fusion module: The data fusion module includes: acquisition board and fusion board. The acquisition board is used to collect GNSS, IMU, and ODO data, and the fusion board is responsible for integrating and calculating various data; The power module is used to supply power.

Images