Vehicle positioning method, system and device based on motion constraint adaptive filtering, medium and product

Abstract

The invention discloses a vehicle positioning method, system and device based on motion constraint adaptive filtering, a medium and a product, and relates to the technical field of vehicle-mounted navigation positioning, and the method comprises the steps: constructing a virtual dead reckoning state model and a virtual dead reckoning measurement model based on historical preprocessing data, determining a virtual dead reckoning state vector, and obtaining a virtual dead reckoning state vector; determining an INS mounting angle and an NHC lever arm; based on INS measurement data, a combined system state model and a mechanical arrangement model are constructed, and a compensation INS installation angle and the three-dimensional position and the three-dimensional speed of an NHC lever arm are determined; obtaining original data of the current epoch; when the original data does not contain the GNSS measurement data, determining a three-dimensional position after filtering updating according to a first positioning process; and when the original data contains the GNSS measurement data, determining the filtered and updated three-dimensional position of the vehicle according to a second positioning process. According to the invention, the positioning precision of the vehicle in the GNSS denial environment is improved.

Description

Technical Field

[0001] The present application relates to the field of vehicle navigation and positioning technology, and in particular to a vehicle positioning method system, device, medium and product based on motion constraint adaptive filtering. Background Art

[0002] Continuous, reliable and real-time position information is a necessary condition for vehicle navigation. Combined navigation based on the Global Navigation Satellite System (GNSS) and the Inertial Navigation System (INS) is one of the most widely used positioning solutions, which can provide accurate position information for vehicles. However, in complex environments such as tunnels, overpasses, and urban canyons, GNSS signals are easily blocked and interfered, resulting in discontinuous and unreliable positioning. Without adding additional sensors, nonholonomic constraints (NHC), an effective and low-cost vehicle-mounted GNSS / INS enhancement technology, can greatly limit the error accumulation rate of INS in GNSS-denied environments. However, accurate compensation of the installation angle and arm and reasonable configuration of noise parameters are prerequisites for the full play of NHC. The use of imprecise parameter configuration will reduce the accuracy of vehicle-mounted positioning.

[0003] However, in practical applications, there are complex nonlinear coupling relationships between INS installation angle, NHC arm and model noise parameters, road conditions, vehicle motion state, observation environment and INS installation position, and it is difficult to obtain the optimal configuration solution by manual parameter adjustment alone. How to estimate the INS installation angle and NHC arm and accurately determine the noise statistics of NHC during information fusion greatly limits the potential of implicit information in improving the navigation accuracy of the combined system, resulting in low positioning accuracy of GNSS for vehicles in denied environments. Summary of the invention

[0004] The purpose of this application is to provide a vehicle positioning method system, device, medium and product based on motion constraint adaptive filtering to solve the problem of low positioning accuracy of GNSS for vehicles in a denied environment.

[0005] To achieve the above objectives, this application provides the following solutions:

[0006] In a first aspect, the present application provides a vehicle positioning method based on motion constraint adaptive filtering, comprising:

[0007] Acquire historical measurement data, and pre-process the historical measurement data to obtain historical pre-processed data; the historical measurement data includes: GNSS measurement data and INS measurement data of multiple historical epochs; the GNSS measurement data includes: multi-frequency multi-mode pseudorange and carrier phase measurement values ​​measured by the GNSS receiver, and the INS measurement data includes: three-dimensional velocity increment measured by the accelerometer in the INS and three-dimensional angle increment measured by the gyroscope in the INS; the pre-processing includes: quality assessment and integrated navigation solution; the historical pre-processed data includes: GNSS / INS navigation information of multiple historical epochs, and the GNSS / INS navigation information includes: three-dimensional position increment, three-dimensional position, pitch angle, roll angle and heading angle of the vehicle;

[0008] Based on the historical preprocessed data, a virtual dead reckoning state model and a virtual dead reckoning measurement model are constructed;

[0009] Determine a virtual dead reckoning state vector based on a virtual dead reckoning state model and a virtual dead reckoning measurement model by using a Kalman filter;

[0010] Based on the virtual dead reckoning state vector, determine the INS installation angle and NHC lever arm; the INS installation angle is the Euler angle from the vehicle system to the carrier system, and the NHC lever arm is the distance vector from the center of the INS to the effective point of the NHC;

[0011] Acquire raw data of the current epoch; the raw data includes: GNSS measurement data and / or INS measurement data;

[0012] Based on INS measurement data, build a combined system state model and mechanical arrangement model;

[0013] Determine the 3D position and 3D velocity of the vehicle's compensated INS mounting angle and NHC lever arm based on the INS mounting angle, NHC lever arm and mechanical arrangement model;

[0014] When the original data does not contain GNSS measurement data, the three-dimensional position of the vehicle after filtering and updating is determined according to the first positioning process to achieve vehicle positioning; wherein the first positioning process includes:

[0015] Construct the NHC measurement model based on the vehicle's compensated INS installation angle and the three-dimensional velocity and nominal velocity of the NHC lever arm;

[0016] Based on the NHC measurement model, the measurement noise covariance matrix is ​​reconstructed by using the inverse gamma distribution, and the NHC measurement model including the reconstructed measurement noise covariance matrix is ​​obtained;

[0017] Based on the combined system state model and the NHC measurement model including the reconstructed measurement noise covariance matrix, the combined system state vector is determined by using variational Bayesian adaptive filtering.

[0018] determining a filtered updated three-dimensional position of the vehicle based on the combined system state vector and the compensated INS mounting angle of the vehicle and the three-dimensional position of the NHC lever arm;

[0019] When the original data includes GNSS measurement data, the filtered updated three-dimensional position of the vehicle is determined according to the second positioning process to achieve vehicle positioning; wherein the second positioning process includes:

[0020] A combined system measurement model is constructed based on the vehicle’s compensated INS mounting angle and the three-dimensional position of the NHC arm and the GNSS measurement data in the raw data;

[0021] Based on the combined system state model and the combined system measurement model, the Kalman filter is used to determine the combined system state vector;

[0022] A filtered updated three-dimensional position of the vehicle is determined based on the combined system state vector and the compensated INS mounting angle of the vehicle and the three-dimensional position of the NHC lever arm.

[0023] Optionally, the virtual dead reckoning state model includes:

[0024]

[0025] in, represents δP at epoch k n ,δP n It represents the three-dimensional position error of the vehicle in the n-frame, where the n-frame is the navigation frame; represents the δP of the k-1 epoch n ; represents the k-1 epoch represents the direction cosine matrix from b system to n system, where b system is the carrier system; represents the k-1 epoch represents the direction cosine matrix from v to b; M represents the auxiliary matrix; α k-1 represents the α of the k-1 epoch, where α represents the pitch installation angle error and heading installation angle error of the INS; represents the three-dimensional position increment of the vehicle in the n-frame at the k-epoch; φ k-1 represents the φ of the k-1 epoch, where φ represents the pitch misalignment angle error, roll misalignment angle error, and heading misalignment angle error of the vehicle; δk k-1 represents δk of the k-1 epoch, where δk represents the virtual odometer scale factor error.

[0026] Optionally, the virtual dead reckoning measurement model includes:

[0027]

[0028] Among them, zDR,k represents the measurement vector of the virtual dead reckoning model at epoch k; represents P of epoch k obtained by the position recursion equation n , P n Represents the three-dimensional position of the vehicle in the n-frame; P represents the k-epoch P of the historical preprocessed data n ; l represents the k-1 epoch b , l b represents the NHC lever arm; × represents the vector cross product operation; represents δl at epoch k b , δl b Indicates the NHC lever arm error; e DR,k H represents the measurement noise vector of the virtual dead reckoning measurement model at epoch k; DR,k represents the coefficient matrix of the virtual dead reckoning measurement model at epoch k; x k represents x at epoch k, and x represents the combined system state vector.

[0029] Optionally, the combined system state model includes:

[0030]

[0031] in, represents the differential of φ; It represents the three-dimensional rotation angular velocity of system n relative to system i, where system i is an inertial system; express The calculation error of Represents the three-dimensional measurement error of the gyroscope; Indicates δv n The differential of δv n represents the three-dimensional velocity error of the vehicle in the n system; f b Represents the three-dimensional velocity increment measured by the accelerometer; express The calculation error of represents the three-dimensional angular velocity of the earth's rotation in the n system; express The calculation error of represents the angular velocity of the three-dimensional carrier motion in the n system; δf b Indicates the three-dimensional measurement error of the accelerometer; δg n Represents the three-dimensional gravity error; Denotes δP n The differential of .

[0032] Optionally, the mechanical arrangement model includes:

[0033]

[0034] in, represents the P of the k-1 epoch obtained by the position recursion equation n ; represents the three-dimensional velocity of the vehicle in the n-frame at the k-epoch; τ represents the sampling interval of INS; represents the three-dimensional velocity of the vehicle in the n-frame at the k-1 epoch; represents the integral term of the specific force at epoch k; represents the harmful acceleration integral term at epoch k; represents the kth epoch represents the direction cosine matrix from epoch k-1 to epoch k in the n system; Represents the direction cosine matrix from epoch k to epoch k-1 in frame b.

[0035] Optionally, the calculation formula for the three-dimensional velocity of the compensated INS installation angle and the NHC lever arm of the vehicle in the v system includes:

[0036]

[0037] in, represents the three-dimensional velocity of the compensated INS installation angle and NHC lever arm of the vehicle at epoch k in the v system, where the v system is the vehicle system; represents the direction cosine matrix from b to v calculated according to the INS installation angle; represents the three-dimensional angle increment measured by the gyroscope at epoch k; represents the kth epoch

[0038] In a second aspect, the present application provides a vehicle positioning system based on motion constraint adaptive filtering, which is used to implement any of the above-mentioned vehicle positioning methods based on motion constraint adaptive filtering, and the vehicle positioning system based on motion constraint adaptive filtering includes:

[0039] The data acquisition module is used to acquire historical measurement data and pre-process the historical measurement data to obtain historical pre-processed data; the historical measurement data includes: GNSS measurement data and INS measurement data of multiple historical epochs; the GNSS measurement data includes: multi-frequency multi-mode pseudorange and carrier phase measurement values ​​measured by the GNSS receiver, and the INS measurement data includes: three-dimensional velocity increment measured by the accelerometer in the INS and three-dimensional angle increment measured by the gyroscope in the INS; the pre-processing includes: quality assessment and integrated navigation solution; the historical pre-processed data includes: GNSS / INS navigation information of multiple historical epochs, and the GNSS / INS navigation information includes: three-dimensional position increment, three-dimensional position, pitch angle, roll angle and heading angle of the vehicle;

[0040] A first model building module, for building a virtual dead reckoning state model and a virtual dead reckoning measurement model based on historical preprocessing data;

[0041] A virtual dead reckoning state vector determination module, for determining a virtual dead reckoning state vector based on a virtual dead reckoning state model and a virtual dead reckoning measurement model by using a Kalman filter;

[0042] INS installation angle and NHC lever arm determination module, used to determine the INS installation angle and NHC lever arm based on the virtual dead reckoning state vector; the INS installation angle is the Euler angle from the vehicle system to the carrier system, and the NHC lever arm is the distance vector from the center of the INS to the effective point of the NHC;

[0043] A raw data acquisition module is used to obtain raw data of the current epoch; the raw data includes: GNSS measurement data and / or INS measurement data;

[0044] The second model building module is used to build a combined system state model and a mechanical arrangement model based on INS measurement data;

[0045] A velocity compensation module for determining the three-dimensional position and three-dimensional velocity of the vehicle's compensated INS mounting angle and NHC lever arm based on the INS mounting angle, NHC lever arm and mechanical arrangement model;

[0046] The first positioning module is used to determine the filtered updated three-dimensional position of the vehicle according to the first positioning process to achieve vehicle positioning when the original data does not contain GNSS measurement data; wherein the first positioning process includes:

[0047] Construct the NHC measurement model based on the vehicle's compensated INS installation angle and the three-dimensional velocity and nominal velocity of the NHC lever arm;

[0048] Based on the NHC measurement model, the measurement noise covariance matrix is ​​reconstructed by using the inverse gamma distribution, and the NHC measurement model including the reconstructed measurement noise covariance matrix is ​​obtained;

[0049] Based on the combined system state model and the NHC measurement model including the reconstructed measurement noise covariance matrix, the combined system state vector is determined by using variational Bayesian adaptive filtering.

[0050] determining a filtered updated three-dimensional position of the vehicle based on the combined system state vector and the compensated INS mounting angle of the vehicle and the three-dimensional position of the NHC lever arm;

[0051] The second positioning module is used to determine the filtered updated three-dimensional position of the vehicle according to the second positioning process to achieve vehicle positioning when the original data contains GNSS measurement data; wherein the second positioning process includes:

[0052] A combined system measurement model is constructed based on the vehicle’s compensated INS mounting angle and the three-dimensional position of the NHC arm and the GNSS measurement data in the raw data;

[0053] Based on the combined system state model and the combined system measurement model, the Kalman filter is used to determine the combined system state vector;

[0054] A filtered updated three-dimensional position of the vehicle is determined based on the combined system state vector and the compensated INS mounting angle of the vehicle and the three-dimensional position of the NHC lever arm.

[0055] In a third aspect, the present application provides a computer device, comprising: a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement any of the above-described vehicle positioning methods based on motion constraint adaptive filtering.

[0056] In a fourth aspect, the present application provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements any of the above-mentioned vehicle positioning methods based on motion constraint adaptive filtering.

[0057] In a fifth aspect, the present application provides a computer program product, including a computer program, which, when executed by a processor, implements any of the above-mentioned vehicle positioning methods based on motion constraint adaptive filtering.

[0058] According to the specific embodiments provided in this application, this application discloses the following technical effects:

[0059] The present application discloses a vehicle positioning method system, device, medium and product based on motion constraint adaptive filtering. First, a series of processing and modeling are performed on the historical pre-processed data to determine the INS installation angle and the NHC lever arm. Then, based on the INS measurement data, a combined system state model and a mechanical arrangement model are constructed, and based on the INS installation angle, the NHC lever arm and the mechanical arrangement model, the three-dimensional position and three-dimensional velocity of the vehicle's compensated INS installation angle and the NHC lever arm are determined. Finally, when the vehicle is positioned at the current epoch: when the original data contains GNSS measurement data, a combined system measurement model is constructed based on the vehicle's compensated INS installation angle and the three-dimensional position of the NHC lever arm and the GNSS measurement data in the original data; based on the combined system state model and the combined system measurement model, a Kalman filter is used to determine the combined system state vector; based on the combined system state vector and the vehicle's compensated INS installation angle and the three-dimensional position of the NHC lever arm, the filtered updated three-dimensional position of the vehicle is determined. When the original data does not contain GNSS measurement data, the NHC measurement model is constructed based on the vehicle's compensated INS installation angle and the three-dimensional speed and nominal speed of the NHC arm. Based on the NHC measurement model, the measurement noise covariance matrix is ​​reconstructed using the inverse gamma distribution to obtain the NHC measurement model containing the reconstructed measurement noise covariance matrix; based on the combined system state model and the NHC measurement model containing the reconstructed measurement noise covariance matrix, the combined system state vector is determined using variational Bayesian adaptive filtering; based on the combined system state vector and the vehicle's compensated INS installation angle and the three-dimensional position of the NHC arm, the vehicle's three-dimensional position after filtering is determined. Since the variational Bayesian method can adaptively adjust the NHC measurement noise covariance matrix based on prior information and observation information, it can improve the positioning accuracy and reliability of vehicles in GNSS-denied environments such as urban canyons, overpasses and tunnels. BRIEF DESCRIPTION OF THE DRAWINGS

[0060] In order to more clearly illustrate the technical solutions in the embodiments of the present application or related technologies, the drawings required for use in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0061] Figure 1 A schematic flow chart of a vehicle positioning method based on motion constraint adaptive filtering provided in an embodiment of the present application;

[0062] Figure 2 Schematic diagram of INS mounting angle and NHC lever arm;

[0063] Figure 3 A schematic diagram of the structure of a computer device provided in one embodiment of the present application. DETAILED DESCRIPTION

[0064] The following will be combined with the drawings in the embodiments of the present application to clearly and completely describe the technical solutions in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.

[0065] The purpose of this application is to provide a vehicle positioning method system, device, medium and product based on motion constraint adaptive filtering, aiming to improve the positioning accuracy of vehicles in GNSS denied environments.

[0066] In order to make the above-mentioned objects, features and advantages of the present application more obvious and easy to understand, the present application is further described in detail below in conjunction with the accompanying drawings and specific implementation methods.

[0067] In an exemplary embodiment, Figure 1 As shown, the vehicle positioning method based on motion constraint adaptive filtering in this embodiment includes:

[0068] Step 1: Obtain historical measurement data, and preprocess the historical measurement data to obtain historical preprocessed data.

[0069] Among them, the historical measurement data includes: GNSS measurement data and INS measurement data of multiple historical epochs; the GNSS measurement data includes: multi-frequency multi-mode pseudorange and carrier phase measurement values ​​measured by the GNSS receiver, and the INS measurement data includes: three-dimensional velocity increment measured by the accelerometer in the INS and three-dimensional angle increment measured by the gyroscope in the INS; preprocessing includes: quality assessment and combined navigation solution; the historical preprocessing data includes: GNSS / INS navigation information of multiple historical epochs, and the GNSS / INS navigation information includes: three-dimensional position increment, three-dimensional position, pitch angle, roll angle and heading angle of the vehicle.

[0070] Specifically, the preprocessing is mainly to evaluate the quality of historical measurement data and to extract a section of historical measurement data containing straight driving and turning for combined navigation solution. The purpose is to ensure that the intercepted historical measurement data has good observability so as to be used to determine the INS installation angle and NHC arm in subsequent steps.

[0071] Step 2: Based on the historical preprocessed data, construct a virtual dead reckoning state model and a virtual dead reckoning measurement model.

[0072] As an optional implementation, the virtual dead reckoning state model includes:

[0073]

[0074] in, represents δP at epoch k n ,δP n It represents the three-dimensional position error of the vehicle in the n-frame, where the n-frame is the navigation frame; represents the δP of the k-1 epoch n ; represents the k-1 epoch represents the direction cosine matrix from b system to n system, where b system is the carrier system; represents the k-1 epoch represents the direction cosine matrix from v to b; M represents the auxiliary matrix; α k-1 represents the α of the k-1 epoch, where α represents the pitch installation angle error and heading installation angle error of the INS; represents the three-dimensional position increment of the vehicle in the n-frame at the k-epoch; φ k-1 represents the φ of the k-1 epoch, where φ represents the pitch misalignment angle error, roll misalignment angle error, and heading misalignment angle error of the vehicle; δk k-1 represents δk of the k-1 epoch, where δk represents the virtual odometer scale factor error.

[0075] As an optional implementation, the virtual dead reckoning measurement model includes:

[0076]

[0077] Among them, z DR,k represents the measurement vector of the virtual dead reckoning model at epoch k; represents P of epoch k obtained by the position recursion equation n , P n Represents the three-dimensional position of the vehicle in the n-frame; P represents the k-epoch P of the historical preprocessed data n ; l represents the k-1 epoch b , l b represents the NHC lever arm; × represents the vector cross product operation; represents δl at epoch k b , δl b Indicates the NHC lever arm error; e DR,k H represents the measurement noise vector of the virtual dead reckoning measurement model at epoch k; DR,k represents the coefficient matrix of the virtual dead reckoning measurement model at epoch k; x k represents x at epoch k, and x represents the combined system state vector.

[0078] Specifically, the position recursion equation is:

[0079]

[0080] in, represents the P of the k-1 epoch obtained by the position recursion equation n ; It represents the three-dimensional position increment of the vehicle in the v system at the k epoch.

[0081] The expression for x is:

[0082] x=[(δP n ) T α T φ T (δl b ) T δk] T .

[0083] For α, φ, δl b and δk, are modeled as random walk processes.

[0084] Step 3: Determine the virtual dead reckoning state vector based on the virtual dead reckoning state model and the virtual dead reckoning measurement model using Kalman filtering.

[0085] Specifically, the expression of step 3 is:

[0086]

[0087]

[0088] Among them, K DR,k represents the gain matrix of virtual dead reckoning at epoch k; H represents the one-step prediction covariance matrix of the virtual dead reckoning state model at epoch k; DR,k represents the coefficient matrix of the virtual dead reckoning measurement model at epoch k; C DR,k represents the measurement noise covariance matrix of the virtual dead reckoning measurement model at epoch k; represents the virtual dead reckoning state vector for epoch k; represents the one-step predicted state vector of the virtual dead reckoning state model at epoch k; represents the error covariance matrix of virtual dead reckoning at epoch k.

[0089] Step 4: Determine the INS installation angle and NHC lever arm based on the virtual dead reckoning state vector.

[0090] Among them, Figure 2As shown in the figure, the INS installation angle is the Euler angle from the vehicle system (v system) to the carrier system (b system). The essence of the INS installation angle is that the v system and the b system are not completely aligned during the INS installation process, and it is generally a small angle. The NHC lever arm is the distance vector from the center of the INS to the NHC effective point. Figure 2 In, x v is the x-axis of the v system, y v is the y-axis of the v system, z v is the z-axis of the v-frame, x b is the x-axis of the b system, y b is the y-axis of the b system, z b is the z-axis of the b system.

[0091] Specifically, yes Perform Euler angle conversion to get the INS installation angle, The calculation formulas for the NHC lever arm are:

[0092]

[0093] in, represents the kth epoch represents the direction cosine matrix from epoch k-1 to epoch k in the v frame, which can be calculated from the pitch angle error and heading angle error of the INS estimated by filtering; represents l at epoch k b Since the above two equations are updated recursively on an epoch-by-epoch basis, considering the convergence and steady-state properties of the filter, the INS installation angle and NHC lever arm calculated from the last epoch in the historical preprocessed data are saved here as the INS installation angle and NHC lever arm of step 4.

[0094] Step 5: Acquire the raw data of the current epoch; the raw data includes: GNSS measurement data and / or INS measurement data.

[0095] Specifically, if the current epoch is blocked by urban high-rise buildings, roadside trees, overpasses and tunnels, and the GNSS receiver cannot receive GNSS measurement data, the original data of the current epoch does not contain GNSS measurement data.

[0096] Step 6: Based on the INS measurement data, construct the combined system state model and mechanical arrangement model.

[0097] As an optional implementation, the combined system state model includes:

[0098]

[0099] in, represents the differential of φ; It represents the three-dimensional rotation angular velocity of system n relative to system i, where system i is an inertial system; express The calculation error of Represents the three-dimensional measurement error of the gyroscope; Indicates δv n The differential of δv n represents the three-dimensional velocity error of the vehicle in the n system; f b Represents the three-dimensional velocity increment measured by the accelerometer; express The calculation error of represents the three-dimensional angular velocity of the earth's rotation in the n system; express The calculation error of represents the angular velocity of the three-dimensional carrier motion in the n system; δf b Indicates the three-dimensional measurement error of the accelerometer; δg n Represents the three-dimensional gravity error; Denotes δP n The differential of .

[0100] As an optional implementation, the mechanical arrangement model includes:

[0101]

[0102] in, represents the P of the k-1 epoch obtained by the position recursion equation n ; represents the three-dimensional velocity of the vehicle in the n-frame at the k-epoch; τ represents the sampling interval of INS; represents the three-dimensional velocity of the vehicle in the n-frame at the k-1 epoch; represents the integral term of the specific force at epoch k; represents the harmful acceleration integral term at epoch k; represents the kth epoch represents the direction cosine matrix from epoch k-1 to epoch k in the n system; Represents the direction cosine matrix from epoch k to epoch k-1 in frame b.

[0103] Step 7: Based on the INS mounting angle, NHC lever arm and mechanical arrangement model, determine the 3D position and 3D velocity of the vehicle's compensated INS mounting angle and NHC lever arm.

[0104] As an optional implementation, the calculation formula for compensating the INS installation angle and the three-dimensional velocity of the NHC lever arm of the vehicle in the V system includes:

[0105]

[0106] in, represents the three-dimensional velocity of the compensated INS installation angle and NHC lever arm of the vehicle at epoch k in the v system, where the v system is the vehicle system; represents the direction cosine matrix from b to v calculated according to the INS installation angle; represents the three-dimensional angle increment measured by the gyroscope at epoch k; represents the kth epoch

[0107] Step 8: When the original data does not contain GNSS measurement data, the three-dimensional position of the vehicle after filtering and updating is determined according to the first positioning process to achieve vehicle positioning.

[0108] The first positioning process includes:

[0109] Step 81: Construct the NHC measurement model based on the compensated INS mounting angle of the vehicle and the three-dimensional velocity and nominal velocity of the NHC lever arm.

[0110] Specifically, when the original data does not contain GNSS measurement data, the combined system state vector is:

[0111]

[0112] in, It represents the three-dimensional zero bias of the gyroscope in the b system; It represents the three-dimensional zero bias of the accelerometer in the b system.

[0113] The three-dimensional zero bias error of the gyroscope and accelerometer is modeled as a first-order Markov process:

[0114]

[0115] in, express The differential of express The differential of; ζ represents the correlation time; w ε The noisy driving vector representing the gyroscope bias; The noisy driving vector representing the accelerometer bias.

[0116] When the original data does not contain GNSS measurement data, NHC is implemented to assist vehicle navigation. Under the NHC assumption, the nominal speed of the vehicle is:

[0117] v NHC,k =[0v y,k 0] T .

[0118] Among them, v NHC,krepresents the nominal speed of the vehicle at epoch k, v y,k It represents the forward velocity of the vehicle in the v frame at epoch k.

[0119] Based on the vehicle's compensated INS installation angle and the three-dimensional velocity and nominal velocity of the NHC lever arm, the NHC measurement model is constructed as follows:

[0120]

[0121] Among them, z k represents the measurement vector of the NHC measurement model at epoch k; represents the kth epoch represents the direction cosine matrix from the n system to the b system; φ k represents φ at epoch k; represents δv at epoch k n ; represents the kth epoch e v,k represents the measurement noise vector of the NHC measurement model at epoch k.

[0122] In the absence of a wheel odometer, the NHC measurement model expressed in the above formula can only constrain the lateral and lateral speeds of the vehicle, which can be converted into the following matrix form:

[0123] z k =H v,k x k +e v,k .

[0124] Among them, H v,k H represents the coefficient matrix of the NHC measurement model at epoch k. v,k Calculated by the following formula:

[0125]

[0126] Among them, h 1,1 Indicates H v,k The element in the first row and first column of 1,2 Indicates H v,k The element in the first row and second column of 1,4 Indicates H v,k The element in the first row and fourth column of 2,1 Indicates H v,k The element in the second row and first column of 2,2 Indicates H v,k The element in the 2nd row and 2nd column of 2,4 Indicates H v,k The element at row 2 and column 4 of ; represents the matrix [·] OK.

[0127] Considering the NHC assumption that the lateral velocity constraint is unrelated to the apical velocity constraint, that is:

[0128] C v,k =diag(C x ,C z ).

[0129] Among them, C v,k represents the measurement noise covariance matrix of the NHC measurement model at epoch k; diag(·) represents the diagonal matrix; C x represents the NHC measurement noise variance of the vehicle side; C z Represents the NHC measurement noise variance of the vehicle's anterograde direction.

[0130] Step 82: Based on the NHC measurement model, reconstruct the measurement noise covariance matrix using inverse gamma distribution to obtain the NHC measurement model including the reconstructed measurement noise covariance matrix.

[0131] Specifically, based on the NHC measurement model, the inverse gamma distribution is used to reconstruct the NHC measurement noise covariance matrix, that is, the NHC measurement noise covariance matrix C v,k is also considered as an unknown parameter. To jointly estimate the NHC measurement noise covariance matrix C v,k With the state vector x k , need to ensure C v,k The prior probability density function of has the same functional form as the posterior probability density function.

[0132] First, the prior probability density function p(C v,k |z 1:k-1 ) is updated as follows:

[0133]

[0134] Among them, z 1:k-1 represents the measurement vector of the NHC measurement model from epoch 1 to epoch k-1; IG(·,α k|k-1,i ,β k|k-1,i ) indicates that the shape parameter is α k|k-1,i , the scale parameter is β k|k-1,i The inverse gamma probability density function, α k|k-1,i represents the i-th prior shape parameter at epoch k, β k|k-1,i represents the i-th prior scale parameter at epoch k; Represents C v,k The i-th element on the diagonal of v,k The dimension of .

[0135] According to the Chapman-Kolmogorov equation, p(C v |z1:k-1 ) can be expressed as:

[0136] p(C v,k |z 1:k-1 )=∫p(C v,k |C v,k-1 )p(C v,k-1 |z 1:k-1 )dC v,k-1 .

[0137] Among them, p(C v,k |C v,k-1 ) represents the dynamic model from epoch k-1 to epoch k; p(C v,k-1 |z 1:k-1 ) represents the measurement noise covariance matrix C of the NHC measurement model at k-1 epoch v,k-1 The posterior probability density function of .

[0138] Since in practice p(C v,k |C v,k-1 ) is generally unknown, and the forgetting factor ρ is usually used to transfer the posterior estimated parameters of the previous epoch, as follows:

[0139]

[0140] Among them, the value range of ρ is (0,1], which reflects the fluctuation degree of measurement noise over time; α k-1|k-1,i represents the i-th posterior shape parameter at k-1 epochs, β k-1|k-1,i represents the i-th posterior scale parameter at k-1 epochs.

[0141] Considering that the lateral velocity and lateral velocity of the vehicle under the V system can be regarded as the fluctuation degree of the measurement noise, the following elastic forgetting factors ρ1 and ρ2 are constructed:

[0142]

[0143] Among them, |·| represents the modulo operation; v x Indicates the lateral speed of the vehicle under the v system; v v Represents the three-dimensional velocity modulus of the vehicle; v z Indicates the vehicle's celestial speed in the V system.

[0144] Next, proceed to C v,k The posterior probability density function p(C v,k |z 1:k ), since the prior probability density function is modeled as an inverse gamma distribution, the posterior probability density function p(C v,k |z 1:k ) also follows the inverse gamma distribution, as follows:

[0145]

[0146] Among them, z 1:k represents the measurement vector of the NHC measurement model from epoch 1 to epoch k; α k|k,i represents the i-th posterior shape parameter at epoch k, β k|k,i represents the i-th posterior scale parameter at epoch k.

[0147] Step 83: Based on the combined system state model and the NHC measurement model including the reconstructed measurement noise covariance matrix, a combined system state vector is determined using variational Bayesian adaptive filtering.

[0148] Specifically, based on variational Bayesian theory, C v,k The approximate posterior probability density Q (j+1) (C v,k ) and the state vector x k The approximate posterior probability density Q (j+1) (x k ) all satisfy the following formula:

[0149]

[0150] Where ln[·] represents the natural logarithm; Q(θ) represents Q (j+1) (C v,k ) or Q (j+1) (x k );E[·] represents the expected operation; Indicates Ξ k The remaining elements except θ in c θ represents a constant that is independent of θ.

[0151] Due to C v,k With x k Mutually coupled, Q cannot be directly calculated (j+1) (C v,k ) and Q (j+1) (x k ), so the fixed-point iteration method is used to obtain Q (j+1) (C v,k ) and Q (j+1) (x k ) is the local optimal solution.

[0152] First, let θ = C v,k , according to variational Bayes theory, Q (j+1) (C v,k ) can be updated to the following inverse gamma distribution:

[0153]

[0154] Among them, Q(j+1) (·) represents the approximate posterior probability density of the j+1th iteration; the shape parameter α k|k,i and the scale parameter β k|k,i Can be updated to:

[0155]

[0156] Among them, the auxiliary matrix of the jth iteration under the k epoch is It can be calculated by the following formula:

[0157]

[0158] in, express The i-th element on the diagonal; E (j) [·] represents the expectation of the jth iteration; represents the state vector of the combined system at the jth iteration; represents the error covariance matrix of the combined system at the jth iteration.

[0159] The measurement noise covariance matrix of the NHC measurement model after the j+1th iteration at epoch k It is expressed as;

[0160]

[0161] Among them, α k|k,1 represents the first posterior shape parameter at epoch k, β k|k,1 represents the first posterior scale parameter at epoch k; α k|k,2 represents the second posterior shape parameter at epoch k, β k|k,2 represents the second posterior scale parameter at epoch k.

[0162] Next, let θ = x k , according to variational Bayes theory, Q (j+1) (x k ) can be updated to the following Gaussian distribution:

[0163]

[0164] in, The state vector is The error covariance matrix is Gaussian probability density function; the combined system state vector of the j+1th iteration and the error covariance matrix It is expressed as:

[0165]

[0166] in, represents the gain matrix of the combined system at the j+1th iteration under the kth epoch; P k|k-1 represents the one-step prediction covariance matrix of the combined system state model at k epochs; represents the one-step predicted state vector of the combined system state model at epoch k.

[0167] Step 84: Determine the filtered updated three-dimensional position of the vehicle based on the combined system state vector and the compensated INS mounting angle of the vehicle and the three-dimensional position of the NHC lever arm.

[0168] Specifically, when the number of iterations reaches the preset maximum number of iterations, the α of the current iteration is output. k|k , β k|k , and According to the obtained combined system state vector The filtered updated 3D position of the vehicle is determined with the compensated INS mounting angle and the 3D position of the NHC lever arm.

[0169] Step 9: When the original data includes GNSS measurement data, the filtered updated three-dimensional position of the vehicle is determined according to the second positioning process to achieve vehicle positioning.

[0170] The second positioning process includes:

[0171] Step 91: Construct a combined system measurement model based on the vehicle's compensated INS mounting angle and the three-dimensional position of the NHC arm and the GNSS measurement data in the raw data.

[0172] Specifically, when the original data contains GNSS measurement data, the combined system state vector is:

[0173]

[0174] Among them, N rb represents the single difference ambiguity vector between the reference station r and the mobile station b.

[0175] Based on the vehicle's compensated INS installation angle and the three-dimensional position of the NHC arm and the GNSS measurement data in the original data, a combined system measurement model is constructed. The combined system measurement model is:

[0176]

[0177] H k x k +e k

[0178] in, represents the difference operator, which means first finding the primary difference between the reference station and the mobile station, and then finding the secondary difference between the reference satellite i and the non-reference satellite g; represents the kth epoch It indicates the pseudo-range measurement value obtained by calculating the single difference between the base station and the mobile station, and the secondary difference between the reference satellite and the non-reference satellite; represents the kth epoch It indicates the pseudo-range measurement value obtained by using the compensation INS installation angle of the vehicle and the three-dimensional position prediction of the NHC arm through single difference between the reference station and the mobile station, and secondary difference between the reference satellite and the non-reference satellite; represents the kth epoch It indicates the carrier phase measurement value obtained by calculating the single difference between the base station and the mobile station and the secondary difference between the reference satellite and the non-reference satellite; represents the kth epoch μ represents the carrier phase measurement value obtained by using the vehicle's compensated INS installation angle and the three-dimensional position prediction of the NHC arm through single difference between the reference station and the mobile station, and secondary difference between the reference satellite and the non-reference satellite; μ i represents μ of the reference satellite, μ represents the unit line of sight vector; μ g represents μ of the non-reference satellite; represents the kth epoch represents the direction cosine matrix from the n system to the e system, where the e system represents the earth system; L b Represents the distance vector from the INS center to the GNSS antenna phase center; represents the double-difference pseudorange measurement noise including non-modeled errors at epoch k; λ represents the wavelength; H represents the double-difference carrier phase measurement noise including the unmodeled error at epoch k; k represents the coefficient matrix of the combined system measurement model at epoch k; e k represents the measurement noise vector of the combined system measurement model at epoch k.

[0179] Step 92: Based on the combined system state model and the combined system measurement model, a Kalman filter is used to determine the combined system state vector.

[0180] Specifically, the Kalman filter is used to determine the state vector of the combined system based on the combined system state model and the combined system measurement model, as follows:

[0181]

[0182] Among them, K k represents the gain matrix of the combined system at k epochs; C k represents the measurement noise covariance matrix of the combined system measurement model at k epochs; represents the one-step prediction covariance matrix of the combined system state model at k epochs; represents the state vector of the combined system at epoch k; represents the one-step predicted state vector of the combined system state model at epoch k; represents the error covariance matrix of the combined system at k epochs.

[0183] Step 93: Determine the filtered updated three-dimensional position of the vehicle based on the combined system state vector and the compensated INS mounting angle of the vehicle and the three-dimensional position of the NHC lever arm.

[0184] In an exemplary embodiment, a vehicle positioning system based on motion constraint adaptive filtering is provided, which is used to implement any of the above-mentioned vehicle positioning methods based on motion constraint adaptive filtering. The vehicle positioning system based on motion constraint adaptive filtering includes:

[0185] The data acquisition module is used to acquire historical measurement data and pre-process the historical measurement data to obtain historical pre-processed data; the historical measurement data includes: GNSS measurement data and INS measurement data of multiple historical epochs; the GNSS measurement data includes: multi-frequency multi-mode pseudorange and carrier phase measurement values ​​measured by the GNSS receiver, and the INS measurement data includes: three-dimensional velocity increment measured by the accelerometer in the INS and three-dimensional angle increment measured by the gyroscope in the INS; the pre-processing includes: quality assessment and combined navigation solution; the historical pre-processed data includes: GNSS / INS navigation information of multiple historical epochs, and the GNSS / INS navigation information includes: three-dimensional position increment, three-dimensional position, pitch angle, roll angle and heading angle of the vehicle.

[0186] The first model building module is used to build a virtual dead reckoning state model and a virtual dead reckoning measurement model based on historical preprocessing data.

[0187] The virtual dead reckoning state vector determination module is used to determine the virtual dead reckoning state vector based on the virtual dead reckoning state model and the virtual dead reckoning measurement model by using Kalman filtering.

[0188] The INS installation angle and NHC lever arm determination module is used to determine the INS installation angle and NHC lever arm based on the virtual dead reckoning state vector; the INS installation angle is the Euler angle from the vehicle system to the carrier system, and the NHC lever arm is the distance vector from the center of the INS to the effective point of the NHC.

[0189] The raw data acquisition module is used to obtain the raw data of the current epoch; the raw data includes: GNSS measurement data and / or INS measurement data.

[0190] The second model building module is used to build a combined system state model and a mechanical arrangement model based on INS measurement data.

[0191] The velocity compensation module is used to determine the three-dimensional position and three-dimensional velocity of the compensated INS installation angle and NHC lever arm of the vehicle based on the INS installation angle, NHC lever arm and mechanical arrangement model.

[0192] The first positioning module is used to determine the filtered updated three-dimensional position of the vehicle according to the first positioning process to achieve vehicle positioning when the original data does not contain GNSS measurement data; wherein the first positioning process includes:

[0193] The NHC measurement model is constructed based on the compensated INS installation angle of the vehicle and the three-dimensional velocity and nominal velocity of the NHC lever arm.

[0194] Based on the NHC measurement model, the measurement noise covariance matrix is ​​reconstructed using the inverse gamma distribution, and the NHC measurement model including the reconstructed measurement noise covariance matrix is ​​obtained.

[0195] Based on the combined system state model and the NHC measurement model including the reconstructed measurement noise covariance matrix, the combined system state vector is determined by variational Bayesian adaptive filtering.

[0196] A filtered updated three-dimensional position of the vehicle is determined based on the combined system state vector and the compensated INS mounting angle of the vehicle and the three-dimensional position of the NHC lever arm.

[0197] The second positioning module is used to determine the filtered updated three-dimensional position of the vehicle according to the second positioning process to achieve vehicle positioning when the original data contains GNSS measurement data; wherein the second positioning process includes:

[0198] A combined system measurement model is constructed based on the vehicle's compensated INS installation angle and the three-dimensional position of the NHC arm and the GNSS measurement data in the original data.

[0199] Based on the combined system state model and the combined system measurement model, Kalman filtering is used to determine the combined system state vector.

[0200] A filtered updated three-dimensional position of the vehicle is determined based on the combined system state vector and the compensated INS mounting angle of the vehicle and the three-dimensional position of the NHC lever arm.

[0201] In an exemplary embodiment, a computer device is provided, including: a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement a vehicle positioning method based on motion constraint adaptive filtering.

[0202] In an exemplary embodiment, a computer-readable storage medium is provided, on which a computer program is stored. When the computer program is executed by a processor, a vehicle positioning method based on motion constraint adaptive filtering is implemented.

[0203] In an exemplary embodiment, a computer program product is provided, comprising a computer program, which implements a vehicle positioning method based on motion constraint adaptive filtering when executed by a processor.

[0204] In an exemplary embodiment, a computer device is provided. The computer device may be a server or a terminal. The internal structure diagram thereof may be as follows: Figure 3 As shown. The computer device includes a processor, a memory, an input / output interface (Input / Output, referred to as I / O) and a communication interface. The processor, the memory and the input / output interface are connected through a system bus, and the communication interface is connected to the system bus through the input / output interface. The processor of the computer device is used to provide computing and control capabilities. The memory of the computer device includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system, a computer program and a database. The internal memory provides an environment for the operation of the operating system and the computer program in the non-volatile storage medium. The input / output interface of the computer device is used to exchange information between the processor and an external device. The communication interface of the computer device is used to communicate with an external terminal through a network connection. When the computer program is executed by the processor, a vehicle positioning method based on motion constraint adaptive filtering is implemented.

[0205] Those skilled in the art will understand that Figure 3 The structure shown in the figure is only a block diagram of a part of the structure related to the solution of the present application, and does not constitute a limitation on the computer device to which the solution of the present application is applied. The specific computer device may include more or fewer components than those shown in the figure, or combine certain components, or have a different arrangement of components.

[0206] It should be noted that the user information (including but not limited to user device information, user personal information, etc.) and data (including but not limited to data used for analysis, stored data, displayed data, etc.) involved in this application are all information and data authorized by the user or fully authorized by all parties, and the collection, use and processing of relevant data must comply with relevant regulations.

[0207] Those of ordinary skill in the art can understand that all or part of the processes in the above-mentioned embodiment methods can be completed by instructing the relevant hardware through a computer program, and the computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the embodiments of the above-mentioned methods. Among them, any reference to the memory, database or other medium used in the embodiments provided in the present application can include at least one of non-volatile and volatile memory. Non-volatile memory can include read-only memory (ROM), magnetic tape, floppy disk, flash memory, optical memory, high-density embedded non-volatile memory, resistive random access memory (ReRAM), magnetic random access memory (MRAM), ferroelectric random access memory (FRAM), phase change memory (PCM), graphene memory, etc. Volatile memory can include random access memory (RAM) or external cache memory, etc. By way of illustration and not limitation, RAM may be in various forms, such as static random access memory (SRAM) or dynamic random access memory (DRAM).

[0208] The database involved in each embodiment provided in this application may include at least one of a relational database and a non-relational database. The non-relational database may include a distributed database based on blockchain, etc., but is not limited thereto. The processor involved in each embodiment provided in this application may be a general-purpose processor, a central processing unit, a graphics processor, a digital signal processor, a programmable logic device, a data processing logic device based on quantum computing, etc., but is not limited thereto.

[0209] It should be noted that the user information (including but not limited to user device information, user personal information, etc.) and data (including but not limited to data used for analysis, stored data, displayed data, etc.) involved in this application are all information and data authorized by the user or fully authorized by all parties, and the collection, use and processing of relevant data must comply with relevant regulations.

[0210] The technical features of the above embodiments may be combined arbitrarily. To make the description concise, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0211] This article uses specific examples to illustrate the principles and implementation methods of this application. The description of the above embodiments is only used to help understand the method and core ideas of this application. At the same time, for those skilled in the art, according to the ideas of this application, there will be changes in the specific implementation methods and application scope. In summary, the content of this specification should not be understood as limiting this application.

Claims

1. A vehicle positioning method based on motion-constrained adaptive filtering, characterized in that: The vehicle positioning method based on motion constraint adaptive filtering includes: Acquire historical measurement data, and pre-process the historical measurement data to obtain historical pre-processed data; the historical measurement data includes: GNSS measurement data and INS measurement data of multiple historical epochs; the GNSS measurement data includes: multi-frequency multi-mode pseudorange and carrier phase measurement values ​​measured by the GNSS receiver, and the INS measurement data includes: three-dimensional velocity increment measured by the accelerometer in the INS and three-dimensional angle increment measured by the gyroscope in the INS; the pre-processing includes: quality assessment and integrated navigation solution; the historical pre-processed data includes: GNSS / INS navigation information of multiple historical epochs, and the GNSS / INS navigation information includes: three-dimensional position increment, three-dimensional position, pitch angle, roll angle and heading angle of the vehicle; Based on the historical preprocessed data, a virtual dead reckoning state model and a virtual dead reckoning measurement model are constructed; Determine a virtual dead reckoning state vector based on a virtual dead reckoning state model and a virtual dead reckoning measurement model by using a Kalman filter; Based on the virtual dead reckoning state vector, determine the INS installation angle and NHC lever arm; the INS installation angle is the Euler angle from the vehicle system to the carrier system, and the NHC lever arm is the distance vector from the center of the INS to the effective point of the NHC; Acquire raw data of the current epoch; the raw data includes: GNSS measurement data and / or INS measurement data; Based on INS measurement data, build a combined system state model and mechanical arrangement model; Determine the 3D position and 3D velocity of the vehicle's compensated INS mounting angle and NHC lever arm based on the INS mounting angle, NHC lever arm and mechanical arrangement model; When the original data does not contain GNSS measurement data, the three-dimensional position of the vehicle after filtering and updating is determined according to the first positioning process to achieve vehicle positioning; wherein the first positioning process includes: Construct the NHC measurement model based on the vehicle's compensated INS installation angle and the three-dimensional velocity and nominal velocity of the NHC lever arm; Based on the NHC measurement model, the measurement noise covariance matrix is ​​reconstructed by using the inverse gamma distribution, and the NHC measurement model including the reconstructed measurement noise covariance matrix is ​​obtained; Based on the combined system state model and the NHC measurement model including the reconstructed measurement noise covariance matrix, the combined system state vector is determined by using variational Bayesian adaptive filtering. determining a filtered updated three-dimensional position of the vehicle based on the combined system state vector and the compensated INS mounting angle of the vehicle and the three-dimensional position of the NHC lever arm; When the original data includes GNSS measurement data, the filtered updated three-dimensional position of the vehicle is determined according to the second positioning process to achieve vehicle positioning; wherein the second positioning process includes: A combined system measurement model is constructed based on the vehicle’s compensated INS mounting angle and the three-dimensional position of the NHC arm and the GNSS measurement data in the raw data; Based on the combined system state model and the combined system measurement model, the Kalman filter is used to determine the combined system state vector; A filtered updated three-dimensional position of the vehicle is determined based on the combined system state vector and the compensated INS mounting angle of the vehicle and the three-dimensional position of the NHC lever arm.

2. The vehicle positioning method based on motion constraint adaptive filtering according to claim 1 is characterized in that: The virtual dead reckoning state model includes: in, represents δP at epoch k n ,δP n It represents the three-dimensional position error of the vehicle in the n-frame, where the n-frame is the navigation frame; represents the δP of the k-1 epoch n ; represents the k-1 epoch represents the direction cosine matrix from b system to n system, where b system is the carrier system; represents the k-1 epoch represents the direction cosine matrix from v to b; M represents the auxiliary matrix; α k-1 represents the α of the k-1 epoch, where α represents the pitch installation angle error and heading installation angle error of the INS; represents the three-dimensional position increment of the vehicle in the n-frame at the k-epoch; φ k-1 represents the φ of the k-1 epoch, where φ represents the pitch misalignment angle error, roll misalignment angle error, and heading misalignment angle error of the vehicle; δk k-1 represents δk of the k-1 epoch, where δk represents the virtual odometer scale factor error.

3. The vehicle positioning method based on motion constraint adaptive filtering according to claim 2 is characterized in that: The virtual dead reckoning measurement model includes: Among them, z DR,k represents the measurement vector of the virtual dead reckoning model at epoch k; represents P of epoch k obtained by the position recursion equation n , P n Represents the three-dimensional position of the vehicle in the n-frame; P represents the k-epoch P of the historical preprocessed data n ; l represents the k-1 epoch b , l b represents the NHC lever arm; × represents the vector cross product operation; represents δl at epoch k b , δl b Indicates the NHC lever arm error; e DR,k H represents the measurement noise vector of the virtual dead reckoning measurement model at epoch k; DR,k represents the coefficient matrix of the virtual dead reckoning measurement model at epoch k; x k represents x at epoch k, and x represents the combined system state vector.

4. The vehicle positioning method based on motion constraint adaptive filtering according to claim 3 is characterized in that: The combined system state model comprises: in, represents the differential of φ; It represents the three-dimensional rotation angular velocity of system n relative to system i, where system i is an inertial system; express The calculation error of Represents the three-dimensional measurement error of the gyroscope; Indicates δv n The differential of δv n represents the three-dimensional velocity error of the vehicle in the n system; f b Represents the three-dimensional velocity increment measured by the accelerometer; express The calculation error of represents the three-dimensional angular velocity of the earth's rotation in the n system; express The calculation error of represents the angular velocity of the three-dimensional carrier motion in the n system; δf b Indicates the three-dimensional measurement error of the accelerometer; δg n Represents the three-dimensional gravity error; Denotes δP n The differential of .

5. The vehicle positioning method based on motion constraint adaptive filtering according to claim 4 is characterized in that: The mechanical arrangement model includes: in, represents the P of the k-1 epoch obtained by the position recursion equation n ; represents the three-dimensional velocity of the vehicle in the n-frame at the k-epoch; τ represents the sampling interval of INS; represents the three-dimensional velocity of the vehicle in the n-frame at the k-1 epoch; represents the integral term of the specific force at epoch k; represents the harmful acceleration integral term at epoch k; represents the kth epoch represents the direction cosine matrix from epoch k-1 to epoch k in the n system; Represents the direction cosine matrix from epoch k to epoch k-1 in frame b.

6. The vehicle positioning method based on motion constraint adaptive filtering according to claim 5 is characterized in that: The calculation formula for the vehicle's compensated INS installation angle and the three-dimensional velocity of the NHC lever arm in the V system includes: in, represents the three-dimensional velocity of the compensated INS installation angle and NHC lever arm of the vehicle at epoch k in the v system, where the v system is the vehicle system; represents the direction cosine matrix from b to v calculated according to the INS installation angle; represents the three-dimensional angle increment measured by the gyroscope at epoch k; represents the kth epoch 7. A vehicle positioning system based on motion constraint adaptive filtering, used to implement the vehicle positioning method based on motion constraint adaptive filtering as claimed in any one of claims 1 to 6, characterized in that: The vehicle positioning system based on motion constraint adaptive filtering includes: The data acquisition module is used to acquire historical measurement data and pre-process the historical measurement data to obtain historical pre-processed data; the historical measurement data includes: GNSS measurement data and INS measurement data of multiple historical epochs; the GNSS measurement data includes: multi-frequency multi-mode pseudorange and carrier phase measurement values ​​measured by the GNSS receiver, and the INS measurement data includes: three-dimensional velocity increment measured by the accelerometer in the INS and three-dimensional angle increment measured by the gyroscope in the INS; the pre-processing includes: quality assessment and integrated navigation solution; the historical pre-processed data includes: GNSS / INS navigation information of multiple historical epochs, and the GNSS / INS navigation information includes: three-dimensional position increment, three-dimensional position, pitch angle, roll angle and heading angle of the vehicle; A first model building module, for building a virtual dead reckoning state model and a virtual dead reckoning measurement model based on historical preprocessing data; A virtual dead reckoning state vector determination module, for determining a virtual dead reckoning state vector based on a virtual dead reckoning state model and a virtual dead reckoning measurement model by using a Kalman filter; INS installation angle and NHC lever arm determination module, used to determine the INS installation angle and NHC lever arm based on the virtual dead reckoning state vector; the INS installation angle is the Euler angle from the vehicle system to the carrier system, and the NHC lever arm is the distance vector from the center of the INS to the effective point of the NHC; A raw data acquisition module is used to obtain raw data of the current epoch; the raw data includes: GNSS measurement data and / or INS measurement data; The second model building module is used to build a combined system state model and a mechanical arrangement model based on INS measurement data; A velocity compensation module for determining the three-dimensional position and three-dimensional velocity of the vehicle's compensated INS mounting angle and NHC lever arm based on the INS mounting angle, NHC lever arm and mechanical arrangement model; The first positioning module is used to determine the filtered updated three-dimensional position of the vehicle according to the first positioning process to achieve vehicle positioning when the original data does not contain GNSS measurement data; wherein the first positioning process includes: Construct the NHC measurement model based on the vehicle's compensated INS installation angle and the three-dimensional velocity and nominal velocity of the NHC lever arm; Based on the NHC measurement model, the measurement noise covariance matrix is ​​reconstructed by using the inverse gamma distribution, and the NHC measurement model including the reconstructed measurement noise covariance matrix is ​​obtained; Based on the combined system state model and the NHC measurement model including the reconstructed measurement noise covariance matrix, the combined system state vector is determined by using variational Bayesian adaptive filtering. determining a filtered updated three-dimensional position of the vehicle based on the combined system state vector and the compensated INS mounting angle of the vehicle and the three-dimensional position of the NHC lever arm; The second positioning module is used to determine the filtered updated three-dimensional position of the vehicle according to the second positioning process to achieve vehicle positioning when the original data contains GNSS measurement data; wherein the second positioning process includes: A combined system measurement model is constructed based on the vehicle’s compensated INS mounting angle and the three-dimensional position of the NHC arm and the GNSS measurement data in the raw data; Based on the combined system state model and the combined system measurement model, the Kalman filter is used to determine the combined system state vector; A filtered updated three-dimensional position of the vehicle is determined based on the combined system state vector and the compensated INS mounting angle of the vehicle and the three-dimensional position of the NHC lever arm.

8. A computer device comprising: A memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the vehicle positioning method based on motion constraint adaptive filtering as described in any one of claims 1 to 6.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the vehicle positioning method based on motion constraint adaptive filtering as described in any one of claims 1 to 6 is implemented.

10. A computer program product, comprising a computer program, characterized in that When the computer program is executed by a processor, the vehicle positioning method based on motion constraint adaptive filtering as described in any one of claims 1 to 6 is implemented.

Images