Vehicle cluster navigation source data fusion cooperative positioning method based on factor graph

Abstract

The invention provides a vehicle cluster navigation source data fusion cooperative positioning method based on a factor graph, and the method comprises the steps: firstly constructing a vehicle cluster distributed cooperative positioning system model, obtaining positioning parameters, and obtaining a vehicle navigation source positioning result and a three-dimensional covariance matrix through calculation; then distance and direction finding information between vehicles is obtained, an obtained vehicle navigation source positioning result is combined, a vehicle internal factor graph is obtained, and a predicted positioning result of a cooperative vehicle on a to-be-positioned vehicle and a three-dimensional covariance matrix of the predicted positioning result are calculated; and finally, carrying out orthogonal decomposition on the predicted positioning result to obtain positioning data on each orthogonal axis, carrying out hierarchical screening on a positioning data set of each axis, identifying and eliminating navigation source positioning data with relatively large errors, and updating factor graph parameters by utilizing the screened data of each axis to obtain a vehicle fusion cooperative positioning result in the vehicle cluster network. The method is suitable for large-scale vehicle cluster navigation positioning requirements in a complex environment, and the obtained positioning result is high in stability and robustness.

Description

Technical Field

[0001] The present invention belongs to the technical field of navigation, and particularly relates to a method for fusing and collaborative positioning of vehicle cluster navigation source data based on a factor graph. Background Art

[0002] With the rapid development of technologies such as the Internet of Things, artificial intelligence, and big data, distributed collaborative positioning technology has shown great potential in many fields. In distributed collaborative positioning, each node has certain sensing and computing capabilities and can exchange and share information with other nodes through communication links. By utilizing the relative position relationships between nodes and the information of collaborative nodes, the position of the target node can be accurately inferred. This technology exhibits high robustness and adaptability and can achieve precise and reliable positioning services in various complex environments.

[0003] Currently, for the multi-navigation source fusion collaborative positioning technology in a vehicle cluster environment, some scholars have proposed some positioning methods. However, existing positioning methods usually directly fuse the positioning calculation results of navigation sources. This fusion method can obtain better positioning accuracy when the positioning accuracy of navigation sources is relatively high. However, when a certain navigation positioning result undergoes a mutation, especially a mutation in a single direction, it often causes a significant increase in the data error of a single axis, thereby having an adverse impact on the accuracy of the overall fusion positioning.

[0004] Therefore, how to optimize and process the navigation source positioning information at the data level and improve the positioning accuracy and anti-interference ability of vehicle clusters has become an urgent problem to be solved currently. Summary of the Invention

[0005] The purpose of the present invention is to solve the problem that the overall fusion positioning accuracy is inaccurate due to the error of a certain navigation positioning result when directly fusing the positioning calculation results of navigation sources in the prior art, and to provide a method for fusing and collaborative positioning of vehicle cluster navigation source data based on a factor graph.

[0006] To achieve the above purpose, the technical solution provided by the present invention is as follows:

[0007] A method for fusing and collaborative positioning of vehicle cluster navigation source data based on a factor graph includes the following steps:

[0008] Step 1, construct a vehicle cluster distributed collaborative positioning system model. The model includes several vehicles, and each vehicle is configured with at least one navigation source. The navigation sources include satellite navigation sources, inertial navigation sources, and radio navigation sources. The vehicles include vehicles to be positioned and collaborative vehicles. Each vehicle is connected to other vehicles and each navigation source through a ranging and direction-finding communication link for ranging and direction-finding communication.

[0009] Obtain the vehicle positioning parameter information according to the navigation source, establish a navigation source positioning solution equation set through the positioning parameter information, solve to obtain the vehicle navigation source positioning result and the corresponding three-dimensional covariance matrix, and calculate the probability density function of each vehicle navigation source;

[0010] Step 2: Obtain the ranging and direction-finding information between vehicles, and construct an internal factor graph model of the vehicle to be positioned based on this;

[0011] According to the vehicle navigation source positioning result and the corresponding three-dimensional covariance matrix obtained in Step 1, perform confidence iteration update on the parameters of the internal factor graph model of the vehicle to be positioned, and obtain the positioning estimation result of the cooperative vehicle for the vehicle to be positioned and the estimated value of its three-dimensional covariance matrix;

[0012] Step 3: Perform data processing on the positioning estimation result of the vehicle to be positioned and the estimated value of its three-dimensional covariance matrix to obtain the fusion cooperative positioning result of all vehicles to be positioned, including:

[0013] Step 3.1: Decompose the positioning estimation result of the vehicle to be positioned onto three mutually independent orthogonal axes X, Y, and Z through orthogonal axis decomposition to obtain the positioning data set of each axis;

[0014] Step 3.2: Perform hierarchical screening and data fusion processing on the positioning data set of each axis in turn to obtain high-precision single-axis fusion data; combine the high-precision single-axis fusion data of each axis to obtain a preliminary cooperative positioning result;

[0015] Step 3.3: Perform iterative update on the parameters of the internal factor graph model of the vehicle to be positioned through the preliminary cooperative positioning result. When the preset update condition is reached, obtain the heterogeneous navigation information data fusion cooperative positioning result of all vehicles in the vehicle cluster network.

[0016] Furthermore, in the above Step 1, if the navigation source is an inertial navigation source, the process of obtaining the vehicle positioning parameter information according to the inertial navigation source, calculating the vehicle navigation source positioning result and its three-dimensional covariance matrix, and obtaining the probability density function of the inertial navigation source of all vehicles is as follows:

[0017] Step 1.1: Obtain the vehicle positioning parameter information according to the inertial navigation source and establish a vehicle coordinate system;

[0018] The vehicle positioning parameter information includes vehicle attitude, gyroscope and accelerometer parameter information, sampling frequency, and calibration period;

[0019] Define the position coordinates of the vehicle to be positioned as p = (x, y, z) T , the attitude of the vehicle to be positioned is G B = (φ, θ, ψ) T , the output of the accelerometer is fB , the static zero biases of the accelerometer and the gyroscope are b a =(b ax , b ay , b az ) T and b ω =(b ωφ , b ωθ , b ωψ ) T , the standard deviations of the accelerometer noise and the gyroscope noise are respectively and The sampling frequency is 1 / T s , and the calibration period is T;

[0020] The established vehicle coordinate system includes the northeast - sky coordinate system, the vehicle - body coordinate system, the geodetic coordinate system, and the Earth - centered Earth - fixed coordinate system. The northeast - sky coordinate system is defined as the N - system, the vehicle - body coordinate system is simply called the B - system, the geodetic coordinate system is simply called the I - system, and the Earth - centered Earth - fixed coordinate system is simply called the E - system;

[0021] Step 1.2, construct the inertial navigation source positioning update equations, and substitute the vehicle positioning parameter information obtained in Step 1.1 into the inertial navigation source positioning update equations for update calculation to obtain the positioning result p of the vehicle to be positioned t ;

[0022] The inertial navigation source positioning update equations include the attitude update differential equation, the attitude error differential equation, the velocity update differential equation, the velocity update equation, the velocity error differential equation, the position update differential equation, the position update equation, and the position error differential equation;

[0023] The attitude update differential equation is:

[0024]

[0025] Among them, is the attitude transfer matrix from the B - system to the N - system, is the differential of , is the projection of the rotation angular rate of the B - system relative to the N - system in the B - system, is The anti - symmetric matrix formed by, and

[0026]

[0027] The attitude error differential equation is:

[0028]

[0029] Among them, η is the attitude error between the output attitude of the inertial navigation source and the true attitude of the vehicle, is the differential of η, is the projection of the angular rate of rotation of the N system relative to the I system in the N system, is the error of, that is, the attitude calculation error of the N system, is the projection of the angular rate of rotation of the B system relative to the I system in the B system, that is, the output value of the gyroscope, is the error of, that is, the measurement error of the gyroscope;

[0030] The velocity update differential equation:

[0031]

[0032] where, v N is the projection of the vehicle velocity in the N system, is the differential of v N , that is, the projection of the vehicle acceleration in the N system, f B is the projection of the accelerometer measurement value in the B system, is the projection of the angular rate of rotation of the E system relative to the I system caused by the earth's rotation in the N system, is the projection of the angular rate of rotation of the N system relative to the vehicle motion in the N system, and g N are the Coriolis calibration term and the projection of the gravitational acceleration in the N system respectively;

[0033] The velocity update equation is:

[0034] where, v t+Δt is the velocity of the moving vehicle at time t + Δt, v t is the velocity of the moving vehicle at time t, is the velocity differential, that is, the acceleration, and Δt is the time interval;

[0035] The velocity error differential equation is:

[0036]

[0037] where, is the projection of the vehicle velocity error differential in the N system, f N is the projection of the accelerometer measurement value in the N system, and [f N × represents the skew-symmetric matrix formed by the accelerometer measurement values in the N system, δv N 、 and δg N ​respectively represent the specific force measurement error, velocity error, calculation error of the earth's angular rotation rate, calculation error of the navigation system rotation, and gravity error in the N system;

[0038] The position update differential equation is: where p N is the projection of the vehicle position in the N system, is the differential of p N and v N is the projection of the vehicle velocity in the N system;

[0039] The position update equation is:

[0040]

[0041] where p t+Δt is the position of the moving vehicle at time t + Δt, and p t is the position of the moving vehicle at time t, is the position differential, i.e., velocity, and Δt is the time interval;

[0042] The position error differential equation is:

[0043] where, is the projection of the vehicle position error differential under the projection in the N system, and δv N is the velocity error of the vehicle in the northeast - up coordinate system;

[0044] Step 1.3, calculate the measurement errors of the inertial navigation sources at each moment;

[0045] First, establish the mathematical models of the accelerometer and gyroscope:

[0046]

[0047] where: and respectively represent the output values of the accelerometer and gyroscope in the B system, f B and ω B respectively represent the true values of the acceleration and attitude angular velocity in the B system, S a and S ω respectively represent the scale factor errors of the accelerometer and gyroscope, N a and N ω respectively represent the non - orthogonality errors of the accelerometer and gyroscope, b a and b ω respectively represent the zero biases of the accelerometer and gyroscope, ε a and ε ω respectively represent the random noises of the accelerometer and gyroscope;

[0048] Then, according to the mathematical model and the inertial navigation source positioning and updating equation set, the measurement error of the inertial navigation source at any time is calculated, and the measurement error of the inertial navigation source is the sum of the bias error and the random noise error;

[0049] It is assumed that the vehicle carrying the inertial navigation source is in a stationary state, and the accelerometer noise and the gyroscope noise are independent of each other, and the accelerometer bias and the attitude error are independent of each other; in this state:

[0050] The bias error is Where: and are respectively the bias of the accelerometer and the bias of the gyroscope, and t is the integration duration;

[0051] The random noise error Where: and are respectively the standard deviation of the accelerometer noise and the standard deviation of the gyroscope noise, t is the integration duration, and δt is the sampling interval;

[0052] Step 1.4, correct the measurement error of the navigation source at the current moment according to the measurement error of the navigation source at the previous moment, and calculate the vehicle inertial navigation source positioning result and the corresponding three-dimensional covariance matrix according to the formula;

[0053] It is assumed that the inertial navigation source error model at the t-T s moment after the inertial navigation source calibration is The inertial navigation source error model at the t moment after the inertial navigation source calibration is

[0054]

[0055] Among them, [f N × represents the skew-symmetric matrix composed of the accelerometer measurement values in the N system, and diag(·) is the diagonal matrix construction function; is the inertial navigation source error at the t-T s moment; μ t is the inertial navigation source error at the t moment; is the covariance matrix of the inertial navigation source error at the t-T s moment; ∑ t is the covariance matrix of the inertial navigation source error at the t moment;

[0056] Definition: At the t-T s moment after the inertial navigation source calibration, the attitude G B =(φ, θ, ψ)T of the vehicle to be positioned, the accelerometer measurement value is f B , and the static biases of the accelerometer speed and the gyroscope are b a =(b​ax , b ay , b az ) T and b ω = (b ωφ , b ωθ , b ωψ ) T , the standard deviations of the accelerometer noise and the gyroscope noise are respectively and The sampling frequency is 1 / T s , the calibration period is T, and the attitude transition matrix is determined by the attitude G B of the vehicle to be positioned;

[0057] Then, at time t after the inertial navigation source is calibrated, the positioning result of the vehicle inertial navigation source corresponding three-dimensional covariance matrix

[0058] Step 1.5, since the random errors are all Gaussian white noises, it is assumed that the positioning results x of the vehicle inertial navigation source all satisfy the three-dimensional Gaussian distribution; according to the results of Step 1.2 and Step 1.3, the positioning information probability function of the navigation source is obtained as

[0059]

[0060] where ζ is a constant term, and

[0061] Step 1.6, according to the calculation process of Step 1.1 - Step 1.5, the probability functions of each vehicle inertial navigation source are obtained.

[0062] Furthermore, in Step 2, according to the positioning result of the vehicle navigation source and the corresponding three-dimensional covariance matrix obtained in Step 1, the specific process of performing confidence iteration update on the internal factor graph model parameters of the vehicle to be positioned to obtain the positioning result of the cooperative vehicle for the vehicle to be positioned and the estimated value of its three-dimensional covariance matrix is as follows: The specific process of Step 2 is as follows:

[0063] First, a three-dimensional spherical coordinate system is established with the vehicle to be positioned as the origin, and the ranging and direction-finding information between the vehicle to be positioned and the cooperative vehicle is defined using where r i is the ranging information, which satisfies a Gaussian distribution with a mean of r i and a variance of ; is the angle formed by the half-plane passing through the Z-axis and the position of the cooperative vehicle and the coordinate plane ZOX, and θ i is the angle between the line connecting the vehicle to be positioned and the cooperative vehicle and the positive direction of the Z-axis, and 0 ≤ θi ≤π;

[0064] Then, using the navigation source positioning result of the cooperative vehicle obtained in step 1 and the ranging and direction-finding information between the vehicle to be positioned and the cooperative vehicle as the input parameters for the first iterative update, the predicted positioning result of the cooperative vehicle for the vehicle to be positioned and its three-dimensional covariance matrix are calculated according to the iterative formula; in subsequent iterations, the output parameters of the previous iteration are used as the input parameters for this iteration for iterative update.

[0065] The iterative calculation formula is:

[0066]

[0067]

[0068] When the number of iterations is reached or the output result meets the accuracy threshold, the iteration terminates; the output value at the end of the iteration is used as the estimated value of the positioning result of the cooperative vehicle for the vehicle to be positioned and its three-dimensional covariance matrix.

[0069] Furthermore, the specific process of step 3.2 is as follows:

[0070] Step 3.2.1, set a set of positioning data for a certain axis which includes n data, namely Solve to obtain the mean μ x and the standard deviation σ x of the positioning data set in

[0071]

[0072] Step 3.2.2, first according to the mean μ x and the standard deviation σ x of the positioning data set, determine the initial screening condition as μ x - 2σ x ≤ x i ≤ μ x + 2σ x , i = 1, 2,..., n; then, from the set of positioning data for this axis, screen out the positioning data located within the initial screening condition.

[0073] Define that the number of positioning data for this axis before initial screening is a, and the standard deviations of b (b ≤ a) positioning data are known.

[0074] Step 3.2.3, define the number of target positioning data after re-screening as c (c ≤ a). If c ≤ b, then select the first c positioning data with the smallest error as the high-precision positioning data for this axis.

[0075] If b, then first select b positioning data with known standard deviations, and then select the c - b positioning data with unknown standard deviations closest to the mean μ x of the positioning data as the high-precision positioning data for this axis;

[0076] In step 3.2.4, first, construct a Gaussian information probability density function based on the high-precision positioning data on each orthogonal axis; define the Gaussian information probability density function of the i-th navigation data after screening as That is

[0077]

[0078] Then, determine the fusion criterion. The fusion criterion adopts a fusion strategy based on the Kullback-Leibler distance, and the fusion criterion formula is:

[0079]

[0080] In the formula, arg inf represents the maximum point of the maximum likelihood function, represents the Kullback-Leibler distance between two information probability density functions, and

[0081] Again, calculate the K-L divergence of the Gaussian information probability density function on the Riemannian manifold through information geometry theory to obtain the Fisher information distance from each point on the Riemannian manifold to each navigation source;

[0082] Finally, select the point in the Riemannian manifold with the shortest sum of Fisher information distances to each navigation source, and take the fusion positioning result and standard deviation corresponding to this point as the high-precision single-axis fusion data;

[0083] In step 3.2.5, combine the high-precision single-axis fusion data of each orthogonal axis to obtain a preliminary cooperative positioning result and its corresponding covariance matrix.

[0084] Furthermore, the specific process of step 3.3 is as follows:

[0085] Take the preliminary cooperative positioning information obtained in step 3.2.5 as the initial iterative input parameter, and iteratively update all the vehicle factor graph parameters to be positioned through the feedback iterative function; in subsequent iterations, use the output parameter information of the previous iteration as the input parameter of the next iteration; when the iteration reaches the convergence condition or the maximum iteration number k max is reached, the iteration process terminates, and use the positioning result output in the last iteration as the heterogeneous navigation information data fusion cooperative positioning result of the vehicles in the vehicle cluster network;

[0086] The feedback iterative function is

[0087]

[0088] Among them, k is the number of iterations, and are the positioning results output by the cooperative vehicle and the vehicle to be positioned in the k-th iteration respectively;

[0089] In each round of iteration, the positioning results of the vehicles in the vehicle cluster network and the corresponding covariance matrices, as well as the ranging and direction-finding information between the cooperative vehicle and the vehicle to be positioned, are synchronously updated.

[0090] The advantages of the present invention are:

[0091] The cooperative positioning method for vehicle cluster navigation source data fusion based on factor graph proposed by the present invention constructs an internal factor graph model of the vehicle to be positioned through the ranging and direction-finding information between vehicles, and constructs a confidence transfer model using the sum-product algorithm to realize the transfer and synchronous iteration of vehicle navigation information data. Through orthogonal axis decomposition, the navigation source positioning information is converted into projections on mutually perpendicular coordinate axes, so as to obtain the positioning data on each orthogonal axis. After obtaining the positioning data sets on the X, Y, and Z axes, based on the hierarchical screening strategy, large error data caused by navigation source mutation errors can be identified and eliminated, thereby effectively reducing the influence brought by navigation source mutation errors and improving the single-axis data fusion accuracy. Finally, distributed data fusion cooperative positioning is realized, which effectively improves the stability and robustness of the positioning result while ensuring the positioning accuracy, and can meet the navigation and positioning requirements of large-scale vehicle clusters in complex environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0092] The above and / or additional aspects and advantages of the present invention will become obvious and easy to understand from the description of the embodiments in conjunction with the following drawings, wherein:

[0093] Figure 1 is the flowchart of the multi-source fusion cooperative positioning method for vehicle cluster based on factor graph;

[0094] Figure 2 is the model of the vehicle cluster distributed cooperative positioning system in the embodiment;

[0095] Figure 3 is the schematic diagram of satellite navigation source positioning in the embodiment;

[0096] Figure 4 is the schematic diagram of spherical coordinate system in the embodiment;

[0097] Figure 5 is the internal factor graph model of the vehicle in the embodiment;

[0098] Figure 6 is the schematic diagram of the hierarchical screening strategy in the embodiment;

[0099] Figure 7 Schematic diagram of the experimental simulation scenario for collaborative positioning of vehicle clusters in the embodiment

[0100] Figure 8 Accuracy comparison between the method of the present invention and traditional methods in the embodiment

[0101] Figure 9 Convergence speed comparison between the method of the present invention and traditional methods in the embodiment

[0102] Figure 10 Stability comparison between the method of the present invention and traditional methods in the face of mutation errors in the embodiment Specific embodiments

[0103] The following details the embodiments of the present invention. The described embodiments are exemplary and are intended to explain the present invention, and should not be construed as a limitation of the present invention.

[0104] Refer to Figure 1 , taking the real-time precise positioning of a vehicle cluster with 40 vehicles as an example in this embodiment, the collaborative positioning method for vehicle cluster navigation source data fusion based on factor graph of the present invention is specifically described, including the following steps:

[0105] Step 1, construct a distributed collaborative positioning system model for the vehicle cluster, and obtain the navigation source positioning results of the vehicles and their three-dimensional covariance matrices, so as to obtain the positioning information probability function of the navigation source.

[0106] See Figure 2 , the constructed distributed collaborative positioning system model for the vehicle cluster includes several vehicles to be positioned. Each vehicle is equipped with multiple navigation source devices, can use multiple navigation sources for positioning, can establish communication with other vehicles within the communication distance, measure the ranging and direction-finding information with each other, and can share the navigation source positioning data through the communication link. Among them, the blue vehicles represent the vehicles to be positioned, and each positioning vehicle can use any combination of satellite navigation source, inertial navigation source and radio navigation source for positioning, that is, each vehicle is equipped with at least one navigation source; the yellow links represent that the vehicles can perform ranging communication and can share various navigation source positioning data through the communication link. The navigation sources include inertial navigation source, satellite navigation source and radio navigation source. Different navigation sources have the same process for calculating the vehicle navigation source positioning results and their three-dimensional covariance matrices according to the vehicle positioning parameters obtained from the navigation sources. The following describes them separately.

[0107] (1) Describe by taking the inertial navigation source as an example.

[0108] Step 1.1, Obtain vehicle positioning parameter information according to the inertial navigation source. The vehicle positioning parameter information includes vehicle attitude, gyroscope and accelerometer parameter information, sampling frequency, and calibration period;

[0109] Define the position coordinates of the vehicle to be positioned as p = (x, y, z) T , the attitude of the vehicle to be positioned is G B = (φ, θ, ψ) T , the output of the accelerometer is f B , the static zero biases of the accelerometer and gyroscope are b a = (b ax , b ay , b az ) T and b ω = (b ωφ , b ωθ , b ωψ ) T , the standard deviations of the accelerometer noise and gyroscope noise are respectively and The sampling frequency is 1 / T s , and the calibration period is T;

[0110] Establish coordinate system definitions, including: Northeast-East-Down coordinate system, body coordinate system, geodetic coordinate system, and Earth-Centered Earth-Fixed coordinate system. Define the Northeast-East-Down coordinate system as the N system, the body coordinate system is simply referred to as the B system, the geodetic coordinate system is simply referred to as the I system, and the Earth-Centered Earth-Fixed coordinate system is simply referred to as the E system.

[0111] Step 1.2, Construct the inertial navigation source positioning solution equations and calculate the vehicle inertial navigation source positioning result according to the vehicle positioning parameter information where p t is the inertial navigation source calculation result, and μ t is the inertial navigation source error The process of inertial navigation source positioning solution is divided into three stages, namely: attitude update, velocity update, and position update.

[0112] Attitude update differential equation:

[0113]

[0114] where, is the attitude transfer matrix from the B system to the N system, is the differential of , is the projection of the rotation angular rate of the B system relative to the N system in the B system, is constituted skew-symmetric matrix, and

[0115] ​

[0116] The attitude error differential equation is as follows:

[0117]

[0118] Where η is the attitude error between the output attitude of the inertial navigation source and the true attitude of the vehicle, is the differential of η, is the projection of the rotation angular rate of the N frame relative to the I frame in the N frame, is the error of, that is, the attitude calculation error of the N frame, is the projection of the rotation angular rate of the B frame relative to the I frame in the B frame, that is, the output value of the gyroscope, is the error of, that is, the measurement error of the gyroscope;

[0119] Velocity update differential equation:

[0120]

[0121] Where v N is the projection of the vehicle velocity in the N frame, is the differential of v N , that is, the projection of the vehicle acceleration in the N frame, f B is the projection of the accelerometer measurement value in the B frame, is the projection of the rotation angular rate of the E frame relative to the I frame caused by the earth's rotation in the N frame, is the projection of the rotation angular rate of the N frame relative to the caused by the vehicle movement in the N frame, and g N are the Coriolis calibration term and the projection of the gravitational acceleration in the N frame respectively;

[0122] The velocity update equation is as follows:

[0123] Where v t+Δt is the velocity of the moving vehicle at time t + Δt, v t is the velocity of the moving vehicle at time t, is the velocity differential, that is, the acceleration, and Δt is the time interval;

[0124] The velocity error differential equation is as follows:

[0125]

[0126] Where, is the projection of the vehicle velocity error differential in the N frame, f N is the projection of the accelerometer measurement value in the N frame, and [fN × represents the skew-symmetric matrix composed of the accelerometer measurement values in the N system, δv N 、 and δg N respectively represent the specific force measurement error, velocity error, calculation error of the earth's angular rotation rate, calculation error of the navigation system rotation, and gravity error in the N system;

[0127] The differential equation for position update is: where p N is the projection of the vehicle position in the N system, is the differential of p N and v N is the projection of the vehicle velocity in the N system;

[0128] The position update equation is:

[0129]

[0130] where p t+Δt is the position of the moving vehicle at time t + Δt, p t is the position of the moving vehicle at time t, is the position differential, i.e., velocity, and Δt is the time interval;

[0131] The differential equation for position error is:

[0132] where, is the projection of the vehicle position error differential under the projection in the N system, and δv N is the velocity error of the vehicle in the northeast-down coordinate system;

[0133] Step 1.3, establish the mathematical models of the accelerometer and gyroscope, and update and calculate the measurement errors of the inertial navigation source.

[0134] The errors of the inertial navigation source are mainly caused by the measurement errors of inertial devices, the errors of the earth's geographical parameters, etc. Considering that for the vehicle-mounted inertial navigation source, the errors caused by the changes of the earth's geographical parameters such as the earth's gravity field and the earth's angular rotation rate have a relatively small impact on the navigation accuracy compared with the navigation errors caused by the inertial device errors in a short time and do not accumulate over time, calculate the errors brought by the changes of the earth's geographical parameters when calculating the errors of the inertial navigation source.

[0135] ​The measurement errors of inertial devices can be divided into two main parts: deterministic errors and random errors. Since deterministic errors can be compensated by laboratory calibration, the random error part is analyzed. The random error part mainly includes bias and random noise. The error model can be composed of two parts: "random constant + white noise", representing the static part and the fast-varying part in random errors respectively.

[0136] First, construct the output mathematical models of the accelerometer and gyroscope. The output mathematical models are

[0137]

[0138] where: and represent the output values of the accelerometer and gyroscope in the B frame respectively. f B and ω B represent the true values of acceleration and attitude angular velocity in the B frame respectively. S a and S ω represent the scale factor errors of the accelerometer and gyroscope respectively. N a and N ω represent the non-orthogonal errors of the accelerometer and gyroscope respectively. b a and b ω represent the biases of the accelerometer and gyroscope respectively. ε a and ε ω represent the random noises of the accelerometer and gyroscope respectively;

[0139] Assume that the vehicle carrying the inertial navigation source is in a stationary state, and the noises of the accelerometer and gyroscope are independent of each other, and the accelerometer bias and attitude error are independent of each other. In this state, calculate the constant error. The calculation process is as follows:

[0140] When there is a bias in the accelerometer, during the update and solution process of the inertial navigation source positioning solution equations, the accelerometer will introduce an error proportional to time t in the velocity, and will cause an error proportional to t 2 in the position. The velocity error is The position error is

[0141] When there is a bias in the gyroscope, it will introduce an angular error proportional to time t. After the update and solution of the inertial navigation source positioning solution equations, it will also cause velocity and position errors. The velocity error is The position error is

[0142] In summary, the position error caused by the constant errors of the gyroscope and accelerometer can be expressed as

[0143]

[0144] Among them, and are the zero biases of the accelerometer and gyroscope respectively, and t is the integration duration.

[0145] Calculate the noise error. The specific calculation process is as follows:

[0146] The epochs of white noise are uncorrelated. Therefore, the mean of each random variable (epoch) is 0, and the variance is all σ 2 . Let N i be the i-th random variable in the white noise sequence. Then the mean and variance satisfy:

[0147] E(N i ) = 0

[0148] Var(N i ) = σ 2

[0149] Due to the uncorrelation of adjacent epochs of white noise, there is:

[0150]

[0151] The result of integrating the white noise signal at time t = n·δt is:

[0152]

[0153] Among them, n is the data sample size, and δt is the interval time between consecutive samples, that is, the sampling time. Based on the following formula:

[0154] E(aX + bY) = aE(X) + bE(Y)

[0155] Var(aX + bY) = a 2 Var(X) + b 2 Var(Y) + 2abCov(X, Y)

[0156] The mean and standard deviation of the parameters (angle information) obtained by integrating white noise are as follows:

[0157]

[0158] The velocity random walk error is the velocity measurement error caused by integrating the accelerometer white noise, and its standard deviation is:

[0159]

[0160] Among them, is the standard deviation of the accelerometer noise, t is the integration duration, and δt is the sampling interval.

[0161] To derive the error caused by the accelerometer white noise in position measurement, the noise needs to be double-integrated:

[0162]

[0163] Therefore, the white noise of the accelerometer will cause a second-order random walk error in position measurement. The mean of the error is 0, and the standard deviation is:

[0164]

[0165] The attitude random walk error is the attitude measurement error caused by the integration of the gyroscope white noise, and its standard deviation is

[0166]

[0167] where is the standard deviation of the gyroscope noise, t is the integration duration, and δt is the sampling interval.

[0168] To derive the error caused by the gyroscope white noise in horizontal position measurement, the noise needs to be triple-integrated:

[0169]

[0170] Therefore, the white noise of the gyroscope will cause a second-order random walk error in horizontal velocity measurement and a third-order random walk error in horizontal position measurement. The mean of the error is 0, and the standard deviation is:

[0171]

[0172]

[0173] Therefore, the standard deviation of the position error caused by the white noise of the gyroscope and accelerometer can be approximately expressed as:

[0174]

[0175] where and are the standard deviation of the accelerometer noise and the standard deviation of the gyroscope noise respectively, t is the integration duration, and δt is the sampling interval.

[0176] Step 1.4: Obtain the positioning result of the inertial navigation source and the corresponding three-dimensional covariance matrix;

[0177] Set at the t-T s moment after the calibration of the inertial navigation source, the attitude G of the vehicle to be positioned B=(φ, θ, ψ) T , the measured value of the accelerometer is f B , the static zero biases of the accelerometer and gyroscope are b a =(b ax , b ay , b az ) T and b ω =(b ωφ , b ωθ , b ωψ ) T , the standard deviations of the accelerometer noise and gyroscope noise are respectively and The sampling frequency is 1 / T s , the calibration period is T, and the attitude transition matrix is determined by the attitude G B of the vehicle to be located. Then, the error model of the inertial navigation source at time t after inertial navigation calibration is where

[0178]

[0179] where, [f N × represents the skew-symmetric matrix formed by the measured values of the accelerometer in the N system, and diag(·) is the diagonal matrix construction function; is the error of the inertial navigation source at time t - T s ; μ t is the error of the inertial navigation source at time t; is the covariance matrix of the error of the inertial navigation source at time t - T s ; Σ t is the covariance matrix of the error of the inertial navigation source at time t.

[0180] Assume that at time t after inertial navigation calibration, the positioning result of the vehicle to be located is p t =(x, y, z) T , then the positioning result of the inertial navigation source is The covariance matrix of the positioning result is

[0181] Step 1.5, since the random errors are all Gaussian white noises, it is determined that the positioning results x of the vehicle inertial navigation source all satisfy the three-dimensional Gaussian distribution; according to the results of Step 1.2 and Step 1.3, the positioning information probability function of the navigation source is obtained as

[0182]

[0183] where, ζ is a constant term and ​

[0184] Step 1.6, according to the calculation process of Steps 1.1 - 1.5, obtain the positioning information probability function of each vehicle inertial navigation source.

[0185] (2) Taking the radio navigation source as an example:

[0186] Refer to Figure 3 , which is the positioning schematic diagram of the radio navigation source.

[0187] Step 1.1, obtain the positioning parameter information according to the radio base stations, where the positioning parameter information includes the number of radio base stations, the positions of each radio base station, and the ranging information between the vehicle and the radio base stations.

[0188] Set the coordinates of the vehicle to be positioned as x = (x, y, z) T , the number of radio base stations is n (n≥4), and the position of the i-th radio base station is V i = {(X i , Y i , Z i ) T |i = 1, 2, 3,..., n}, and the ranging information between the vehicle to be positioned and the radio base stations is d = (d1, d2,..., d n ) T .

[0189] Step 1.2, according to the positioning parameter information of the vehicle, construct a radio navigation source positioning solution equation set, and solve to obtain the navigation source positioning result of the vehicle based on the radio navigation source.

[0190] According to the Euclidean distance calculation formula, the radio navigation source positioning solution equation set is

[0191]

[0192] To reduce the influence of the inherent error of the vehicle to be positioned itself, subtract the adjacent two equations to obtain:

[0193]

[0194] To further simplify, define:

[0195]

[0196] Obtain:

[0197] Ax = b

[0198] Solve the above equation using the least squares method, and its solution is:

[0199] x = (A T A) -1 A T b

[0200] For further simplification, define:

[0201] P = (A T A) -1 A T

[0202] The navigation source positioning result of the vehicle based on the radio navigation source can be obtained:

[0203] x = Pd

[0204] Among them, the matrix P is a coefficient matrix that only depends on the geometric distribution of the base stations.

[0205] Step 1.3, calculate the three-dimensional covariance of the navigation source positioning result x of the vehicle's radio navigation source.

[0206] In practical applications, the geometric distribution of the wireless base stations can be considered fixed, that is, the matrix P is determined. At the same time, the ranging information between the vehicle and each wireless base station is independent of each other and satisfies a Gaussian distribution with a mean of and a standard deviation of , that is

[0207] Using the covariance property, the covariance ∑ of the positioning result x is obtained x :

[0208] ∑ x = P∑ b P T

[0209] Among them, diag(·) is the diagonal matrix construction function, is the square of the ranging information variance, and:

[0210] Step 1.4, since the random errors are all Gaussian white noise, it is determined that the navigation source positioning result x of the vehicle's radio navigation source all satisfies a three-dimensional Gaussian distribution; according to the results of Step 1.2 and Step 1.3, calculate the positioning information probability function of the radio navigation source.

[0211] Through the above calculation and derivation process, the navigation source positioning result of the vehicle and its three-dimensional covariance matrix information probability model are finally obtained Among them, μ RNS is the positioning result of the RNS, and ∑ RNS is the covariance of the positioning result.

[0212] Since the random errors are all Gaussian white noise, it is determined that the positioning results of the vehicle radio navigation sources all satisfy the three-dimensional Gaussian distribution; according to the results of steps 1.2 and 1.3, calculate the probability function of the positioning information of the radio navigation source, which is

[0213]

[0214] , where ζ is a constant term and

[0215] Step 1.5, according to the calculation process of steps 1.1 - 1.4, obtain the probability function of the positioning information of the radio navigation sources of all vehicles.

[0216] (2) Taking the satellite navigation source as an example, describe the process of obtaining the positioning result of the vehicle and its three-dimensional covariance matrix. The schematic diagram of the satellite navigation source positioning is as Figure 3 shown.

[0217] Step 1.1, obtain the vehicle positioning parameter information according to the satellite navigation source. The vehicle positioning parameter information includes the number and positions of the navigation satellites participating in the positioning, and the pseudo-range information between the vehicle to be positioned and the navigation satellites.

[0218] Set the position coordinates of the vehicle to be positioned as p = (x, y, z) T , the number of satellites participating in the positioning is N (N≥4), and the position of the i-th positioning satellite is S i ={(X i , Y i , Z i ) T |i = 1, 2, 3,..., N}, the pseudo-range information between the vehicle to be positioned and the positioning satellites is ρ = (ρ1, ρ2,..., ρ N ) T , c is the speed of light, and t u is the advance of the vehicle receiver clock compared to the satellite clock.

[0219] Step 1.2, combining the vehicle positioning parameter information obtained in step 1.1, establish a satellite navigation source solution equation set according to the Euclidean distance calculation formula,

[0220] The established navigation source solution equation set is:

[0221]

[0222] Step 1.3, solve the navigation source solution equation set:

[0223] First, set the approximate position of the vehicle to be positioned as Subtract the approximate position from the true position (x, y, z) The offset between them is represented as (Δx, Δy, Δz); the predicted value of the receiver clock difference offset uses Δt u to represent. Then we get:

[0224]

[0225] Expand the navigation source solution equations at the approximate position by Taylor series, and the offset (Δx, Δy, Δz) can be expressed as a linear function of the known coordinates and pseudorange measurement values. Specifically as follows:

[0226] First, represent a single pseudorange value ρ i as:

[0227]

[0228] Using the approximate position and the time deviation estimate value as we can get:

[0229]

[0230] Perform Taylor series expansion on f(x, y, z, t u ) at and only retain the terms of the first-order partial derivatives, we can get:

[0231]

[0232] Among them,

[0233]

[0234] For convenience of description, define:

[0235]

[0236] Among them, Δρ i represents the difference between the approximate pseudorange value and the pseudorange value, a xi , a yi and σ zi respectively represent the direction cosines of the unit vector pointing from the approximate position to the i-th satellite. For the i-th satellite, the unit vector is defined as a i =(a xi , a yi , a zi ).

[0237] Combining the above equations, we can get:

[0238] Δρ i =a xi Δx + a yi Δy + azi Δz - cΔt u

[0239] For the unknowns Δx, Δy, Δz, and Δt u , they can be solved by the following system of linear equations:

[0240]

[0241] Define:

[0242]

[0243] We can get: Δρ = HΔx.

[0244] Using the least squares method to solve, the solution is: Δx = (HTH) -1 H T Δρ, and calculate the unknowns Δx, Δy, Δz, and Δt respectively u , according to Δx, Δy, Δz, and Δt u calculate the coordinates (x, y, z) of the vehicle to be located and the advance amount t of the vehicle receiver clock relative to the satellite clock u . By continuous iteration, the satellite navigation source positioning result that meets the accuracy requirements is finally obtained as μ = (x, y, z) T .

[0245] Then, calculate the corresponding three - dimensional covariance matrix:

[0246] By analyzing the parameter composition of matrix H, it is found that matrix H is a coefficient matrix that only depends on the geometric distribution of the vehicle or satellite navigation source. In practical applications, it can be considered that the vehicle / satellite geometric distribution is fixed in a short time, so matrix H is regarded as a known parameter. Therefore, using the covariance property, we can get:

[0247] ∑ Δx =(H T H) -1 H T ∑ Δρ ((H T H) -1 H T ) T =(H T H) -1 H T ∑ Δρ H(H T H) -1

[0248] Among them, ∑ represents the covariance matrix.

[0249] Step 1.4: Since the random errors are all Gaussian white noises, it is determined that the vehicle satellite navigation source positioning results x all satisfy the three-dimensional Gaussian distribution; according to the results of Step 1.2 and Step 1.3, the positioning information probability function of the navigation source is obtained as

[0250]

[0251] where ζ is a constant term and

[0252] Step 1.5: According to the calculation process of Step 1.1 - Step 1.4, the positioning information probability functions of each vehicle satellite navigation source are obtained.

[0253] (3) Taking the radio navigation source as an example:

[0254] Step 1.1: Obtain the positioning parameter information of the vehicle;

[0255] The positioning parameter information includes the number of radio base stations, the positions of each radio base station, and the ranging information between the vehicle and the radio base stations;

[0256] Definition: The vehicle coordinates are x = (x, y, z) T , the number of radio base stations is n (n≥4), and the position of the i-th radio base station is V i ={(X i , Y i , Z i ) T |i = 1, 2, 3,..., n}, and the ranging information between the vehicle and the radio base stations is d = (d1, d2,..., d n ) T ;

[0257] Step 1.2: According to the positioning parameter information of the vehicle, construct a radio navigation source positioning solution equation set as

[0258]

[0259] and solve to obtain the navigation source positioning result x = Pb of the vehicle based on the radio navigation source,

[0260] where P = (A T A) -1 A T ,

[0261]

[0262] Step 1.3: Define that the ranging information between the vehicle and each radio base station is independent of each other and satisfies a Gaussian distribution with a mean of and a standard deviation of , that is

[0263] Calculate the three-dimensional covariance ∑ of the positioning result x of the vehicle radio navigation source x = P∑ b P T ,

[0264] where diag(·) is the diagonal matrix construction function, is the square of the ranging information the variance of, and

[0265] Step 1.4, since the random errors are all Gaussian white noise, it is determined that the positioning result x of the vehicle radio navigation source satisfies a three-dimensional Gaussian distribution; according to the results of Step 1.2 and Step 1.3, the positioning information probability function of the navigation source is obtained as

[0266]

[0267] where ζ is a constant term and

[0268] Step 1.5, according to the calculation process of Step 1.1 - Step 1.4, the positioning information probability functions of each vehicle radio navigation source are obtained

[0269] In summary, we express the positioning information probability functions of the satellite navigation source, radio navigation source, and inertial navigation source as:

[0270]

[0271] where indicates that the positioning result of each type of navigation source is μ = (x, y, z) T , and the covariance matrix of the positioning result is Σ

[0272] is the information probability function of the positioning result of the vehicle's satellite navigation source and its three-dimensional covariance matrix, where μ GNSS is the positioning result of the satellite navigation source, and ψ is the covariance of the positioning result of the satellite navigation source

[0273] is the information probability function of the positioning result of the vehicle's radio navigation source and its three-dimensional covariance matrix, where μ RNS is the positioning result of the radio navigation source, and ∑ RNS is the covariance of the positioning result of the radio navigation source

[0274] is the information probability function of the positioning result of the vehicle's inertial navigation source and its three-dimensional covariance matrix, where is the positioning result of the inertial navigation source is the covariance ∑ of the inertial navigation source positioning result x .

[0275] Since the random errors are all Gaussian white noises, it can be approximately considered that the positioning results x of various navigation sources all satisfy the three-dimensional Gaussian distribution, and its mean value is μ = (x, y, z) T , and the covariance matrix is ∑. Then the information probability function of the navigation source can be expressed as:

[0276]

[0277] where ζ is a constant term and

[0278] Step 2: Obtain the ranging and direction-finding information between vehicles, and construct a factor graph model of the vehicle to be located accordingly. Combine the navigation source positioning results of the vehicles in Step 1 and the corresponding three-dimensional covariance matrix, update the confidence of the internal factor graph model of the vehicle, and calculate the predicted positioning result of the collaborative vehicle for the vehicle to be located and the corresponding three-dimensional covariance matrix.

[0279] The core part of the vehicle cluster distributed collaborative positioning model is to obtain the internal factor graph model of the vehicle to be located through the ranging and direction-finding information between vehicles, and then use the sum-product algorithm to construct a confidence transfer model to realize the effective transfer and fusion of the positioning information between vehicles. Since the confidence update method for each vehicle to be located in this method is the same, therefore, the following takes any vehicle M to be located in the vehicle cluster network q as an example, and refers to Figure 5 the process of updating the confidence of the parameters of the internal factor graph model of the vehicle to be located is described in detail. The confidence information in the update process is the covariance matrix and covariance corresponding to the vehicle navigation source result.

[0280] First, assume that there are N vehicles in the vehicle cluster collaborative positioning network. For any vehicle to be located, there are respectively a i independent positioning sources that can locate it. Then the positioning results of each navigation source of the vehicle to be located are expressed as M ik , where k = 1, 2,..., a i , i = 1, 2, 3,..., N. At the same time, for any vehicle to be located, there are also n i cooperative vehicles, and there are respectively a j independent positioning sources that can locate it. Then the positioning results of each navigation source of the cooperative vehicles are expressed as C ijk , where k = 1, 2,..., a j , j = 1, 2,..., n iTaking the vehicle to be located as the origin, a three-dimensional spherical coordinate system is established, and the ranging and direction-finding information between the vehicle to be located and the cooperative vehicle is represented by where r i is the ranging information, which satisfies a Gaussian distribution with a mean of r i and a variance of . is the angle formed by the half-plane passing through the Z-axis and point C ij and the coordinate plane ZOX, and θ i is the angle between the line segment M i C ij and the positive direction of the Z-axis, and 0 ≤ θ i ≤ π. Figure 4 Fig. shows the schematic diagram of the position relationship between the vehicle to be located and the cooperative vehicle. In the figure, M i is the vehicle to be located, and C ij is the cooperative vehicle.

[0281] Then, taking the navigation source positioning result of the cooperative vehicle obtained in step 1 and the ranging and direction-finding information between the vehicle to be located and the cooperative vehicle as the input parameters for the first iterative update, the predicted positioning result of the cooperative vehicle for the vehicle to be located and its three-dimensional covariance matrix are calculated according to the iterative formula; in subsequent iterations, the output parameters of the previous iteration are used as the input parameters for this iteration for iterative update. The iterative calculation formula is:

[0282]

[0283] In the above formula, is the navigation source positioning result of the vehicle to be located output in the k-th iteration, the navigation source positioning results of each navigation source of the cooperative vehicle output in the (k - 1)-th iteration; is the ranging and direction-finding information of the cooperative vehicle relative to the vehicle to be located, where is the ranging information, is the variance.

[0284] When the maximum number of iterations is reached or the result converges to meet the required accuracy threshold, the iteration terminates. The accuracy threshold is determined according to the positioning accuracy required by the scenario. Generally, the accuracy threshold is taken as 0.02 m. The output value at the end of the iteration is used as the position estimation information of the cooperative vehicle for the vehicle to be located, denoted as

[0285] Step 3: Through orthogonal axis decomposition and hierarchical screening, a high-precision single-axis data set is obtained, and the single-axis fusion positioning results are fused and combined into a multi-navigation source fusion cooperative positioning result to obtain a heterogeneous navigation information data fusion positioning result, realizing high-precision positioning of the vehicle cluster. Specifically as follows:

[0286] Step 3.1: Perform orthogonal axis decomposition on the predicted positioning result obtained in Step 2. Through orthogonal axis decomposition, the navigation source positioning information can be converted into projections on mutually perpendicular coordinate axes, thereby obtaining the positioning data on each orthogonal axis.

[0287] The purpose of orthogonal axis decomposition is to decompose the position estimation result of the vehicle to be positioned onto three mutually independent orthogonal axes: X, Y, and Z.

[0288] For different types of navigation sources, their positioning calculation methods are generally different, and the finally obtained navigation source positioning information can be represented by (μ, ∑), where μ is the positioning result and ∑ is a measure of the positioning accuracy, which generally exists in the form of a covariance matrix. The specific form is as follows:

[0289] μ = (μ x , μ y , μ z ) T

[0290] where the values of i and j both range from 1 to 3 and respectively correspond to x, y, and z. Taking c 11 as an example, the specific calculation method is:

[0291]

[0292] Therefore, the navigation source positioning information (μ, ∑) can be decomposed onto the three mutually independent orthogonal axes X, Y, and Z to obtain three sets of navigation data; the orthogonal decomposition function D i is:

[0293]

[0294] After being calculated by the orthogonal axis decomposition function D i , the position estimation result of the vehicle to be positioned is decomposed onto the three mutually independent orthogonal axes X, Y, and Z and respectively enters the corresponding data sets, denoted as and

[0295] Step 3.2: After obtaining the positioning data sets of the X, Y, and Z axes, through a hierarchical screening strategy based on a statistical model, identify and eliminate the navigation source positioning data with large errors to obtain a high-precision single-axis data set.

[0296] For a single positioning vehicle, the position estimation information it can obtain not only includes its own navigation source positioning information, but also covers the navigation source positioning information of surrounding cooperative vehicles, resulting in an extremely large amount of data to be fused. To improve the fusion speed and reduce the impact of large-error data on the fusion positioning accuracy, the factor graph model introduces a hierarchical screening strategy based on a statistical model.

[0297] Next, in this embodiment, taking the X-axis positioning data set as an example, the hierarchical screening strategy will be introduced:

[0298] (a) Pretreatment

[0299] Assume that the X-axis positioning data set contains n data, denoted as

[0300] Solve for the mean μ x and the standard deviation σ x of the positioning data in it, which are respectively:

[0301]

[0302] (b) Primary screening

[0303] The purpose of primary screening is to reduce the impact of positioning results with large errors on the overall fusion positioning. Therefore, in this embodiment, the mean μ x and the standard deviation σ x of the positioning data are used to perform primary screening on the X-axis positioning information.

[0304] The primary screening rule is as follows:

[0305] μ x - 2σ x ≤ x i ≤ μ x + 2σ x , i = 1, 2, …, n

[0306] Assume that there are a total of a positioning data that meet the primary screening conditions, which can be denoted as Among them, the standard deviations of b (b ≤ a) positioning data are known, denoted as σ i , 1 ≤ i ≤ b. Assume that the navigation source positioning information is independent and identically distributed, and all satisfy Then the sample mean and standard deviation satisfy Therefore, for the remaining a - b positioning information with unknown standard deviations, it can be considered that their standard deviations

[0307]

[0308] (c) Re-screening

[0309] To further reduce the complexity of fusion and improve the positioning accuracy, the optimal values of the existing positioning data can be selected for fusion.

[0310] Define the number of target positioning data after re-screening as c (c ≤ a). If c ≤ b, then directly take the first c values with the smallest error to obtain the high-precision positioning data on the X-axis. If b < c ≤ a, first select b positioning data with known errors, and then select c - b distances with the mean μ x The nearest data as the data required for re-screening. Through re-screening, a high-precision positioning data set on the X-axis can be obtained.

[0311] Next, this embodiment will illustrate the hierarchical screening strategy of this subsection through a legend. Assume that there are 10 X-axis positioning data before screening, among which the standard deviations of 4 positioning data are known and marked with red circles, and the remaining 6 positioning data with unknown errors are marked with blue circles. Finally, a high-precision X-axis positioning data set of 6 is required. The schematic diagram of the hierarchical screening strategy is as Figure 6 shown.

[0312] First, obtain the mean μ x and the standard deviation σ x from these 10 data, and obtain the initial screening interval (μ x - 2σ x , μ x +2σ x ). After initial screening, 2 positioning data that do not meet the requirements can be removed to avoid the influence of large error data on the overall fusion positioning accuracy. To further reduce the complexity of the fusion positioning algorithm, 4 positioning data with known errors and two positioning data closest to the mean μ x are selected to complete the re-screening. Through hierarchical screening, a high-precision positioning data set on the X-axis can finally be obtained. According to the above method, high-precision positioning data sets on the Y-axis and Z-axis are obtained respectively to prepare for the next navigation information data fusion.

[0313] Step 3.3, perform data fusion processing on the data of each axis processed in Step 3.2.

[0314] Next, taking the X-axis as an example, the data fusion node B will be introduced.

[0315] First, construct a Gaussian probability density function for the high-precision data on each orthogonal axis, calculate the single-axis fusion positioning result and its variance through information geometry theory and fusion criteria, and combine to obtain the multi-navigation source fusion collaborative positioning result.

[0316] Assume that there are c remaining navigation data after screening. Among them, the information probability density function of the i-th navigation data is

[0317] p i The functional expression of (·) is:

[0318]

[0319] Select the fusion strategy based on the Kullback-Leibler distance, and the fusion criterion is as follows:

[0320]

[0321] Among them, arg inf represents the maximum point of the maximum likelihood function, represents the Kullback-Leibler distance between the two information probability density functions, and

[0322]

[0323] For the normal distribution, it can be obtained that:

[0324]

[0325] Solving the above formula, it can be obtained that:

[0326]

[0327]

[0328] Among them, x f is the fused positioning result, is the standard deviation of the fused positioning result.

[0329] Approximate the Fisher information distance by calculating the K-L divergence of the probability density function of the positioning result on the Riemannian manifold through information geometry theory. When the sum of the Fisher information distances from a point in the Riemannian manifold to each navigation result is the shortest, the mean and variance of the probability density function corresponding to this point are the uniaxial fused positioning result and the confidence information.

[0330] And so on, the fused positioning information of the Y-axis and Z-axis can also be obtained and Finally, the data fusion collaborative positioning result after the k-th iteration is obtained after combination and the covariance matrix

[0331] Then, after obtaining the preliminary collaborative positioning results, use these results to update the parameters in the factor graph. When the preset maximum number of convergence times is reached or the convergence condition is satisfied, the fused collaborative positioning results of all vehicles in the vehicle cluster network are obtained. Specifically as follows:

[0332] Take the preliminary cooperative positioning information obtained in step 3.2 as the initial iterative input parameter, and iteratively update the factor graph parameters of all vehicles to be positioned through the feedback iterative function; in subsequent iterations, use the output parameter information of the previous iteration as the input parameter of the next iteration; process the positioning result through the feedback iterative function to obtain the initial values of the ranging and direction-finding information for the (k + 1)-th iteration

[0333] The feedback iterative function is as follows:

[0334]

[0335] where

[0336]

[0337] Thus, the k-th iteration of the internal factor graph model of the vehicle is completed; k is the number of iterations, and are the positioning results output for the k-th iteration of the cooperative vehicle and the vehicle to be positioned respectively;

[0338] In each round of iteration, the fused navigation positioning result, positioning covariance matrix, and ranging and direction-finding information in the vehicle cluster network are synchronously updated to ensure the temporal consistency of the positioning data. When the convergence condition or the maximum number of iterations k max is reached, the iterative process terminates. At this time, the positioning result output by the last iteration is used as the heterogeneous navigation information data fusion cooperative positioning result of the vehicles in the vehicle cluster network.

[0339] Use the method of the present invention to conduct a simulation experiment on the vehicle cluster in the embodiment, and compare it with the current mainstream positioning methods including recursive least squares estimation (RLS), unscented Kalman filter (UKF), and BP neural network (BPNN). Calculate the positioning accuracy using the root mean square error (RMSE).

[0340] Set the simulation scenario as two parallel roads with obstacles in the middle. There are 40 vehicles driving on the lanes in this simulation scenario, and their initial positions are randomly distributed in the simulation scenario. The schematic diagram of the vehicle cluster cooperative positioning experiment simulation scenario is as Figure 7 shown. Set the vehicle movement speed to be between 15 and 20 m / s, move from the left end to the right end of the occlusion scenario, the total movement duration is 30 s, the maximum communication distance between vehicles is 100 m, the standard deviation of the ranging error is σ r = 0.1 m, the standard deviation of the pseudorange error σ1 is 1.0 m, the standard deviation of the radio ranging error σ2 is 0.2 m, the minimum interval time of the positioning output is 0.5 s, and the number of Monte Carlo runs is 10,000.

[0341] The average positioning accuracy of experimental vehicles A and B is selected as the output quantity, and the simulation results of the accuracy comparison experiment are as Figure 8 shown. From Figure 8 it can be seen that under the same simulation conditions, the method of the present invention shows significant advantages in positioning accuracy, with the lowest average positioning error. When there are obstacles blocking, it can be found that the positioning accuracy of the method of the present invention basically remains stable. This is because these two methods construct a factor graph model for distributed cooperative positioning, enabling navigation information data to be transmitted in the factor graph. This mechanism significantly improves the reusability of the data, thereby effectively suppressing the problem of positioning accuracy loss caused by occlusion. In addition, the jitter amplitude of the method of the present invention is the smallest, indicating that the stability of the method of the present invention is the best among these four positioning methods.

[0342] Analyze the real-time performance of these four positioning methods through a single cooperative positioning process, select the average positioning error of 40 vehicles as the output quantity, and the simulation results of the convergence speed comparison experiment are as Figure 9 shown. From Figure 9 it can be seen that under this simulation condition, the convergence speed of the method of the present invention is second only to the RLS method, and its convergence time is approximately 0.13 s. This result shows that the method of the present invention simplifies the originally complex three-dimensional fusion problem into three one-dimensional fusion problems by performing orthogonal axis decomposition on the positioning information of the navigation source, significantly reducing the computational complexity of the fusion method. In addition, the screening strategy adopted in the method of the present invention also plays an important role. This strategy can not only eliminate data with large errors, thereby improving the positioning accuracy, but also effectively reduce the amount of data to be processed for fusion. These two aspects of optimization jointly ensure the real-time performance of the method of the present invention, providing a solid guarantee for its application in the actual vehicle cluster positioning scenario.

[0343] The simulation test of the positioning stability of the vehicle cluster in the embodiment when facing mutation errors by using the method of the present invention is as Figure 10 shown. It is set that at time t = 3 - 7 s, the standard deviation σ1 of the pseudorange error changes from 1.0 m to 1.5 m, at time t = 13 - 17 s, the standard deviation σ2 of the radio ranging error changes from 0.2 m to 0.3 m, and at time t = 23 - 27 s, the standard deviation σ r of the ranging error between vehicles changes from 0.1 m to 0.2 m, and other parameter settings remain the same.

[0344] By Figure 10It can be seen that when there are sudden errors during the positioning process, the average positioning error increment of the method of the present invention shows obvious superiority compared with the other three methods. This result indicates that the method of the present invention decomposes the positioning information of the navigation source along the orthogonal axes into the uniaxial data of the navigation source, and then conducts hierarchical screening on each uniaxial data. This processing method can eliminate the large error data caused by the sudden error of the navigation source, thereby effectively reducing the influence brought by the sudden error of the navigation source. In addition, the method of the present invention also constructs a distributed factor graph model, and constrains the positioning result through the ranging and direction-finding information between vehicles, further reducing the influence of sudden errors and enhancing the robustness of the method. In summary, the method of the present invention shows significant advantages in improving the anti-interference ability of positioning and reducing the influence of sudden errors, providing a reliable solution for the actual vehicle cluster positioning problem.

[0345] The above is only the specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any person skilled in the art within the technical scope disclosed by the present invention can easily think of various equivalent modifications or substitutions, and these modifications or substitutions should be covered within the protection scope of the present invention.

Claims

1. A collaborative positioning method for vehicle cluster navigation source data fusion based on factor graph, characterized in that Including the following steps: Step 1: Construct a distributed cooperative positioning system model for a vehicle cluster. The model includes several vehicles, and each vehicle is equipped with at least one navigation source. The navigation sources include satellite navigation sources, inertial navigation sources, and radio navigation sources. The vehicles include vehicles to be positioned and cooperative vehicles. Each vehicle is connected to other vehicles and each navigation source through a ranging and direction-finding communication link for ranging and direction-finding communication; Obtain vehicle positioning parameter information according to the navigation source, establish a navigation source positioning solution equation set through the positioning parameter information, solve to obtain the vehicle navigation source positioning result and the corresponding three-dimensional covariance matrix, and calculate the probability density function of each vehicle navigation source; Step 2: Obtain the ranging and direction-finding information between vehicles and construct an internal factor graph model for the vehicle to be positioned accordingly; According to the vehicle navigation source positioning result and the corresponding three-dimensional covariance matrix obtained in Step 1, perform confidence iteration update on the parameters of the internal factor graph model of the vehicle to be positioned to obtain the positioning estimation result of the cooperative vehicle for the vehicle to be positioned and the estimated value of its three-dimensional covariance matrix; Step 3: Perform data processing on the positioning estimation result of the vehicle to be positioned and the estimated value of its three-dimensional covariance matrix to obtain the fusion cooperative positioning result of all vehicles to be positioned, including: Step 3.1: Decompose the positioning estimation result of the vehicle to be positioned onto three mutually independent orthogonal axes of X, Y, and Z through orthogonal axis decomposition to obtain the positioning data set of each axis; Step 3.2: Perform hierarchical screening and data fusion processing on the positioning data set of each axis in turn to obtain high-precision single-axis fusion data; combine the high-precision single-axis fusion data of each axis to obtain a preliminary cooperative positioning result; Step 3.3: Perform iterative update on the parameters of the internal factor graph model of the vehicle to be positioned through the preliminary cooperative positioning result. When the preset update condition is reached, obtain the heterogeneous navigation information data fusion cooperative positioning result of all vehicles in the vehicle cluster network.

2. The collaborative positioning method for vehicle cluster navigation source data fusion based on factor graph according to claim 1, characterized in that In Step 1, if the navigation source is an inertial navigation source, the process of obtaining the vehicle positioning parameter information according to the inertial navigation source, calculating the vehicle navigation source positioning result and its three-dimensional covariance matrix, and obtaining the probability density function of the inertial navigation source of all vehicles is as follows: Step 1.1: Obtain vehicle positioning parameter information according to the inertial navigation source and establish a vehicle coordinate system; The vehicle positioning parameter information includes vehicle attitude, gyroscope and accelerometer parameter information, sampling frequency, and calibration period; Define the position coordinates of the vehicle to be located as p = (x, y, z) T , and the attitude of the vehicle to be located as G B = (φ, θ, ψ) T , the output of the accelerometer is f B , and the static zero biases of the accelerometer and gyroscope are b a = (b ax , b ay , b az ) T and b ω = (b ωφ , b ωθ , b ωψ ) T , and the standard deviations of the accelerometer noise and gyroscope noise are respectively and The sampling frequency is 1 / T s , and the calibration period is T; The established vehicle coordinate systems include the northeast celestial coordinate system, the body coordinate system, the geodetic coordinate system, and the earth-centered earth-fixed coordinate system. Define the northeast celestial coordinate system as the N system, the body coordinate system is simply referred to as the B system, the geodetic coordinate system is simply referred to as the I system, and the earth-centered earth-fixed coordinate system is simply referred to as the E system; Step 1.2: Construct the inertial navigation source positioning update equation set, and substitute the vehicle positioning parameter information obtained in Step 1.1 into the inertial navigation source positioning update equation set for update calculation to obtain the positioning result p of the vehicle to be positioned t ; The inertial navigation source positioning update equation set includes an attitude update differential equation, an attitude error differential equation, a velocity update differential equation, a velocity update equation, a velocity error differential equation, a position update differential equation, a position update equation, and a position error differential equation; The attitude update differential equation is: Among them, is the attitude transfer matrix from the B system to the N system, is the differential of, is the projection of the rotational angular velocity of the B system relative to the N system in the B system, is the skew-symmetric matrix formed by, and The attitude error differential equation is: where η is the attitude error between the output attitude of the inertial navigation source and the true attitude of the vehicle, is the differential of η, is the projection of the rotation angular rate of the N system relative to the I system in the N system, is the error of, that is, the attitude calculation error of the N system, is the projection of the rotation angular rate of the B system relative to the I system in the B system, that is, the output value of the gyroscope, is the error of, that is, the measurement error of the gyroscope; The velocity update differential equation: where, v N is the projection of the vehicle speed in the N system, is the differential of v N , that is, the projection of the vehicle acceleration in the N system, f B is the projection of the accelerometer measurement value in the B system, is the projection of the angular rate of rotation of the E system relative to the I system caused by the Earth's rotation in the N system, is the projection of the angular rate of rotation of the N system relative to the vehicle's motion in the N system, and g N are the Coriolis calibration term and the projection of the gravitational acceleration in the N system, respectively; The speed update equation is as follows: Among them, v t+Δt is the speed of the moving vehicle at time t + Δt, and v t is the speed of the moving vehicle at time t, is the velocity differential, i.e., the acceleration, and Δt is the time interval; The velocity error differential equation is: Among them, is the projection of the differential of the vehicle speed error in the N system, and f N is the projection of the accelerometer measurement value in the N system, and represents the skew-symmetric matrix formed by the accelerometer measurement values in the N system, and δg N respectively represent the specific force measurement error, speed error, earth's angular rotation rate calculation error, navigation system rotation calculation error, and gravity error in the N system; The position update differential equation is as follows: where p N is the projection of the vehicle position in the N system, is the differential of p N , and v N is the projection of the vehicle speed in the N system; The position update equation is: where p t+Δt is the position of the moving vehicle at time t + Δt, and p t is the position of the moving vehicle at time t, is the differential of the position, i.e., the velocity, and Δt is the time interval; The position error differential equation is as follows: Among them, is the projection of the differential of the vehicle position error under the projection in the N system, and δv N is the velocity error of the vehicle in the northeast celestial coordinate system; Step 1.3, calculate the measurement errors of the inertial navigation source at each moment; First, establish the mathematical models of the accelerometer and gyroscope: Wherein: and respectively represent the output values of the accelerometer and gyroscope in the B system, f B and ω B respectively represent the true values of the acceleration and attitude angular velocity in the B system, S a and S ω respectively represent the scale factor errors of the accelerometer and gyroscope, N a and N ω respectively represent the non-orthogonal errors of the accelerometer and gyroscope, b a and b ω respectively represent the zero biases of the accelerometer and gyroscope, ε a and ε ω respectively represent the random noises of the accelerometer and gyroscope; Then, according to the mathematical models and the inertial navigation source positioning update equations, calculate the measurement errors of the inertial navigation source at any moment, where the measurement errors of the inertial navigation source are the sum of the bias error and the random noise error; Assume that the vehicle carrying the inertial navigation source is in a stationary state, and the accelerometer and gyroscope noises are independent of each other, and the accelerometer bias and attitude errors are independent of each other; in this state: The zero bias error is Wherein: and are the zero biases of the accelerometer and the gyroscope respectively, and t is the integration duration; Random noise error Where: and are the standard deviations of the accelerometer noise and the gyroscope noise respectively, t is the integration duration, and δt is the sampling interval; Step 1.4, correct the measurement error of the navigation source at the current moment according to the measurement error of the navigation source at the previous moment, and calculate the positioning result of the vehicle inertial navigation source and the corresponding three-dimensional covariance matrix according to the formula; Set at t - T after the inertial navigation source calibration s The error model of the inertial navigation source at the moment is The error model of the inertial navigation source at time t after the inertial navigation source calibration is Among them, [f N × represents the skew-symmetric matrix formed by the accelerometer measurement values in the N system, and diag(·) is the diagonal matrix construction function; is the inertial navigation source error at time t-T s ; μ t is the inertial navigation source error at time t; is the covariance matrix of the inertial navigation source error at time t-T s ; ∑ t is the covariance matrix of the inertial navigation source error at time t;​ Definition: At the moment of t - T after the inertial navigation source calibration, the attitude G s of the vehicle to be located B =(φ, θ, ψ) T , the accelerometer measurement value is f B , and the static zero biases of the accelerometer and gyroscope are b a =(b ax , b ay , b az ) T and b ω =(b ωφ , b ωθ , b ωψ ) T , the standard deviations of the accelerometer noise and gyroscope noise are respectively and The sampling frequency is 1 / T s , the calibration period is T, and the attitude transition matrix is calculated and determined from the attitude G B of the vehicle to be located; Then, at time t after the inertial navigation source is calibrated, the positioning result of the vehicle inertial navigation source is obtained The corresponding three-dimensional covariance matrix Step 1.5, since the random errors are all Gaussian white noises, it is determined that the positioning results x of the vehicle inertial navigation source all satisfy the three-dimensional Gaussian distribution; according to the results of Step 1.2 and Step 1.3, obtain the positioning information probability function of the navigation source, which is where ζ is a constant term, and Step 1.6, according to the calculation process of Step 1.1 - Step 1.5, obtain the probability functions of each vehicle inertial navigation source.

3. The collaborative positioning method for vehicle cluster navigation source data fusion based on a factor graph according to claim 1, characterized in that, In Step 2, according to the positioning result of the vehicle navigation source and the corresponding three-dimensional covariance matrix obtained in Step 1, the specific process of performing confidence iteration update on the internal factor graph model parameters of the vehicle to be located to obtain the positioning result of the collaborative vehicle for the vehicle to be located and the estimated value of its three-dimensional covariance matrix is as follows: The specific process of Step 2 is as follows: First, a three-dimensional spherical coordinate system is established with the vehicle to be located as the origin, and the ranging and direction-finding information between the vehicle to be located and the cooperative vehicle is defined using where r i is the ranging information, which satisfies a Gaussian distribution with a mean of r i and a variance of ; is the angle formed by the half-plane passing through the Z-axis and the position of the cooperative vehicle and the coordinate plane ZOX, and θ i is the angle between the straight line connecting the vehicle to be located and the cooperative vehicle and the positive direction of the Z-axis, and 0 ≤ θ i ≤ π; Then, use the positioning result of the navigation source of the collaborative vehicle obtained in Step 1 and the ranging and direction-finding information between the vehicle to be located and the collaborative vehicle as the input parameters for the first iteration update, and calculate the predicted positioning result of the collaborative vehicle for the vehicle to be located and its three-dimensional covariance matrix according to the iteration formula; in subsequent iterations, use the output parameters of the previous iteration as the input parameters for this iteration for iteration update; The iteration calculation formula is: When the iteration times are reached or the output result meets the accuracy threshold, the iteration terminates; use the output value at the end of the iteration as the estimated value of the positioning result of the collaborative vehicle for the vehicle to be located and its three-dimensional covariance matrix.

4. The method for collaborative positioning by fusing source data for vehicle cluster navigation based on factor graph according to claim 3, characterized in that The specific process of Step 3.2 is as follows: Step 3.2.1, set a set of positioning data for a certain axis includes n data, that is Solve to obtain the mean μ of the positioning data set in x and the standard deviation σ x , respectively: Step 3.2.2, first, according to the mean value μ of the positioning data set x and the standard deviation σ x , determine the initial screening condition as μ x - 2σ x ≤ x i ≤ μ x + 2σ x , i = 1, 2, …, n; then, from the positioning data set of this axis, screen the positioning data located within the initial screening condition; Define the number of positioning data on this axis before preliminary screening as a, where the standard deviations of b (b ≤ a) positioning data are known; Step 3.2.3, define the number of target positioning data after re-screening as c (c ≤ a). If c ≤ b, then select the first c positioning data with the smallest error as the high-precision positioning data on this axis; If b < c ≤ a, first select b positioning data with known standard deviations, and then select the positioning data with unknown standard deviations that are closest to the mean μ of the distance c - b from the remaining positioning data as the high-precision positioning data for this axis; x ​ Step 3.2.

4. First, according to the high-precision positioning data on each orthogonal axis, Gaussian information probability density functions are respectively constructed; the Gaussian information probability density function of the i-th navigation data after screening is defined as That is Then, determine the fusion criterion. The fusion criterion adopts a fusion strategy based on the Kullback-Leibler distance, and the fusion criterion formula is: where arg inf represents the maximum point of the maximum likelihood function, represents the Kullback-Leibler distance between two information probability density functions, and Again, calculate the K-L divergence of the Gaussian information probability density function on the Riemannian manifold through information geometry theory to obtain the Fisher information distance from each point on the Riemannian manifold to each navigation source; Finally, select the point on the Riemannian manifold with the shortest sum of the Fisher information distances to each navigation source, and take the fusion positioning result and standard deviation corresponding to this point as the high-precision single-axis fusion data; Step 3.2.5: Combine the high-precision single-axis fusion data of each orthogonal axis to obtain the preliminary cooperative positioning result and its corresponding covariance matrix.

5. The collaborative positioning method for vehicle cluster navigation source data fusion based on a factor graph according to claim 4, wherein The specific process of Step 3.3 is as follows: Use the preliminary cooperative positioning information obtained in Step 3.2.5 as the initial iterative input parameter, and iteratively update the factor graph parameters of all vehicles to be positioned through the feedback iterative function; in subsequent iterations, use the output parameter information of the previous iteration as the input parameter of the next iteration; When the iteration reaches the convergence condition or the maximum number of iterations k max the iteration process terminates, and the positioning result output by the last iteration is used as the heterogeneous navigation information data fusion collaborative positioning result of the vehicles in the vehicle cluster network; The feedback iterative function is; Among them, k is the number of iterations, and are the positioning results output by the cooperative vehicle and the vehicle to be positioned at the k-th iteration respectively; In each round of iteration, the vehicle positioning results and their corresponding covariance matrices in the vehicle cluster network, as well as the ranging and direction-finding information between cooperative vehicles and vehicles to be positioned, are synchronously updated.

Images