Vehicle cluster multi-source fusion cooperative positioning method based on information geometry

Abstract

The invention provides a vehicle cluster multi-source fusion cooperative positioning method based on information geometry, and the method comprises the steps: firstly constructing a vehicle cluster heterogeneous multi-source fusion cooperative positioning system model, and obtaining navigation source information density functions of all vehicles; carrying out fusion calculation on a vehicle navigation source information density function, and respectively obtaining single-vehicle multi-navigation-source fusion positioning results of the to-be-positioned vehicle and the cooperative vehicle and corresponding three-dimensional covariance matrixes; and finally, constructing a vehicle cluster factor graph co-location model through distance and direction finding information between the to-be-located vehicle and the co-location vehicle, obtaining an internal factor graph model of the to-be-located vehicle, and carrying out confidence coefficient information iteration updating to obtain a multi-source fusion co-location result of the to-be-located vehicle and the co-location vehicle. According to the method, heterogeneous navigation source data is converted into a unified information probability function form, navigation source fusion is realized by using a theoretical framework of information geometry, the vehicle cluster positioning precision and positioning speed are improved, and the large-scale vehicle cluster navigation positioning requirements in a complex environment are met.

Description

Technical Field

[0001] The present invention belongs to the technical field of vehicle navigation, and in particular relates to a vehicle cluster multi-source fusion collaborative positioning method based on information geometry. Background Art

[0002] With the rapid development of core smart city services such as autonomous driving and intelligent transportation, the demand for real-time, high-precision vehicle navigation and positioning is growing. To improve vehicle navigation and positioning accuracy, various technologies, such as satellite navigation, inertial navigation, laser navigation, radio navigation, and visual navigation, are being applied. Due to the limited accuracy and significant drawbacks of a single navigation source, multi-source combined navigation systems that aggregate, coordinate, fuse, and optimize information from multiple sensors and navigation sources have become the subject of much research.

[0003] Existing fusion technologies primarily achieve navigation and positioning fusion through Kalman filtering. While this approach can effectively improve positioning accuracy, it suffers from high algorithmic complexity and poor scalability. Because the observation matrix continues to expand with the number of navigation sources, fusing information from more than two sources increases the computational complexity of the Kalman filter and the communication overhead, making it difficult to meet the practical needs of real-time vehicle positioning. New fusion methods, such as neural networks, leverage their powerful nonlinear processing and adaptive learning capabilities to adapt to a variety of complex scenarios while offering strong noise immunity and robustness. However, these methods also suffer from high complexity and computational overhead. Furthermore, existing multi-source fusion positioning methods fail to fully account for the differences among various navigation source sensors in terms of sampling rate, measurement quality, and spatiotemporal characteristics of the data. This makes unifying the data types of heterogeneous navigation source positioning difficult in practical applications.

[0004] At the same time, with the rapid development of large-scale vehicle cluster collaborative services such as the Internet of Vehicles and smart transportation, collaborative localization for vehicle clusters has become a key research focus. Currently, large-scale vehicle cluster collaborative localization primarily leverages inter-vehicle ranging and direction-finding information to exchange vehicle positioning information. However, in real-world vehicle cluster environments, due to the high-speed mobility of vehicles, collaborative localization architectures must accommodate the requirements of on-demand access and free gathering and dispersal, significantly increasing the difficulty of collaborative localization.

[0005] Therefore, there is an urgent need for a vehicle cluster positioning method with high positioning accuracy, strong real-time performance, and the ability to connect and disperse freely under high-speed movement. This method has important practical significance for building large-scale high-precision navigation and positioning systems in intelligent vehicle scenarios such as vehicle networking and autonomous driving. Summary of the Invention

[0006] The purpose of the present invention is to overcome the defects of existing vehicle cluster positioning methods, such as complex process, poor positioning accuracy, and inability to adapt to complex scenes, and to provide a vehicle cluster multi-source fusion collaborative positioning method based on information geometry.

[0007] To achieve the above objectives, the technical solutions provided by the present invention are:

[0008] The multi-source fusion collaborative localization method for vehicle clusters based on information geometry is special in that it includes the following steps:

[0009] Step 1: Build a vehicle cluster heterogeneous multi-source fusion collaborative positioning system model, obtain vehicle positioning parameter information through different navigation sources, calculate the vehicle navigation source positioning results and the corresponding three-dimensional covariance matrix, and obtain the navigation source information density function of all vehicles;

[0010] The vehicle cluster heterogeneous multi-source fusion collaborative positioning system model includes a vehicle to be positioned, several collaborative vehicles and multiple navigation source devices;

[0011] The vehicles are connected to each other and to the navigation source via ranging and direction finding communication links;

[0012] Step 2: Fusion calculates the vehicle navigation source information density function obtained in step 1 to obtain the single-vehicle multi-navigation source fusion positioning results and corresponding three-dimensional covariance matrices of the vehicle to be positioned and the cooperative vehicle respectively;

[0013] Step 3: Based on the ranging and direction information between the vehicle to be located and the cooperative vehicle, as well as the single-vehicle multi-navigation source fusion positioning results of the vehicle to be located and the cooperative vehicle obtained in step 2 and the corresponding three-dimensional covariance matrix, a vehicle cluster factor graph collaborative positioning model is constructed, the internal factor graph model of the vehicle to be located is obtained, and the confidence information of the internal factor graph model parameters is iteratively updated to obtain the multi-source fusion collaborative positioning result of the vehicle to be located.

[0014] Furthermore, in step 1, the navigation source device includes an inertial navigation source, a radio navigation source, and a satellite navigation source.

[0015] Furthermore, in step 1, if the navigation source device is an inertial navigation source, the process of calculating the vehicle's inertial navigation source positioning result and the corresponding three-dimensional covariance matrix to obtain the inertial navigation source information density function of all vehicles is as follows:

[0016] Step 1.1: Obtain vehicle positioning parameter information based on the inertial navigation source and establish the vehicle coordinate system;

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

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

[0019] The established vehicle coordinate system includes the Northeast Celestial coordinate system, the carrier coordinate system, the Earth coordinate system and the Earth-centered Earth-fixed coordinate system. The Northeast Celestial coordinate system is defined as the N system, the carrier coordinate system is referred to as the B system, the Earth coordinate system is referred to as the I system, and the Earth-centered Earth-fixed coordinate system is referred to as the E system.

[0020] Step 1.2: Construct the inertial navigation source positioning update equation group, and substitute the vehicle positioning parameter information obtained in step 1.1 into the inertial navigation source positioning update equation group for update calculation to obtain the positioning result p of the vehicle to be positioned. t ;

[0021] The inertial navigation source positioning update equation group 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;

[0022] The attitude update differential equation is:

[0023]

[0024] in, is the attitude transfer matrix from B system to N system, for The differential of is the projection of the rotation angular velocity of the B system relative to the N system in the B system, for The antisymmetric matrix is formed, and

[0025]

[0026] The attitude error differential equation is:

[0027]

[0028] 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, for The error, that is, the attitude calculation error of the N system, is the projection of the rotation angular rate of system B relative to system I in system B, that is, the output value of the gyroscope, for The error is the measurement error of the gyroscope;

[0029] The velocity update differential equation is:

[0030]

[0031] Among them, v N is the projection of vehicle speed in the N system, v N The differential of the vehicle acceleration, i.e. the projection of the vehicle acceleration in the N system, 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 system relative to the I system caused by the rotation of the Earth in the N system, is the projection of the rotation angular rate 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 gravitational acceleration in the N system respectively;

[0032] The velocity update equation is:

[0033] Among them, v t+Δt is the speed of the moving vehicle at time t+Δt, v t is the speed of the moving vehicle at time t, is the velocity differential, i.e. acceleration, and Δt is the time interval;

[0034] The velocity error differential equation is:

[0035]

[0036] in, is the projection of the vehicle speed 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 antisymmetric matrix of the accelerometer measurements in the N system, δv N 、 and δg N They represent the specific force measurement error, velocity error, Earth rotation angular rate calculation error, navigation system rotation calculation error, and gravity error in the N system respectively;

[0037] The position update differential equation is: Among them, p N is the projection of the vehicle position in the N system, For p N The differential of v N is the projection of vehicle speed in the N system;

[0038] The position update equation is:

[0039]

[0040] Among them, 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;

[0041] The position error differential equation is:

[0042] in, is the projection of the vehicle position error differential in the N system, δv N is the velocity error of the vehicle in the northeast celestial coordinate system;

[0043] Step 1.3, calculate the inertial navigation source measurement error at each moment;

[0044] First, establish the mathematical model of the accelerometer and gyroscope:

[0045]

[0046] in: and Represent the output values of the accelerometer and gyroscope in the B frame, f B and ω B They represent the true values of acceleration and attitude angular velocity in frame B, S a and S ω Represent the scale factor errors of the accelerometer and gyroscope, N a and N ω are the non-orthogonal errors of the accelerometer and gyroscope, respectively, and b a and b ω Denote the zero bias of the accelerometer and gyroscope, ε aand ε ω represent the random noise of accelerometer and gyroscope respectively;

[0047] Then, according to the mathematical model and the inertial navigation source positioning update equation group, the inertial navigation source measurement error at any time is calculated, where the inertial navigation source measurement error is the sum of the zero bias error and the random noise error;

[0048] Assume that the vehicle carrying the inertial navigation source is stationary, the accelerometer and gyroscope noises are independent of each other, and the accelerometer bias and attitude error are independent of each other. In this state:

[0049] The bias error is in: and are the zero bias of the accelerometer and the zero bias of the gyroscope, respectively, and t is the integration time;

[0050] Random noise error in: and are the standard deviation of accelerometer noise and gyroscope noise, t is the integration time, and δt is the sampling interval;

[0051] Step 1.4: Correct the current navigation source measurement error based on the previous navigation source measurement error, and calculate the vehicle inertial navigation source positioning result and the corresponding three-dimensional covariance matrix according to the formula;

[0052] Set tT after inertial navigation source calibration s The inertial navigation source error model at time The error model of the inertial navigation source at time t after the inertial navigation source is calibrated is:

[0053]

[0054] Among them, [f N ] × represents the antisymmetric matrix formed by the accelerometer measurements in the N-frame, and diag(·) is the diagonal matrix construction function; tT s Time inertial navigation source error; μ t is the inertial navigation source error at time t; tT s Covariance matrix of inertial navigation source error at the moment; Σ t is the covariance matrix of the inertial navigation source error at time t;

[0055] Definition: tT after inertial navigation source calibration s At this moment, the posture of the vehicle to be positioned G B =(φ,θ,ψ)T , the accelerometer measurement value is f B , the static zero bias of the accelerometer and gyroscope are b a =(b ax ,b ay ,b az ) T and b ω =(b ωφ ,b ωθ ,b ωψ ) T The standard deviation of the accelerometer noise and the standard deviation of the gyroscope noise are and The sampling frequency is 1 / T s , the calibration period is T, the attitude transfer matrix The posture G of the vehicle to be positioned B Calculate and determine;

[0056] Then the vehicle inertial navigation source positioning result at time t after the inertial navigation source is calibrated is obtained The corresponding three-dimensional covariance matrix

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

[0058]

[0059] where ζ is a constant term, and

[0060] Step 1.6: Based on the calculation process of steps 1.1 to 1.5, the probability function of the positioning information of the inertial navigation source of each vehicle is obtained.

[0061] Furthermore, the fusion process of step 2 is:

[0062] Step 2.1: Based on the positioning information probability functions of each vehicle corresponding to different types of navigation sources, a three-dimensional Gaussian probability density function of the fusion positioning results of each vehicle's own navigation source is constructed;

[0063] Definition: The number of types of navigation sources corresponding to the vehicle is N, and the probability density function of the i-th positioning information is p i (·), the corresponding weight factor in the fusion is a i , the information density function after fusion is p f (·);

[0064] The fusion criterion formula is:

[0065]

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

[0067] Step 2.2: Calculate the KL divergence of the three-dimensional Gaussian probability density function on the Riemannian manifold using information geometry theory to obtain the Fisher information distance from each point on the Riemannian manifold to each navigation source.

[0068] In step 2.3, select the point in the Riemann manifold with the shortest sum of Fisher information distances to each navigation source, and take the mean and covariance matrix of the probability density function corresponding to this point as the single-vehicle multi-navigation source fusion positioning result and the corresponding three-dimensional covariance matrix.

[0069] Furthermore, in step 3, the specific process of obtaining the multi-source fusion collaborative positioning results of the vehicle to be positioned and the cooperative vehicle by iteratively updating the confidence information of the internal factor graph model of the vehicle to be positioned is as follows:

[0070] Step 3.1: Based on the single-vehicle multi-navigation source fusion positioning results of the vehicle to be positioned and the cooperative vehicle and the corresponding three-dimensional covariance matrix, the initial information of the vehicle to be positioned (μ q0 ,Σ q0 ) and collaborative initial position information (μ i ,Σ i );

[0071] To be positioned vehicle M q Establish a three-dimensional spherical coordinate system as the origin and define the cooperative vehicle M i To the vehicle M to be located q Distance information where r i is the ranging information, satisfying the mean value of r i , the variance is Gaussian distribution; To pass through the Z axis and the cooperative vehicle M i The angle formed by the half plane of and the coordinate plane ZOX, and θ i is line segment M q M i The angle with the positive direction of the Z axis, and 0≤θ i ≤π;

[0072] According to the three-dimensional coordinate relationship, we can get

[0073] Step 3.2: Collect the input parameters of the kth iteration involved in the confidence information update, including the vehicle M to be located q Positioning information output at the k-1th iteration Cooperative vehicle M i Positioning information output at the k-1th iteration and ranging information

[0074] Step 3.3, Upward Iteration:

[0075] First, through the function equation C i Distance measurement information The information is processed and converted into the coordinated vehicle M in the Euclidean space. i and the vehicle M to be positioned q Coordinate difference information

[0076] The functional equation C i for:

[0077]

[0078] Then, through the function equation B i For cooperative vehicle M i Location information and coordinate difference information Processing is performed to obtain the cooperative vehicle M i The vehicle M to be positioned q Prediction information

[0079] The functional equation B i for:

[0080]

[0081] Finally, the vehicle M to be located is transformed into q Prediction information and initial value Perform fusion to obtain the vehicle M to be positioned q The initial value of the next iteration

[0082] The functional equation A is:

[0083]

[0084] Step 3.4, downward iteration:

[0085] First, the feedback information is obtained by calculating the function equation A′ The functional equation A′ is:

[0086]

[0087] Then, through the function equation B i Feedback information and cooperative vehicle M i Location information Processing is performed to obtain the coordinate difference information of the downlink iteration The functional equation B i 'for:

[0088]

[0089] Finally, through the function equation C i 'right Process it and convert it into the initial value of the ranging information for the next iteration The functional equation C i 'for:

[0090]

[0091]

[0092] In step 3.5, the confidence is updated according to steps 3.1 to 3.4. When the preset maximum number of convergences is reached or the convergence condition is met, the iteration is terminated and the multi-source fusion collaborative positioning result of the vehicle to be located is obtained.

[0093] The advantages of the present invention are:

[0094] The proposed method for multi-source fusion collaborative localization for vehicle swarms, based on information geometry, focuses on transforming heterogeneous navigation source data into a unified information probability function by establishing an information probability model for the navigation sources. This method also utilizes the theoretical framework of information geometry to achieve navigation source fusion, overcoming the limitations of traditional methods in processing data of varying formats and frequencies. Furthermore, the method leverages the theoretical framework of factor graphs to enable the transmission and synchronous iteration of navigation information between distributed vehicles, thus achieving distributed multi-source fusion collaborative localization. This method effectively improves the localization accuracy and speed of vehicle swarms, meeting the navigation and localization needs of large-scale vehicle swarms in complex environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0095] Figure 1 This is a flow chart of the vehicle cluster multi-source fusion collaborative positioning method based on information geometry of the present invention;

[0096] Figure 2 1 is a model diagram of a vehicle cluster heterogeneous multi-source fusion collaborative positioning system constructed in an embodiment of the present invention;

[0097] Figure 3This is a schematic diagram of radio navigation source positioning in an embodiment of the present invention;

[0098] Figure 4 It is a distributed collaborative positioning model of vehicle clusters in an embodiment of the present invention;

[0099] Figure 5 is a schematic diagram of a spherical coordinate system established in an embodiment of the present invention;

[0100] Figure 6 is a vehicle internal factor graph model in an embodiment of the present invention;

[0101] Figure 7 2. It is a schematic diagram of a simulation scenario of a vehicle cluster collaborative positioning experiment in an embodiment of the present invention;

[0102] Figure 8 It is a comparison of the accuracy of the method of the present invention and the traditional method;

[0103] Figure 9 It is a comparison of the convergence speed between the method of the present invention and the traditional method;

[0104] Figure 10 This is a comparison of the stability of the method of the present invention and the traditional method in the face of mutation errors. DETAILED DESCRIPTION

[0105] The following describes in detail embodiments of the present invention, examples of which are shown in the accompanying drawings, wherein the same or similar reference numerals throughout represent the same or similar elements or elements having the same or similar functions. The following describes in detail embodiments of the present invention, which are exemplary and intended to be used to explain the present invention, and are not to be construed as limiting the present invention.

[0106] This embodiment takes the real-time precise positioning of a vehicle cluster of n=40 vehicles in a sloped circular overpass with an average circular radius of 100m and a maximum height difference of 30m as an example to describe the vehicle cluster multi-source fusion collaborative positioning method based on information geometry of the present invention. Specifically, the method includes the following steps:

[0107] Step 1: Build a vehicle cluster heterogeneous multi-source fusion collaborative positioning system model to obtain the vehicle navigation source positioning results and their three-dimensional covariance matrix, as follows:

[0108] In the constructed vehicle cluster heterogeneous multi-source fusion collaborative positioning system model, each vehicle is equipped with multiple navigation source devices, can use multiple navigation sources for positioning, and can establish communication with other collaborative vehicles within the communication distance to measure the distance and direction information of the other party.

[0109] Reference Figure 2The orange area represents the cluster of vehicles to be located. The blue links between vehicles indicate that the vehicles can communicate with each other using ranging. The red links represent the ranging communication links between the vehicles to be located and satellites, indicating that the vehicles to be located can use satellite navigation sources for positioning. The yellow links represent the ranging communication links between the vehicles to be located and wireless base stations, indicating that the vehicles to be located can use radio navigation sources for positioning. The black dashed lines indicate that the vehicles to be located carry inertial devices, indicating that the vehicles to be located can use inertial navigation sources for positioning. The positioning parameters of each navigation source are obtained by using the various types of navigation sources carried by the vehicles. The positioning equations for each navigation source are then used to solve the positioning results for each navigation source on each vehicle. A navigation source error propagation model is established based on the error sources and error propagation. Combined with the acquired positioning parameters, the three-dimensional covariance matrix of the positioning results for each navigation source on each vehicle is obtained.

[0110] The following uses inertial navigation sources, radio navigation sources, and satellite navigation sources as examples to illustrate the specific processes of obtaining vehicle positioning parameter information based on different navigation sources, calculating the vehicle's corresponding navigation source positioning results and the corresponding three-dimensional covariance matrix, and obtaining the navigation source information density function for all vehicles.

[0111] (1) Inertial navigation source:

[0112] Step 1.1, obtaining vehicle positioning parameter information based on an inertial navigation source, wherein the vehicle positioning parameter information includes vehicle attitude, gyroscope and accelerometer parameter information, sampling frequency, and calibration period;

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

[0114] Establish coordinate system definitions, including: Northeast Celestial Coordinate System, Carrier Coordinate System, Geodetic Coordinate System and Earth-centered Earth-fixed Coordinate System. Define the Northeast Celestial Coordinate System as N System, the Carrier Coordinate System as B System, the Geodetic Coordinate System as I System, and the Earth-centered Earth-fixed Coordinate System as E System.

[0115] Step 1.2: Construct the inertial navigation source positioning solution equation group, and solve the vehicle inertial navigation source positioning result based on the vehicle positioning parameter information. where p t is the calculation result of the inertial navigation source, μ t is the inertial navigation source error The process of inertial navigation source positioning solution is divided into three stages: attitude update, velocity update and position update.

[0116] Attitude update differential equation:

[0117]

[0118] in, is the attitude transfer matrix from the B system to the N system, for The differential of is the projection of the rotation angular velocity of the B system relative to the N system in the B system, for The antisymmetric matrix is formed, and

[0119]

[0120] The attitude error differential equation is:

[0121]

[0122] 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, for The error, that is, the attitude calculation error of the N system, is the projection of the rotation angular rate of system B relative to system I in system B, that is, the output value of the gyroscope, for The error is the measurement error of the gyroscope;

[0123] Velocity update differential equation:

[0124]

[0125] Among them, v N is the projection of vehicle speed in the N system, vN The differential of the vehicle acceleration, i.e. the projection of the vehicle acceleration in the N system, 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 system relative to the I system caused by the rotation of the Earth in the N system, is the projection of the rotation angular rate 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 gravitational acceleration in the N system respectively;

[0126] The velocity update equation is:

[0127] Among them, v t+Δt is the speed of the moving vehicle at time t+Δt, v t is the speed of the moving vehicle at time t, is the velocity differential, i.e. acceleration, and Δt is the time interval;

[0128] The velocity error differential equation is:

[0129]

[0130] in, is the projection of the vehicle speed 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 antisymmetric matrix of the accelerometer measurements in the N system, δv N 、 and δg N They represent the specific force measurement error, velocity error, Earth rotation angular rate calculation error, navigation system rotation calculation error, and gravity error in the N system respectively;

[0131] The position update differential equation is: Among them, p N is the projection of the vehicle position in the N system, For p N The differential of v N is the projection of vehicle speed in the N system;

[0132] The position update equation is:

[0133]

[0134] Among them, 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;

[0135] The position error differential equation is:

[0136] in, is the projection of the vehicle position error differential in the N system, δv N is the velocity error of the vehicle in the northeast celestial coordinate system;

[0137] Step 1.3: Establish the mathematical model of the accelerometer and gyroscope, and update the calculation to obtain the inertial navigation source measurement error.

[0138] The inertial navigation source error is mainly caused by the measurement error of the inertial device and the error of the Earth's geographic parameters. Considering that for the vehicle-mounted inertial navigation source, the error caused by the change of the Earth's geographic parameters such as the Earth's gravity field and the Earth's rotation angular rate in a short period of time has a smaller impact on the navigation accuracy than the navigation error caused by the inertial device error and does not accumulate over time, the error caused by the change of the Earth's geographic parameters is ignored when calculating the inertial navigation source error.

[0139] Inertial device measurement errors can be divided into two main components: deterministic error and random error. Since deterministic error can be compensated through laboratory calibration, the random error component is analyzed. The random error component primarily includes bias and random noise. The error model can be composed of two components: a random constant + white noise, representing the static and rapidly varying components of the random error, respectively.

[0140] First, the output mathematical model of the accelerometer and gyroscope is constructed. The output mathematical model is

[0141]

[0142] in: and Represent the output values of the accelerometer and gyroscope in the B frame, f B and ω B They represent the true values of acceleration and attitude angular velocity in frame B, S a and S ω Represent the scale factor errors of the accelerometer and gyroscope, N a and N ω are the non-orthogonal errors of the accelerometer and gyroscope, respectively, and b a and b ω Denote the zero bias of the accelerometer and gyroscope, ε a and ε ω represent the random noise of accelerometer and gyroscope respectively;

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

[0144] When there is a zero bias in the accelerometer When the inertial navigation source positioning solution equations are updated, the accelerometer will introduce an error in the velocity that is proportional to time t, and in the position that is proportional to t 2 Proportional error, where the speed error is The position error is

[0145] When there is a zero bias in the gyroscope When , an angle error proportional to time t will be introduced. After the inertial navigation source positioning solution equations are updated and solved, speed and position errors will be caused, where the speed error is The position error is

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

[0147]

[0148] in, and are the zero bias of the accelerometer and gyroscope respectively, and t is the integration time.

[0149] Calculate the noise error. The specific calculation process is:

[0150] The epochs of white noise are uncorrelated, so each random variable (epoch) has a mean of 0 and a variance of σ. 2 Let N i is the i-th random variable in the white noise sequence, then the mean and variance satisfy:

[0151] E(N i )=0

[0152] Var(N i )=σ 2

[0153] Due to the irrelevance of adjacent epochs of white noise, we have:

[0154]

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

[0156]

[0157] Where n is the number of data samples and δt is the interval between consecutive samples, i.e. the sampling time. Based on the following formula:

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

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

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

[0161]

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

[0163]

[0164] in, is the standard deviation of the accelerometer noise, t is the integration time, and δt is the sampling interval.

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

[0166]

[0167]

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

[0169]

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

[0171]

[0172] in, is the standard deviation of the gyroscope noise, t is the integration time, and δt is the sampling interval.

[0173] In order to derive the error caused by the gyroscope white noise to the horizontal position measurement, the noise needs to be integrated three times:

[0174]

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

[0176]

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

[0178]

[0179] in, and are the standard deviation of accelerometer noise and the standard deviation of gyroscope noise, t is the integration time, and δt is the sampling interval.

[0180] Step 1.4, obtain the positioning result of the inertial navigation source and the corresponding three-dimensional covariance matrix;

[0181] Set tT after inertial navigation source calibration s At this moment, the posture of the vehicle to be positioned G B =(φ,θ,ψ) T , the accelerometer measurement value is f B , the static zero bias of the accelerometer and gyroscope are b a =(b ax ,b ay ,b az ) T and b ω =(b ωφ ,b ωθ ,b ωψ ) T The standard deviation of the accelerometer noise and the standard deviation of the gyroscope noise are and The sampling frequency is 1 / T s , the calibration period is T, the attitude transfer matrix The posture G of the vehicle to be positioned B Determine, then the error model of the inertial navigation source at time t after the inertial navigation calibration is in

[0182]

[0183] Among them, [f N ] × represents the antisymmetric matrix formed by the accelerometer measurements in the N-frame, and diag(·) is the diagonal matrix construction function; tT s Time inertial navigation source error; μt is the inertial navigation source error at time t; tT s Covariance matrix of inertial navigation source error at the moment; Σ t is the covariance matrix of the inertial navigation source error at time t.

[0184] Assume that at time t after the inertial navigation calibration, the positioning result of the vehicle to be positioned 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

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

[0186]

[0187] where ζ is a constant term and

[0188] Step 1.6: According to the calculation process of steps 1.1 to 1.5, the positioning information probability function of each vehicle inertial navigation source is obtained.

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

[0190] Reference Figure 3 , which is a schematic diagram of radio navigation source positioning.

[0191] Step 1.1: Acquire positioning parameter information based on wireless base stations. The positioning parameter information includes the number of wireless base stations, the location of each wireless base station, and distance measurement information between the vehicle and the wireless base stations.

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

[0193] Step 1.2: Based on the vehicle's positioning parameter information, a set of radio navigation source positioning solution equations is constructed and solved to obtain the vehicle's navigation source positioning result based on the radio navigation source.

[0194] According to the Euclidean distance calculation formula, the equations for solving radio navigation source positioning are:

[0195]

[0196] In order to reduce the influence of the inherent error of the vehicle to be positioned, the two adjacent equations are subtracted to obtain:

[0197]

[0198] To simplify further, define:

[0199]

[0200] get:

[0201] Ax=b

[0202] The least squares method is used to solve the above equation, and the solution is:

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

[0204] To simplify further, define:

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

[0206] The vehicle's navigation source positioning results based on the radio navigation source can be obtained:

[0207] x=Pb

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

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

[0210] In practical applications, the geometric distribution of 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 the mean value The standard deviation is Gaussian distribution, that is

[0211] Using the covariance property, we can get the covariance Σ of the positioning result x. x :

[0212] Σ x =PΣ b P T

[0213] in, diag(·) is a diagonal matrix construction function, is the square of the ranging information The variance of , and:

[0214] In step 1.4, since all random errors are Gaussian white noise, it is assumed that the positioning results x of the vehicle radio navigation source all satisfy the three-dimensional Gaussian distribution; based on the results of steps 1.2 and 1.3, the positioning information probability function of the radio navigation source is calculated.

[0215] Through the above calculation and derivation process, we can finally obtain the vehicle's radio navigation source positioning result and its three-dimensional covariance matrix information probability model. Among them, μ RNS is the positioning result of RNS, Σ RNS is the covariance of the positioning results.

[0216] Since the random errors are all Gaussian white noise, it is assumed that the positioning results of the vehicle radio navigation source all satisfy the three-dimensional Gaussian distribution. According to the results of steps 1.2 and 1.3, the positioning information probability function of the radio navigation source is calculated as follows:

[0217]

[0218] where ζ is a constant term and

[0219] Step 1.5: Based on the calculation process of steps 1.1 to 1.4, obtain the positioning information probability function of the radio navigation sources of all vehicles.

[0220] (3) Taking satellite navigation source as an example:

[0221] Step 1.1, obtaining vehicle positioning parameter information based on navigation satellites, wherein the vehicle positioning parameter information includes the number and positions of navigation satellites involved in positioning, and pseudo-range information between the vehicle to be positioned and the navigation satellites;

[0222] Define the position coordinates of the vehicle to be located as p = (x, y, z) T , the number of satellites involved in 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 satellite is ρ=(ρ1,ρ2,…,ρ N ) T ;

[0223] Step 1.2: Based on the vehicle positioning parameter information and the Euclidean distance formula, a satellite navigation source solution equation group is established and solved to obtain the vehicle satellite navigation source positioning result;

[0224] The navigation source solution equations are:

[0225]

[0226] Where c is the speed of light, t u is the advance of the vehicle receiver clock relative to the satellite clock;

[0227] And solve the vehicle satellite navigation source positioning result μ=(x,y,z) T ,

[0228] Set the approximate position of the vehicle to be located to True position (x, y, z) and approximate position The offset between them is (Δx, Δy, Δz), and the receiver clock difference offset prediction value is Δt u ;

[0229] The single pseudorange value ρ i Expressed as:

[0230]

[0231] Using approximate location and the time bias is estimated to be get:

[0232]

[0233] For f(x,y,z,t u )exist Performing Taylor series expansion at , and retaining only the first-order partial derivatives, we can obtain:

[0234]

[0235] in,

[0236] definition:

[0237]

[0238] Where Δρ i Represents the difference between the approximate pseudorange value and the pseudorange value, a xi 、a yi and a zi They represent the direction cosines of the unit vector pointing from the approximate position to the i-th navigation satellite; for the i-th navigation satellite, the unit vector is defined as a i =(axi ,a yi ,a zi );

[0239] Combining the above formulas, we get:

[0240]

[0241] definition:

[0242]

[0243]

[0244] Then we get: Δρ=HΔx;

[0245] Δx, Δy, Δz and Δt are calculated using the least squares method. u , according to Δx, Δy, Δz and Δt u Calculate the real coordinates (x, y, z) of the vehicle to be positioned and the advance t of the vehicle receiver clock relative to the navigation source clock u , then the vehicle satellite navigation source positioning result μ=(x,y,z) T

[0246] Step 1.3: Consider that in a short period of time, the matrix H is a coefficient matrix that depends only on the geometric distribution of the vehicle or satellite navigation source, and the matrix H is considered to be a known parameter;

[0247] The three-dimensional covariance matrix corresponding to the vehicle satellite navigation source positioning result is calculated as follows:

[0248] Σ Δ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 ;

[0249] In step 1.4, since the random errors are all Gaussian white noise, it is assumed that the vehicle satellite navigation source positioning results all satisfy the three-dimensional Gaussian distribution; according to the results of steps 1.2 and 1.3, the positioning information probability function of the satellite navigation source is calculated as follows:

[0250]

[0251] where ζ is a constant term and

[0252] Step 1.5: According to the calculation process of steps 1.1 to 1.4, the positioning information probability function of all vehicle satellite navigation sources is obtained.

[0253] To summarize, we express the positioning information probability function of satellite navigation source, radio navigation source, and inertial navigation source as:

[0254]

[0255] in, The positioning results of various navigation sources are represented by μ = (x, y, z) T , the covariance matrix of the positioning result is Σ.

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

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

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

[0259] Since random errors are all Gaussian white noise, it can be approximately assumed that the positioning results of various navigation sources all satisfy the three-dimensional Gaussian distribution, and its mean is μ = (x, y, z) T , the covariance matrix is Σ, then the information probability function of the navigation source is It can be expressed as:

[0260]

[0261] where ζ is a constant term and

[0262] Step 2: Fuse the navigation source positioning results of each vehicle to calculate the single-vehicle fusion positioning result and its three-dimensional covariance matrix, as follows:

[0263] First, the navigation source positioning results on the vehicle and its three-dimensional covariance matrix are constructed as a three-dimensional Gaussian probability density function;

[0264] For the vehicle to be positioned, the number of navigation sources that can obtain navigation positioning information is set to N, and the probability density function of the i-th positioning information is function p i (·), the corresponding weight factor in the fusion is expressed as a i , the fused information density function is expressed as p f (·).

[0265] The fusion criterion formula is:

[0266]

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

[0268]

[0269] Then, the KL divergence of the probability density function of the positioning result on the Riemann manifold is calculated through information geometry theory to approximate its Fisher information distance.

[0270] Since the navigation source positioning information all satisfies the three-dimensional Gaussian distribution and is independent of each other, we can obtain:

[0271]

[0272] Substituting in:

[0273]

[0274] Among them, μ i is the positioning result of the i-th navigation source, Σ i is the covariance matrix of the i-th navigation source, μ f is the positioning result after fusion of multiple navigation sources, Σ f is the covariance matrix after fusion of multiple navigation sources, and

[0275]

[0276] By comparison, we can get that p f (x|μ f ,Σ f ) satisfies the three-dimensional Gaussian distribution, and the parameter information satisfies:

[0277]

[0278] The solution is:

[0279]

[0280] Finally, the point in the Riemann manifold with the shortest sum of Fisher information distances to each navigation source is selected, and the mean and covariance matrix of the probability density function corresponding to this point are the single-vehicle fusion positioning result and its three-dimensional covariance matrix.

[0281] Step 3: Obtain the distance and direction measurement information between vehicles, build a distributed collaborative positioning model of the vehicle cluster factor graph, and combine it with the single vehicle fusion positioning results in step 2 to finally obtain the collaborative positioning results and achieve real-time high-precision positioning of the vehicle cluster. The details are as follows:

[0282] The constructed vehicle cluster distributed collaborative positioning model is as follows Figure 4 As shown in the figure, the core part is to obtain the internal factor graph model of the vehicle to be located through the distance and direction measurement information between vehicles, and then use the sum-product algorithm to transfer the confidence. Since the confidence update method of each vehicle to be located in this method is the same, any vehicle to be located M in the vehicle cluster network will be used. q Take this as an example to introduce.

[0283] The vehicle M to be positioned is obtained from the single vehicle fusion positioning result in step 2 q The initial information after the single vehicle multi-source fusion positioning is (μ q0 ,Σ q0 ), assume that there are n cooperative vehicles, each with M i , i=1,2,…,n, their initial position information is represented by (μ i ,Σ i ) is used to represent it. Where μ represents the positioning result, Σ represents the covariance matrix of the positioning result. q Establish a spherical coordinate system for the origin, and coordinate vehicle M i to M q The distance information can be expressed as where r i is the ranging information, satisfying the mean value of r i , the variance is Gaussian distribution, is the point passing through the Z axis and point M i The angle formed by the half plane of and the coordinate plane ZOX, and θ i is line segment M q M i The angle with the positive direction of the Z axis, and 0≤θ i ≤π. Vehicle M to be located q and cooperative vehicle M i The position relationship diagram is as follows Figure 5 shown.

[0284] pass Figure 5 The position relationship in can be obtained:

[0285]

[0286] Vehicle M to be located q The internal factor graph model of Figure 6 As shown in the figure, the information transfer on the vehicle internal factor graph is calculated by the information transfer criterion based on the sum-product algorithm, and after completing the uplink iterative transfer and downlink iterative transfer of the confidence information, the vehicle M to be located is obtained. q The following takes the kth iteration as an example to analyze the confidence information transmission between function nodes and variable nodes.

[0287] (1) Initialization;

[0288] Initialization mainly collects the input parameter information participating in the kth iteration to provide guarantee for the next iteration.

[0289] The collected parameter information includes the vehicle M to be positioned q The k-1th positioning information Cooperative vehicle M i The k-1th positioning information and ranging information

[0290] (2) Upward iteration

[0291] The uplink iteration mainly uses collaborative vehicles to predict the vehicles to be positioned, and then fuses these predicted values with the navigation source positioning results to obtain a more accurate positioning result, which will be used as the initial value for the next iteration.

[0292] The upward iteration is divided into three steps:

[0293] First, through the function equation C i Distance measurement information The information is processed and converted into the coordinated vehicle M in the Euclidean space. i and the vehicle M to be positioned q Coordinate difference information The functional equation C i for:

[0294]

[0295] Then through the function equation B i For cooperative vehicle M i Location information and coordinate difference information Processing is performed to obtain the cooperative vehicle M iThe vehicle M to be positioned q Prediction information The functional equation B i for:

[0296]

[0297] Finally, the vehicle M to be located is q Prediction information and initial value Perform fusion to obtain the vehicle M to be positioned q The initial value of the next iteration

[0298] The functional equation A is:

[0299]

[0300] (3) Downward iteration

[0301] The main purpose of the downlink iteration is to correct the directional information in the ranging information to provide higher accuracy for the next iteration prediction. This correction operation plays a key role in improving overall positioning accuracy.

[0302] Corresponding to the upward iteration, the downward iteration is also divided into three steps:

[0303] First, the feedback information is obtained by calculating the function equation A′

[0304] The functional equation A′ is:

[0305]

[0306] Then through function node B i Feedback and cooperative vehicle M i Location information Processing is performed to obtain the coordinate difference information of the downlink iteration

[0307]

[0308] Finally, through the function equation C i 'right Process it and convert it into the initial value of the ranging information for the next iteration The functional equation C i 'for:

[0309]

[0310]

[0311] After obtaining preliminary collaborative localization results, these results are used to iteratively update the positioning and ranging information in the factor graph. In each iteration, the positioning and ranging results of each vehicle in the cluster are synchronously updated to ensure real-time and consistent positioning information. The iteration terminates when the preset maximum number of convergences is reached or the convergence condition is met, resulting in a multi-source fusion collaborative localization result for all vehicles in the cluster network.

[0312] The method of the present invention was used to simulate the vehicle cluster in the embodiment, and compared with the current mainstream fusion methods including unscented Kalman filter (UKF) and BP neural network (BPNN). The root mean square error (RMSE) was used to calculate the positioning accuracy. The simulation scene was set as a circular overpass with a slope, the average circular radius was 100m, and the maximum height difference was 30m. In this simulation scene, there were 40 vehicles driving on the lane, and their initial positions were randomly distributed in the simulation scene. The schematic diagram of the simulation scene of the vehicle cluster collaborative positioning experiment is shown in the figure below. Figure 7 As shown. Set the vehicle speed to between 15-20m / s, the maximum communication distance between vehicles to 100m, and the standard deviation of the ranging error to σ r =0.1m, the standard deviation of pseudorange error σ1 is 1.0m, the standard deviation of radio ranging error σ2 is 0.2m, the total simulation time is 30s, the minimum interval time of positioning output is 0.5s, and the number of Monte Carlo times is 10000.

[0313] The simulation results of the accuracy comparison experiment under ideal scenarios are as follows: Figure 8 As shown. Figure 8 It can be seen that under the same simulation conditions, the method of the present invention demonstrates significant advantages in positioning accuracy, with an average positioning error of only 0.251m. By comparing the positioning accuracy of various methods, it can be seen that the method of the present invention significantly improves the overall positioning accuracy of the cluster network by utilizing information geometry fusion and distributed factor graph networks. In addition, it can be observed that the fluctuation range of the average positioning accuracy of the method of the present invention is approximately equal to that of the BPNN method and less than that of the UKF method, indicating that the method of the present invention also has the characteristics of strong stability.

[0314] The simulation results of the convergence speed comparison experiment under ideal scenarios are as follows: Figure 9 As shown. Figure 9As can be seen from the figure, under the given simulation conditions, the method of the present invention achieves the fastest convergence speed, approximately 0.15 seconds. This demonstrates that the method of the present invention, by processing heterogeneous navigation source data in the form of information probability and simultaneously constructing a distributed factor graph model, effectively reduces algorithmic complexity, ensuring high accuracy while meeting real-time requirements. Furthermore, by comparing the change in positioning error before and after collaborative positioning, it can be seen that the method of the present invention achieves the largest change in error. This simulation result strongly demonstrates that, among the three multi-source fusion positioning methods, the method of the present invention has the most significant effect on improving positioning accuracy.

[0315] The positioning stability of the vehicle cluster positioning in the embodiment is simulated and tested in the face of sudden error using the method of the present invention. Figure 10 As shown in the figure, when the time is set to t = 3 to 7s, the standard deviation of the pseudorange error σ1 changes from 1.0m to 1.5m, when the time is set to t = 13 to 17s, the standard deviation of the radio ranging error σ2 changes from 0.2m to 0.3m, and when the time is set to t = 23 to 27s, the standard deviation of the vehicle ranging error σ r The distance is changed from 0.1m to 0.2m, and other parameter settings are consistent with the ideal scenario.

[0316] Depend on Figure 10 It can be seen that when mutation errors occur during positioning, the positioning accuracy of the three methods is affected to varying degrees. A comparison reveals that, under the same mutation error, the method of the present invention exhibits the best positioning accuracy, the smallest error increment, and the fastest convergence speed. This demonstrates that, by implementing distributed collaborative positioning, the method of the present invention significantly reduces the impact of mutation errors on the collaborative positioning accuracy of vehicle clusters, demonstrating good robustness.

[0317] The above description is only a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any technician familiar with this technical field can easily think of various equivalent modifications or replacements within the technical scope disclosed in the present invention, and these modifications or replacements should all be included in the scope of protection of the present invention.

Claims

1. A multi-source fusion collaborative positioning method for vehicle clusters based on information geometry, characterized by: The following steps are involved: Step 1: Build a vehicle cluster heterogeneous multi-source fusion collaborative positioning system model, obtain vehicle positioning parameter information through different navigation sources, calculate the vehicle navigation source positioning results and the corresponding three-dimensional covariance matrix, and obtain the navigation source information density function of all vehicles; The vehicle cluster heterogeneous multi-source fusion collaborative positioning system model includes a vehicle to be positioned, several collaborative vehicles and multiple navigation source devices; The vehicles are connected to each other and to the navigation source via ranging and direction finding communication links; Step 2: Fusion calculates the vehicle navigation source information density function obtained in step 1 to obtain the single-vehicle multi-navigation source fusion positioning results and corresponding three-dimensional covariance matrices of the vehicle to be positioned and the cooperative vehicle respectively; Step 3: Based on the ranging and direction information between the vehicle to be located and the cooperative vehicle, as well as the single-vehicle multi-navigation source fusion positioning results of the vehicle to be located and the cooperative vehicle obtained in step 2 and the corresponding three-dimensional covariance matrix, a vehicle cluster factor graph collaborative positioning model is constructed, the internal factor graph model of the vehicle to be located is obtained, and the confidence information of the internal factor graph model parameters is iteratively updated to obtain the multi-source fusion collaborative positioning result of the vehicle to be located.

2. The vehicle cluster multi-source fusion collaborative positioning method based on information geometry according to claim 1 is characterized in that: In step 1, the navigation source equipment includes an inertial navigation source, a radio navigation source, and a satellite navigation source.

3. The vehicle cluster multi-source fusion collaborative positioning method based on information geometry according to claim 2 is characterized in that: In step 1, if the navigation source device is an inertial navigation source, the process of calculating the vehicle's inertial navigation source positioning result and the corresponding three-dimensional covariance matrix to obtain the inertial navigation source information density function of all vehicles is as follows: Step 1.1: Obtain vehicle positioning parameter information based on the inertial navigation source and establish the vehicle coordinate system; The vehicle positioning parameter information includes vehicle posture, 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 , the posture of the vehicle to be positioned is G B =(φ,θ,ψ) T , the output of the accelerometer is f B , the static zero bias of the accelerometer and gyroscope are b a =(b ax ,b ay ,b az ) T and b ω =(b ωφ ,b ωθ ,b ωψ ) T The standard deviation of the accelerometer noise and the standard deviation of the gyroscope noise are and The sampling frequency is 1 / T s , the calibration period is T; The established vehicle coordinate system includes the Northeast Celestial coordinate system, the carrier coordinate system, the Earth coordinate system and the Earth-centered Earth-fixed coordinate system. The Northeast Celestial coordinate system is defined as the N system, the carrier coordinate system is referred to as the B system, the Earth coordinate system is referred to as the I system, and the Earth-centered Earth-fixed coordinate system is referred to as the E system. Step 1.2: Construct the inertial navigation source positioning update equation group, and substitute the vehicle positioning parameter information obtained in step 1.1 into the inertial navigation source positioning update equation group for update calculation to obtain the positioning result p of the vehicle to be positioned. t ; The inertial navigation source positioning update equation group 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: in, is the attitude transfer matrix from the B system to the N system, for The differential of is the projection of the rotation angular velocity of the B system relative to the N system in the B system, for The antisymmetric matrix is formed, 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, for The error, that is, the attitude calculation error of the N system, is the projection of the rotation angular rate of system B relative to system I in system B, that is, the output value of the gyroscope, for The error is the measurement error of the gyroscope; The velocity update differential equation is: Among them, v N is the projection of vehicle speed in the N system, v N The differential of the vehicle acceleration, i.e. the projection of the vehicle acceleration in the N system, 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 system relative to the I system caused by the rotation of the Earth in the N system, is the projection of the rotation angular rate 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 gravitational acceleration in the N system respectively; The velocity update equation is: Among them, v t+Δt is the speed of the moving vehicle at time t+Δt, v t is the speed of the moving vehicle at time t, is the velocity differential, i.e. acceleration, and Δt is the time interval; The velocity error differential equation is: in, is the projection of the vehicle speed error differential in the N system, f N is the projection of the accelerometer measurement value in the N system, and represents the antisymmetric matrix of the accelerometer measurements in the N system, and They represent the specific force measurement error, velocity error, Earth rotation angular rate calculation error, navigation system rotation calculation error, and gravity error in the N system respectively; The position update differential equation is: Among them, p N is the projection of the vehicle position in the N system, For p N The differential of v N is the projection of vehicle speed in the N system; The position update equation is: Among them, 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; The position error differential equation is: in, is the projection of the vehicle position error differential in the N system, δv N is the velocity error of the vehicle in the northeast celestial coordinate system; Step 1.3, calculate the inertial navigation source measurement error at each moment; First, establish the mathematical model of the accelerometer and gyroscope: in: and Represent the output values of the accelerometer and gyroscope in the B frame, f B and ω B They represent the true values of acceleration and attitude angular velocity in frame B, S a and S ω Represent the scale factor errors of the accelerometer and gyroscope, N a and N ω are the non-orthogonal errors of the accelerometer and gyroscope, respectively, and b a and b ω Denote the zero bias of the accelerometer and gyroscope, ε a and ε ω represent the random noise of accelerometer and gyroscope respectively; Then, according to the mathematical model and the inertial navigation source positioning update equation group, the inertial navigation source measurement error at any time is calculated, where the inertial navigation source measurement error is the sum of the zero bias error and the random noise error; Assume that the vehicle carrying the inertial navigation source is stationary, the accelerometer and gyroscope noises are independent of each other, and the accelerometer bias and attitude error are independent of each other. In this state: The bias error is in: and are the zero bias of the accelerometer and the zero bias of the gyroscope, respectively, and t is the integration time; The random noise error is in: and are the standard deviation of accelerometer noise and gyroscope noise, t is the integration time, and δt is the sampling interval; Step 1.4: Correct the current navigation source measurement error based on the previous navigation source measurement error, and calculate the vehicle inertial navigation source positioning result and the corresponding three-dimensional covariance matrix according to the formula; Set tT after inertial navigation source calibration s The inertial navigation source error model at time The error model of the inertial navigation source at time t after the inertial navigation source is calibrated is: Among them, [f N ] × represents the antisymmetric matrix formed by the accelerometer measurements in the N-frame, and diag(·) is the diagonal matrix construction function; tT s Time inertial navigation source error; μ t is the inertial navigation source error at time t; tT s Covariance matrix of inertial navigation source error at time; ∑ t is the covariance matrix of the inertial navigation source error at time t; Definition: tT after inertial navigation source calibration s At this moment, the posture of the vehicle to be positioned G B =(φ,θ,ψ) T , the accelerometer measurement value is f B , the static zero bias of the accelerometer and gyroscope are b a =(b ax ,b ay ,b az ) T and b ω =(b ωφ ,b ωθ ,b ωψ ) T The standard deviation of the accelerometer noise and the standard deviation of the gyroscope noise are and The sampling frequency is 1 / T s , the calibration period is T, the attitude transfer matrix The posture G of the vehicle to be positioned B Calculate and determine; Then the vehicle inertial navigation source positioning result at time t after the inertial navigation source is calibrated is obtained The corresponding three-dimensional covariance matrix In step 1.5, since the random errors are all Gaussian white noise, it is assumed that the vehicle inertial navigation source positioning results x all satisfy the three-dimensional Gaussian distribution; according to the results of steps 1.2 and 1.3, the positioning information probability function of the navigation source is obtained as follows: where ζ is a constant term, and Step 1.6: According to the calculation process of steps 1.1 to 1.5, the probability function of the positioning information of the inertial navigation source of each vehicle is obtained.

4. The vehicle cluster multi-source fusion collaborative positioning method based on information geometry according to claim 1 is characterized in that: The fusion process of step 2 is: Step 2.1: Based on the positioning information probability functions of each vehicle corresponding to different types of navigation sources, a three-dimensional Gaussian probability density function of the fusion positioning results of each vehicle's own navigation source is constructed; Definition: The number of types of navigation sources corresponding to the vehicle is N, and the probability density function of the i-th positioning information is p i (·), the corresponding weight factor in the fusion is a i , the information density function after fusion is p f (·); The fusion criterion formula is: In the formula, arg inf represents the maximum value point of the maximum likelihood function, represents the Kullback-Leibler distance between two information probability density functions, and Step 2.2: Calculate the KL divergence of the three-dimensional Gaussian probability density function on the Riemannian manifold using information geometry theory to obtain the Fisher information distance from each point on the Riemannian manifold to each navigation source. In step 2.3, select the point in the Riemann manifold with the shortest sum of Fisher information distances to each navigation source, and take the mean and covariance matrix of the probability density function corresponding to this point as the single-vehicle multi-navigation source fusion positioning result and the corresponding three-dimensional covariance matrix.

5. The vehicle cluster multi-source fusion collaborative positioning method based on information geometry according to claim 4 is characterized in that: In step 3, the specific process of obtaining the multi-source fusion collaborative positioning results of the vehicle to be positioned and the cooperative vehicle by iteratively updating the confidence information of the internal factor graph model of the vehicle to be positioned is as follows: Step 3.1: Based on the single-vehicle multi-navigation source fusion positioning results of the vehicle to be positioned and the cooperative vehicle and the corresponding three-dimensional covariance matrix, the initial information of the vehicle to be positioned (μ q0 ,∑ q0 ) and collaborative initial position information (μ i ,∑ i ); To be positioned vehicle M q Establish a three-dimensional spherical coordinate system as the origin and define the cooperative vehicle M i To the vehicle M to be located q Distance information where r i is the ranging information, satisfying the mean value of r i , the variance is Gaussian distribution; To pass through the Z axis and the cooperative vehicle M i The angle formed by the half plane of and the coordinate plane ZOX, and θ i is line segment M q M i The angle with the positive direction of the Z axis, and 0≤θ i ≤π; According to the three-dimensional coordinate relationship, we can get Step 3.2: Collect the input parameters of the kth iteration involved in the confidence information update, including the vehicle M to be located q Positioning information output at the k-1th iteration Cooperative vehicle M i Positioning information output at the k-1th iteration and ranging information Step 3.3, Upward Iteration: First, through the function equation C i Distance measurement information The information is processed and converted into the coordinated vehicle M in the Euclidean space. i and the vehicle M to be positioned q Coordinate difference information The functional equation C i for: Then, through the function equation B i For cooperative vehicle M i Location information and coordinate difference information Processing is performed to obtain the cooperative vehicle M i The vehicle M to be positioned q Prediction information The functional equation B i for: Finally, the vehicle M to be located is transformed into q Prediction information and initial value Perform fusion to obtain the vehicle M to be positioned q The initial value of the next iteration The functional equation A is: Step 3.4, downward iteration: First, the feedback information is obtained by calculating the function equation A′ The functional equation A′ is: Then, through the function equation B i Feedback information and cooperative vehicle M i Location information Processing is performed to obtain the coordinate difference information of the downlink iteration The functional equation B i 'for: Finally, through the function equation C i 'right Process it and convert it into the initial value of the ranging information for the next iteration The functional equation C i 'for: In step 3.5, the confidence is updated according to steps 3.1 to 3.

4. When the preset maximum number of convergences is reached or the convergence condition is met, the iteration is terminated and the multi-source fusion collaborative positioning result of the vehicle to be located is obtained.

Images