Ground vehicle inertial positioning enhancement method considering motion constraints

Abstract

The application discloses a ground vehicle inertial positioning enhancement method considering motion constraints, and comprises the following steps: ground vehicle pose recursion calculation based on acceleration and angular velocity measured by a micro-electromechanical inertial measurement unit (MEMS-IMU); deep neural network is trained by using data in two driving conditions of vehicle straight driving and turning, so that the deep neural network can predict lateral and vertical velocities of the vehicle in real time according to MEMS-IMU measurement, wheel speed measurement and turning angle sensor measurement, and confidence of the prediction is given in the form of variance; the lateral and vertical velocities predicted by the deep neural network and vehicle rear axle wheel speed and front wheel turning angle jointly constitute motion constraints, cumulative error of MEMS-IMU pose calculation is corrected through an error state Kalman filtering algorithm, and accurate vehicle pose is obtained. The application can correct cumulative positioning error of ground vehicle inertial positioning, and improve the precision of ground vehicle inertial positioning.

Description

Technical Field

[0001] The present invention belongs to the field of intelligent transportation technology, and in particular relates to a ground vehicle inertial positioning enhancement method considering motion constraints. Background Art

[0002] Ground-based unmanned vehicles are recognized for their enormous potential in improving road safety and traffic efficiency, and have garnered extensive attention and research in recent years. Currently, unmanned vehicles using integrated navigation and positioning using RTK-GNSS and low-cost MEMS-IMUs can achieve centimeter-level positioning that meets driving requirements under ideal conditions. However, in urban environments, where tall buildings block GNSS signals, integrated navigation degenerates to inertial positioning. MEMS-IMU inertial positioning suffers from cumulative errors, which gradually increase over time, ultimately leading to divergent and unusable positioning results.

[0003] At present, the methods for enhancing the inertial positioning of MEMS-IMU mainly include denoising methods such as discrete wavelet transform and empirical mode decomposition. However, the effectiveness of these methods is highly dependent on the setting of the threshold, the process is complex, and good results cannot be guaranteed under different load or road conditions. Another type of inertial enhancement method is to use deep learning technology. The patent "MEMS Inertial Navigation System Positioning Enhancement Method Based on LSTM Neural Network Model" (Application Number: 202110798898.1) designs an LSTM neural network model to denoise the MEMS-IMU and directly predict the vehicle's posture changes. However, the neural network is a black box, and directly predicting the vehicle's posture changes may result in uncontrollable outliers with errors, resulting in direct positioning failure. The above methods only start from the perspective of denoising the original measurement of MEMS-IMU, and do not fully consider the gains brought by the motion constraints of ground vehicles during driving to improve the positioning effect. Summary of the Invention

[0004] In order to solve the technical problems existing in the background technology, the present invention aims to provide a ground vehicle inertial positioning enhancement method taking into account motion constraints, which corrects the MEMS-IMU cumulative positioning error with the help of the motion constraints when the ground vehicle is driving, thereby improving the positioning accuracy.

[0005] In order to solve the technical problem, the technical solution of the present invention is:

[0006] A method for enhancing inertial positioning of a ground vehicle considering motion constraints, the method comprising:

[0007] S1: Based on the acceleration and angular velocity measured by the MEMS-IMU, a recursive equation is used to calculate the preliminary pose estimate of the ground vehicle and update it over time;

[0008] S2: Deep neural network training, using data from both straight-ahead and turning driving conditions. The deep neural network predicts the vehicle's lateral and vertical velocities in real time based on MEMS-IMU measurements, wheel speedometer measurements, and steering angle sensor measurements, and provides prediction confidence in the form of variance.

[0009] S3: Motion constraint construction: Constructing a vehicle motion constraint model by combining the lateral and vertical velocities predicted by the deep neural network, as well as the rear axle wheel speed and front wheel angle;

[0010] S4: Error state Kalman filter correction: Based on the preliminary pose estimation and motion constraints, the accumulated error in the MEMS-IMU pose solution is corrected by applying the error state Kalman filter algorithm to obtain the corrected vehicle pose.

[0011] Furthermore, the step S1 specifically includes:

[0012] The vehicle's linear and angular motion information is measured using a MEMS-IMU. The vehicle's posture is deduced based on the vehicle's initial posture and the posture recursion equation. Assuming that the accelerometer and gyroscope measurements have only one fixed bias, the vehicle's state is called the nominal state. The vehicle's discrete-time nominal posture recursion equation is:

[0013]

[0014] v k+1 =v k +[R k (a mk -a bk )+g k ]Δt

[0015]

[0016] a b(k+1) =a bk

[0017] ω b(k+1) =ω bk

[0018] Where, the subscripts k and k+1 represent two adjacent moments, and the corresponding time interval is Δt; p and v represent the position and velocity of the vehicle in the navigation coordinate system respectively; R represents the rotation matrix from the body coordinate system to the navigation coordinate system; a m 、a b They represent the acceleration measurement value and its bias respectively; g represents the gravity at the vehicle's location; q represents the rotation quaternion from the body coordinate system to the navigation coordinate system; q k {(ω mk -ω bk)Δt} represents the axis angle vector (ω mk -ω bk )Δt corresponding quaternion;ω m 、ω b Represent the angular rate measurement value and its bias respectively;

[0019] During the actual movement of the vehicle, the error state variables to be estimated are:

[0020] δX=[(δp) T (δv) T (δθ) T (δa b ) T (δω b ) T ] T

[0021] Where δp, δv and δθ represent the position error, velocity error and attitude error of the vehicle respectively; δa b and δω b They represent the errors of acceleration bias estimation and angular velocity bias estimation respectively;

[0022] The recursive equation for the error state discrete time is established as:

[0023] δp k+1 =δp k +δv k Δt

[0024]

[0025] In the formula, subscripts k and k+1 represent two adjacent moments, and the corresponding time interval is Δt; R, R T Represents the rotation matrix from the body coordinate system to the navigation coordinate system and its transpose respectively; a m 、a b They represent the acceleration measurement value and its bias respectively; δθ represents the attitude error in the vehicle body coordinate system; represents the disturbance vector of the velocity error estimate; ω m 、ω b Represent the angular rate measurement value and its bias respectively; represents the disturbance pulse vector of the attitude error estimate; denote the disturbance pulse vectors for acceleration bias estimation and angular velocity bias estimation, respectively.

[0026] Furthermore, the step S2 specifically includes:

[0027] According to the average yaw angular velocity measured by MEMS-IMU within the time window Determine whether the vehicle is in a straight-line or turning condition, and classify the data measured by MEMS-IMU, wheel speedometer, and steering angle sensor. The calculation is as follows:

[0028]

[0029] Where k and W are the start time and window size of the time window, respectively; Δt is the time interval between two adjacent moments; ω z (i) is the yaw angular velocity measured by the MEMS-IMU at time i; When the value is greater than the threshold value τ, it is turning, otherwise it is going straight. τ is set according to the data and experience;

[0030] The classified MEMS-IMU measurements, wheel speedometer measurements, and front wheel angle sensor measurements are input into a deep neural network, which outputs the predicted vehicle lateral and vertical velocities, as well as the variance representing the confidence level of the prediction. The input to the deep neural network is a one-dimensional tensor of size N×9, consisting of the acceleration, angular velocity, rear axle left and right wheel speeds, and average front wheel angle for N consecutive epochs. The main structure of the deep neural network is three end-to-end convolutional layers with a convolution kernel of 3 and a dilation factor of 1. The activation function between convolutional layers is ReLU, and the dropout coefficient between each layer is 0.5. The output of the convolutional layer is transformed through a fully connected layer to finally output the vehicle's lateral and vertical velocities and the corresponding variances.

[0031] The deep neural network performs supervised training, and the output of the deep neural network and the true label value are defined as follows:

[0032]

[0033] Where N is the number of tensors in the input deep neural network, logdet(·) represents the logarithm of the determinant of the matrix, is the variance corresponding to the i-th input, v i 、v i are the output of the deep neural network and its corresponding label value, In the definition of variance The square distance under the norm of ; During the training phase, the Adam optimizer is used to optimize the weights in the deep neural network model, and the initial learning rate is set to 10 -4 In each training cycle, the predicted output of the neural network model and the corresponding loss function are calculated through forward propagation, and the weights of the model are optimized through the backpropagation algorithm and the Adam optimizer. When the loss function converges to within the preset threshold, the training of the neural network model is terminated.

[0034] Furthermore, the step S3 specifically includes:

[0035] MEMS-IMU raw measurements have acceleration measurement bias a b and angular rate measurement bias ω b , which results in cumulative errors when using MEMS-IMU measurement for inertial positioning. Therefore, the error state variable of vehicle inertial positioning is selected as:

[0036] δX=[(δp) T (δv) T (δθ) T (δa b ) T (δω b ) T ] T

[0037] Where δp, δv and δθ represent the position error, velocity error and attitude error of the vehicle respectively; δa b and δω b They represent the errors of acceleration bias estimation and angular velocity bias estimation respectively;

[0038] The recursive equation for the error state discrete time is established as:

[0039] δp k+1 =δp k +δv k Δt

[0040]

[0041] In the formula, subscripts k and k+1 represent two adjacent moments, and the corresponding time interval is Δt; Represents the rotation matrix from the body coordinate system to the navigation coordinate system and its transpose respectively; a m 、a b They represent the acceleration measurement value and its bias respectively; δθ represents the attitude error in the vehicle body coordinate system; represents the disturbance vector of the velocity error estimate; ω m 、ω b Represent the angular rate measurement value and its bias respectively; represents the disturbance pulse vector of the attitude error estimate; denote the disturbance pulse vectors of acceleration bias estimation and angular velocity bias estimation respectively;

[0042] According to the error state recursion equation, the prior estimate of the vehicle error state is obtained and its corresponding covariance for:

[0043]

[0044] Where I is the 3×3 identity matrix; 0 is the 3×3 zero matrix; Q k represents the variance of the disturbance pulse vector n, represent velocity random walk and angle random walk respectively; Represent the power spectral density of the dynamic zero bias of the accelerometer and gyroscope respectively;

[0045] The lateral and vertical velocities predicted by the deep neural network, the rear axle wheel speed, and the front wheel angle constitute the constraints of the vehicle motion, where the vehicle lateral velocity v lat and vertical velocity v up Obtained according to the output of the deep neural network;

[0046] Vehicle longitudinal velocity constraint v lon Determine the speed of the rear axle wheel of the vehicle by measuring it with a wheel speed meter:

[0047]

[0048] Where, ω l 、ω r are the speeds of the left and right wheels on the rear axle of the vehicle, respectively, and r is the wheel radius;

[0049] Vehicle yaw rate constraint γ obs According to the longitudinal speed v lon and the front wheel steering angle δ f Jointly determine:

[0050]

[0051] Where, δ f is the front wheel turning angle of the vehicle, L is the distance between the front and rear axles of the vehicle;

[0052] Assuming that the vehicle coordinate system is consistent with the body coordinate system of the MEMS-IMU, the three-dimensional velocity constraint v consisting of the longitudinal, lateral and vertical velocities is obs =[v lon v lat v up ] T With the yaw rate constraint γ obs Together they form the following observation equation:

[0053] Z=HδX+R

[0054]

[0055] Where R is the variance of the confidence level corresponding to the three-dimensional velocity constraint and the yaw rate constraint; v INS , γINS are the three-dimensional nominal velocity and yaw rate with errors in the navigation coordinate system calculated recursively based on the MEMS-IMU; is the rotation matrix from the navigation coordinate system to the vehicle coordinate system; [·]× represents an antisymmetric operation; I and 0 are the unit matrix and the zero matrix, respectively, and the subscripts correspond to the dimensions of the matrix.

[0056] Furthermore, the step S4 specifically includes:

[0057] According to the Kalman filter equation, the posterior estimate of the vehicle error state is and its corresponding covariance for:

[0058]

[0059] Where I is the 3×3 identity matrix;

[0060] The lateral and vertical velocities, the vehicle's rear axle wheel speed, and the front wheel angle predicted by the deep neural network together constitute motion constraints, which can periodically correct the accumulated error of MEMS-IMU inertial positioning and obtain a more accurate vehicle posture.

[0061] Compared with the prior art, the advantages of the present invention are:

[0062] Considering the motion constraints composed of longitudinal, lateral, vertical velocity and yaw angular velocity when the ground vehicle is driving, the accumulated error of MEMS-IMU inertial positioning is corrected to improve the positioning accuracy of the vehicle. BRIEF DESCRIPTION OF THE DRAWINGS

[0063] Figure 1 , Flowchart of inertial positioning enhancement method;

[0064] Figure 2 , lateral and vertical velocity prediction deep neural network structure diagram. DETAILED DESCRIPTION

[0065] The specific implementation of the present invention is described below in conjunction with examples:

[0066] It should be noted that the structures, proportions, sizes, etc. shown in this specification are only used to match the contents disclosed in the specification for people familiar with this technology to understand and read, and are not used to limit the conditions under which the present invention can be implemented. Any structural modification, change in proportional relationship or adjustment of size should still fall within the scope of the technical content disclosed in the present invention without affecting the efficacy and purpose that can be achieved by the present invention.

[0067] At the same time, the terms such as "upper", "lower", "left", "right", "middle" and "one" quoted in this specification are only for the convenience of description and are not used to limit the scope of implementation of the present invention. Changes or adjustments to their relative relationships should be regarded as the scope of implementation of the present invention without substantially changing the technical content.

[0068] Example 1:

[0069] Reference Figure 1 The present invention proposes a method for enhancing inertial positioning of ground vehicles with motion constraints, including: using MEMS-IMU to measure noisy vehicle linear motion information and angular motion information to perform inertial positioning of the vehicle, using a deep neural network to predict the lateral and vertical speeds of the vehicle during driving and giving the prediction confidence in the form of variance, the lateral and vertical speeds predicted by the deep neural network together with the vehicle's rear axle wheel speed and front wheel angle constitute motion constraints, and the accumulated error of the MEMS-IMU pose solution is corrected through an error state Kalman filter algorithm to obtain an accurate vehicle pose.

[0070] Step 1: Use data from both straight-ahead and turning driving conditions to train a deep neural network. This deep neural network can predict the vehicle's lateral and vertical velocities in real time based on MEMS-IMU measurements, wheel speedometer measurements, and steering angle sensor measurements, and provide the confidence level of the prediction in the form of variance. Specifically, the following steps are performed:

[0071] According to the average yaw angular velocity measured by MEMS-IMU within the time window Determine whether the vehicle is in a straight-line or turning condition, and classify the data measured by MEMS-IMU, wheel speedometer, and steering angle sensor. The calculation is as follows:

[0072]

[0073] Where k and W are the start time and window size of the time window, respectively; Δt is the time interval between two adjacent moments; ω z (i) is the yaw angular velocity measured by the MEMS-IMU at time i. When the speed is greater than the threshold τ, it is turning, otherwise it is going straight. τ is set based on experience according to the data.

[0074] like Figure 2The classified MEMS-IMU measurements, wheel speedometer measurements, and front wheel angle sensor measurements are input into a deep neural network, which outputs the predicted vehicle lateral and vertical velocities, as well as the variance representing the confidence level of the predictions. The deep neural network input is a one-dimensional tensor of size N×9, consisting of the acceleration, angular velocity, rear axle left and right wheel speeds, and the average front wheel angle for N consecutive epochs. The main structure of the deep neural network consists of three end-to-end convolutional layers, each with a convolution kernel of 3 and a dilation factor of 1. The activation function between convolutional layers is ReLU, and the dropout coefficient between each layer is 0.5. The output of the convolutional layer is transformed through a fully connected layer, ultimately outputting the vehicle's lateral and vertical velocities and their corresponding variances.

[0075] The deep neural network performs supervised training, and the output of the deep neural network and the true label value are defined as follows:

[0076]

[0077] Where N is the number of tensors in the input deep neural network, logdet(·) represents the logarithm of the determinant of the matrix, is the covariance matrix corresponding to the i-th input, v i 、v i are the output of the deep neural network and its corresponding label value, In the definition of variance The Adam optimizer is used in the training phase to optimize the weights in the deep neural network model, and the initial learning rate is set to 10 -4 In each training cycle, the predicted output of the neural network model and the corresponding loss function are calculated through forward propagation. The weights of the model are optimized through the backpropagation algorithm and the Adam optimizer. When the loss function converges to within the preset threshold, the training of the neural network model is terminated.

[0078] Step 2: The lateral and vertical velocities predicted by the deep neural network, together with the rear axle wheel speed and front wheel angle, constitute the motion constraints. The error state Kalman filter algorithm is used to correct the accumulated error of the MEMS-IMU pose solution to obtain the accurate vehicle pose. Specifically, the following steps are performed:

[0079] MEMS-IMU raw measurements have acceleration measurement bias a b and angular rate measurement bias ω b , which results in cumulative errors when using MEMS-IMU measurement for inertial positioning. Therefore, the error state variable of vehicle inertial positioning is selected as:

[0080] δX=[(δp)T (δv) T (δθ) T (δa b ) T (δω b ) T ] T

[0081] Where δp, δv and δθ represent the position error, velocity error and attitude error of the vehicle respectively; δa b and δω b They represent the errors of acceleration bias estimation and angular velocity bias estimation respectively.

[0082] The recursive equation for the error state discrete time is established as:

[0083] δp k+1 =δp k +δv k Δt

[0084]

[0085] In the formula, subscripts k and k+1 represent two adjacent moments, and the corresponding time interval is Δt; Represents the rotation matrix from the body coordinate system to the navigation coordinate system and its transpose respectively; a m 、a b They represent the acceleration measurement value and its bias respectively; δθ represents the attitude error in the vehicle body coordinate system; represents the disturbance vector of the velocity error estimate; ω m 、ω b Represent the angular rate measurement value and its bias respectively; represents the disturbance pulse vector of the attitude error estimate; denote the disturbance pulse vectors for acceleration bias estimation and angular velocity bias estimation, respectively.

[0086] According to the error state recursion equation, the prior estimate of the vehicle error state is obtained and its corresponding covariance for:

[0087]

[0088]

[0089] Where I is the 3×3 identity matrix; 0 is the 3×3 zero matrix; Q k represents the variance of the disturbance pulse vector n, represent velocity random walk and angle random walk respectively; Represent the power spectral density of the dynamic zero bias of the accelerometer and gyroscope respectively.

[0090] The lateral and vertical velocities predicted by the deep neural network, the rear axle wheel speed, and the front wheel angle constitute the constraints of the vehicle motion, where the vehicle lateral velocity v lat and vertical velocity v up Obtained based on the output of the deep neural network.

[0091] Vehicle longitudinal velocity constraint v lon Determine the speed of the rear axle wheel of the vehicle by measuring it with a wheel speed meter:

[0092]

[0093] Where, ω l 、ω r are the speeds of the left and right wheels on the rear axle of the vehicle, and r is the wheel radius.

[0094] Vehicle yaw rate constraint γ obs According to the longitudinal speed v lon and the front wheel steering angle δ f Jointly determine:

[0095]

[0096] Where, δ f is the front wheel turning angle of the vehicle, and L is the distance between the front and rear axles of the vehicle.

[0097] Assuming that the vehicle coordinate system is consistent with the body coordinate system of the MEMS-IMU, the three-dimensional velocity constraint v consisting of the longitudinal, lateral and vertical velocities is obs =[v lon v lat v up ] T With the yaw rate constraint γ obs Together they form the following observation equation:

[0098] Z=HδX+R

[0099]

[0100] Where R is the variance of the confidence level corresponding to the three-dimensional velocity constraint and the yaw rate constraint; v INS , γ INS are the three-dimensional nominal velocity and yaw rate with errors in the navigation coordinate system calculated recursively based on the MEMS-IMU; is the rotation matrix from the navigation coordinate system to the vehicle coordinate system; [·]× represents an antisymmetric operation; I and 0 are the unit matrix and the zero matrix, respectively, and the subscripts correspond to the dimensions of the matrix.

[0101] According to the Kalman filter equation, the posterior estimate of the vehicle error state is and its corresponding covariance for:

[0102]

[0103] Where I is the 3×3 identity matrix.

[0104] In summary, the lateral and vertical velocities, the rear axle wheel speed, and the front wheel angle predicted by the deep neural network together constitute the motion constraints, which can periodically correct the accumulated error of MEMS-IMU inertial positioning and obtain a more accurate vehicle posture.

[0105] It will be understood by those skilled in the art that embodiments of the present invention may be provided as methods, systems, or computer program products. Thus, the present invention may take the form of an entirely hardware embodiment, an entirely software embodiment, or an embodiment combining software and hardware. Furthermore, the present invention may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to magnetic disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0106] The present invention is described with reference to flowcharts and / or block diagrams of methods, devices (systems), and computer program products according to embodiments of the present invention. It should be understood that each process and / or block in the flowcharts and / or block diagrams, as well as combinations of processes and / or blocks in the flowcharts and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowcharts and / or block diagrams. Figure 1 a process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0107] These computer program instructions may also be stored in a computer readable memory that can direct a computer or other programmable data processing device to work in a specific manner, so that the instructions stored in the computer readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 a process or multiple processes and / or boxes Figure 1 The function specified in one or more boxes.

[0108] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operational steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing the instructions executed on the computer or other programmable device for implementing the process. Figure 1a process or multiple processes and / or boxes Figure 1 A step that specifies a function in one or more boxes.

[0109] The preferred embodiments of the present invention are described in detail above, but the present invention is not limited to the above embodiments. Various changes can be made within the knowledge of ordinary technicians in this field without departing from the scope of the present invention.

[0110] Many other changes and modifications can be made without departing from the spirit and scope of the present invention. It should be understood that the present invention is not limited to the specific embodiments, and the scope of the present invention is defined by the appended claims.

Claims

1. A method for enhancing inertial positioning of ground vehicles considering motion constraints, characterized in that: The method comprises: S1: Based on the acceleration and angular velocity measured by the MEMS-IMU, a recursive equation is used to calculate the preliminary pose estimate of the ground vehicle and update it over time; S2: Deep neural network training, using data from both straight-ahead and turning driving conditions. The deep neural network predicts the vehicle's lateral and vertical velocities in real time based on MEMS-IMU measurements, wheel speedometer measurements, and steering angle sensor measurements, and provides prediction confidence in the form of variance. S3: Motion constraint construction: Constructing a vehicle motion constraint model by combining the lateral and vertical velocities predicted by the deep neural network, as well as the rear axle wheel speed and front wheel angle; MEMS-IMU raw measurements have acceleration measurement bias a b and angular rate measurement bias ω b , which results in cumulative errors when using MEMS-IMU measurement for inertial positioning. Therefore, the error state variable of vehicle inertial positioning is selected as: δX=[(δp) T (dv) T (sth) T (da) b ) T (see b ) T ] T Where δp, δv and δθ represent the position error, velocity error and attitude error of the vehicle respectively; δa b and δω b They represent the errors of acceleration bias estimation and angular velocity bias estimation respectively; The lateral and vertical velocities predicted by the deep neural network, the rear axle wheel speed, and the front wheel angle constitute the constraints of the vehicle motion, where the vehicle lateral velocity v lat and vertical velocity v up Obtained according to the output of the deep neural network; Vehicle longitudinal velocity constraint v lon Determine the speed of the rear axle wheel of the vehicle by measuring it with a wheel speed meter: Where, ω l 、ω r are the speeds of the left and right wheels on the rear axle of the vehicle, respectively, and r is the wheel radius; Vehicle yaw rate constraint γ obs According to the longitudinal speed v lon and the front wheel steering angle δ f Jointly determine: Where, δ f is the front wheel turning angle of the vehicle, L is the distance between the front and rear axles of the vehicle; Assuming that the vehicle coordinate system is consistent with the body coordinate system of the MEMS-IMU, the three-dimensional velocity constraint v consisting of the longitudinal, lateral and vertical velocities is obs =[v lon v lat v up ] T With the yaw rate constraint γ obs Together they form the following observation equation: Z=HδX+R Where R is the variance of the confidence level corresponding to the three-dimensional velocity constraint and the yaw rate constraint; v INS , γ INS are the three-dimensional nominal velocity and yaw rate with errors in the navigation coordinate system calculated recursively based on the MEMS-IMU; is the rotation matrix from the navigation coordinate system to the vehicle coordinate system; [·]× represents an antisymmetric operation; I and 0 are the unit matrix and the zero matrix, respectively, and the subscripts correspond to the dimensions of the matrices; S4: Error state Kalman filter correction: Based on the preliminary pose estimation and motion constraints, the error state Kalman filter algorithm is applied to correct the accumulated error in the MEMS-IMU pose solution to obtain the corrected vehicle pose.

2. A method for enhancing ground vehicle inertial positioning considering motion constraints according to claim 1, characterized in that: The step S1 specifically includes: The vehicle's linear and angular motion information is measured using a MEMS-IMU. The vehicle's posture is deduced based on the vehicle's initial posture and the posture recursion equation. Assuming that the accelerometer and gyroscope measurements have only one fixed bias, the vehicle's state is called the nominal state. The vehicle's discrete-time nominal posture recursion equation is: v k+1 =v k +[R k (a mk -a bk )+g k ]Δt a b(k+1) =a bk oh b(k+1) =ω bk Where, the subscripts k and k+1 represent two adjacent moments, and the corresponding time interval is Δt; p and v represent the position and velocity of the vehicle in the navigation coordinate system respectively; R represents the rotation matrix from the body coordinate system to the navigation coordinate system; a m 、a b They represent the acceleration measurement value and its bias respectively; g represents the gravity at the vehicle's location; q represents the rotation quaternion from the body coordinate system to the navigation coordinate system; q k {(ω mk -ω bk )Δt} represents the axis angle vector (ω mk -ω bk )Δt corresponding quaternion;ω m 、ω b Represent the angular rate measurement value and its bias respectively; During the actual movement of the vehicle, the error state variables to be estimated are: δX=[(δp) T (dv) T (sth) T (da) b ) T (see b ) T ] T Where δp, δv and δθ represent the position error, velocity error and attitude error of the vehicle respectively; δa b and δω b They represent the errors of acceleration bias estimation and angular velocity bias estimation respectively; The recursive equation for the error state discrete time is established as: δp k+1 =δp k +δv k Δt In the formula, subscripts k and k+1 represent two adjacent moments, and the corresponding time interval is Δt; R, R T Represents the rotation matrix from the body coordinate system to the navigation coordinate system and its transpose respectively; a m 、a b They represent the acceleration measurement value and its bias respectively; δθ represents the attitude error in the vehicle body coordinate system; represents the disturbance vector of the velocity error estimate; ω m 、ω b Represent the angular rate measurement value and its bias respectively; represents the disturbance pulse vector of the attitude error estimate; denote the disturbance pulse vectors for acceleration bias estimation and angular velocity bias estimation, respectively.

3. The method for enhancing ground vehicle inertial positioning considering motion constraints according to claim 1, characterized in that: The step S2 specifically includes: According to the average yaw angular velocity measured by MEMS-IMU within the time window Determine whether the vehicle is in a straight-line or turning condition, and classify the data measured by MEMS-IMU, wheel speedometer, and steering angle sensor. The calculation is as follows: Where k and W are the start time and window size of the time window, respectively; Δt is the time interval between two adjacent moments; ω z (i) is the yaw angular velocity measured by the MEMS-IMU at time i; When the value is greater than the threshold value τ, it is turning, otherwise it is going straight. τ is set according to the data and experience; The classified MEMS-IMU measurements, wheel speedometer measurements, and front wheel angle sensor measurements are input into a deep neural network, which outputs the predicted vehicle lateral and vertical velocities, as well as the variance representing the confidence level of the prediction. The input to the deep neural network is a one-dimensional tensor of size N×9, consisting of the acceleration, angular velocity, rear axle left and right wheel speeds, and average front wheel angle for N consecutive epochs. The main structure of the deep neural network is three end-to-end convolutional layers with a convolution kernel of 3 and a dilation factor of 1. The activation function between convolutional layers is ReLU, and the dropout coefficient between each layer is 0.

5. The output of the convolutional layer is transformed through a fully connected layer to finally output the vehicle's lateral and vertical velocities and the corresponding variances. The deep neural network performs supervised training, and the output of the deep neural network and the true label value are defined as follows: Where N is the number of tensors in the input deep neural network, logdet(·) represents the logarithm of the determinant of the matrix, is the variance corresponding to the i-th output, v i 、 are the output of the deep neural network and its corresponding label value, express The square distance under the norm of ; During the training phase, the Adam optimizer is used to optimize the weights in the deep neural network model, and the initial learning rate is set to 10 -4 In each training cycle, the predicted output of the neural network model and the corresponding loss function are calculated through forward propagation, and the weights of the model are optimized through the backpropagation algorithm and the Adam optimizer. When the loss function converges to within the preset threshold, the training of the neural network model is terminated.

4. The method for enhancing ground vehicle inertial positioning considering motion constraints according to claim 1, characterized in that: The step S3 specifically includes: The recursive equation for the error state discrete time is established as: δp k+1 =δp k +δv k Δt In the formula, subscripts k and k+1 represent two adjacent moments, and the corresponding time interval is Δt; Represents the rotation matrix from the body coordinate system to the navigation coordinate system and its transpose respectively; a m 、a b They represent the acceleration measurement value and its bias respectively; δθ represents the attitude error in the vehicle body coordinate system; represents the disturbance vector of the velocity error estimate; ω m 、ω b Represent the angular rate measurement value and its bias respectively; represents the disturbance pulse vector of the attitude error estimate; denote the disturbance pulse vectors of acceleration bias estimation and angular velocity bias estimation respectively; According to the error state recursion equation, the prior estimate of the vehicle error state is obtained and its corresponding covariance for: Where I is the 3×3 identity matrix; 0 is the 3×3 zero matrix; Q k represents the variance of the disturbance pulse vector n, represent velocity random walk and angle random walk respectively; Represent the power spectral density of the dynamic zero bias of the accelerometer and gyroscope respectively.

5. The method for enhancing ground vehicle inertial positioning considering motion constraints according to claim 4, characterized in that: The step S4 specifically includes: According to the Kalman filter equation, the posterior estimate of the vehicle error state is and its corresponding covariance for: Where I is the 3×3 identity matrix; The lateral and vertical velocities, the vehicle's rear axle wheel speed, and the front wheel angle predicted by the deep neural network together constitute motion constraints, which can periodically correct the accumulated error of MEMS-IMU inertial positioning and obtain a more accurate vehicle posture.

Images