Ground vehicle inertial positioning enhancement method considering motion constraint

Abstract

The invention discloses a ground vehicle inertial positioning enhancement method considering motion constraints. The method comprises the following steps: carrying out ground vehicle pose recursion calculation based on acceleration and angular velocity measured by a micro-electro-mechanical inertial measurement unit (MEMS-IMU); the method comprises the following steps: training a deep neural network by using data under two driving working conditions of straight running and turning of a vehicle, so that the deep neural network can predict the transverse and vertical speeds of the vehicle in real time according to MEMS-IMU measurement, wheel speed meter measurement and angle sensor measurement, and give the prediction confidence in the form of variance; and the transverse and vertical speeds predicted by the deep neural network, the rotating speed of the rear axle wheel of the vehicle and the rotating angle of the front wheel of the vehicle jointly form motion constraint, and an accumulated error of MEMS-IMU pose calculation is corrected through an error state Kalman filtering algorithm to obtain an accurate vehicle pose. According to the invention, the accumulated positioning error of ground vehicle inertial positioning can be corrected, and the precision of ground vehicle inertial positioning is improved.

Description

Technical Field

[0001] The 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 unmanned vehicles are considered to have great potential in improving road safety and traffic operation efficiency, and have received extensive attention and research in recent years. At present, unmanned vehicles that use RTK-GNSS and low-cost MEMS-IMU for combined navigation and positioning can achieve centimeter-level positioning that meets driving requirements under ideal conditions. However, in urban environments, when tall buildings block GNSS signals, combined navigation degenerates into inertial positioning, and MEMS-IMU inertial positioning has cumulative errors, which will gradually increase over time, eventually causing the positioning results to diverge and become unusable.

[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 effects of these methods are highly dependent on the setting of thresholds, the process is complicated, and good results cannot be guaranteed under different loads 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 abnormal values ​​with uncontrollable 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 two driving conditions, straight driving and turning, to train the deep neural network. The deep neural network predicts the lateral and vertical speeds of the vehicle in real time based on MEMS-IMU measurements, wheel speedometer measurements, and steering angle sensor measurements, and provides the confidence of the prediction in the form of variance;

[0009] S3: Motion constraint construction: The vehicle motion constraint model is constructed by combining the lateral and vertical speeds predicted by the deep neural network as well as the rear axle wheel speed and front wheel steering angle;

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

[0011] Further, the step S1 specifically includes:

[0012] The vehicle linear motion information and angular motion information are measured by MEMS-IMU. The vehicle's posture is deduced based on the vehicle's initial posture and 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 nominal posture recursion equation of the vehicle in discrete time 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] In the formula, 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 to the quaternion; ω m ,ω b Respectively represent the angular rate measurement value and its bias;

[0019] During the actual movement of the vehicle, the error state variable to be estimated is selected as:

[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, the subscripts k and k+1 represent two adjacent moments, and the corresponding time interval is Δt; R, R T Respectively represent the rotation matrix from the body coordinate system to the navigation coordinate system and its transpose; 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 velocity error estimation; ω m ,ω b Respectively represent the angular rate measurement value and its bias; The disturbance pulse vector representing the attitude error estimate; denote the disturbance pulse vectors of acceleration bias estimation and angular velocity bias estimation respectively.

[0026] Further, the step S2 specifically includes:

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

[0028]

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

[0030] The classified MEMS-IMU measurements, wheel speed meter measurements and front wheel steering sensor measurements are input into the deep neural network, and the deep neural network outputs the predicted lateral and vertical speeds of the vehicle and the variance representing the prediction confidence; the input of the deep neural network is a one-dimensional tensor of size N×9, which consists of the acceleration, angular velocity, left and right wheel speeds of the rear axle and the average steering angle of the front wheel for N consecutive epochs; the main structure of the deep neural network is three end-to-end convolutional layers, the convolution kernel of the convolutional layer is 3, the expansion coefficient is 1, the activation function between the convolutional layers is ReLU, and the Dropout coefficient between each layer is 0.5; the output of the convolutional layer is transformed in dimension through the fully connected layer, and finally the lateral and vertical speeds of the vehicle and the corresponding variance are output;

[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 variance definition 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 back propagation 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] Further, the step S3 specifically includes:

[0035] MEMS-IMU raw measurements have acceleration measurement bias a b and angular rate measurement bias ω b , which leads to cumulative errors when using MEMS-IMU to measure 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; Respectively represent the rotation matrix from the body coordinate system to the navigation coordinate system and its transpose; 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 velocity error estimation; ω m ,ω b Respectively represent the angular rate measurement value and its bias; 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, denote velocity random walk and angle random walk respectively; Represent the power spectral density of the dynamic 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 steering angle of the vehicle 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] In the formula, ω 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] In the formula, δ 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 They 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] Further, 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 unit matrix;

[0060] The lateral and vertical velocities, the wheel speed of the rear axle, and the front wheel steering angle predicted by the deep neural network together constitute the motion constraints, which can periodically correct the accumulated errors 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 the 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 , flow chart of inertial positioning enhancement method;

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

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

[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 so that people familiar with this technology can understand and read them, 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 effects and purposes that can be achieved by the present invention.

[0067] At the same time, the terms such as "upper", "lower", "left", "right", "middle" and "one" cited 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] Embodiment 1:

[0069] Reference Figure 1 The present invention proposes a method for enhancing inertial positioning of a ground vehicle considering 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 and the rear axle wheel speed and front wheel steering angle of the vehicle together constitute motion constraints, and the accumulated error of MEMS-IMU posture solution is corrected by an error state Kalman filter algorithm to obtain an accurate vehicle posture.

[0070] Step 1: Use the data of the vehicle under two driving conditions, straight driving and turning, to train the deep neural network, so that the deep neural network can predict the lateral and vertical speeds of the vehicle in real time based on the MEMS-IMU measurement, wheel speed meter measurement and steering angle sensor measurement, and give the prediction confidence in the form of variance, including:

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

[0072]

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

[0074] like Figure 2As described, the classified MEMS-IMU measurements, wheel speed meter measurements and front wheel steering sensor measurements are input into a deep neural network, and the deep neural network outputs the predicted lateral and vertical speeds of the vehicle and the variance representing the prediction confidence. The input of the deep neural network is a one-dimensional tensor of size N×9, which consists of the acceleration, angular velocity, left and right wheel speeds of the rear axle and the average steering angle of the front wheel for N consecutive epochs. The main structure of the deep neural network is three end-to-end convolutional layers, the convolution kernel of the convolutional layer is 3, the expansion coefficient is 1, the activation function between the convolutional layers is ReLU, and the Dropout coefficient between each layer is 0.5. The output of the convolutional layer is transformed in dimension through the fully connected layer, and finally the lateral and vertical speeds of the vehicle and the corresponding variance are output.

[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 variance definition 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, and the weights of the model are optimized through the back propagation 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 steering angle of the vehicle constitute motion constraints. The accumulated error of the MEMS-IMU posture solution is corrected by the error state Kalman filter algorithm to obtain the accurate vehicle posture, including:

[0079] MEMS-IMU raw measurements have acceleration measurement bias a b and angular rate measurement bias ω b , which leads to cumulative errors when using MEMS-IMU to measure 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; Respectively represent the rotation matrix from the body coordinate system to the navigation coordinate system and its transpose; 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 velocity error estimation; ω m ,ω b Respectively represent the angular rate measurement value and its bias; The disturbance pulse vector representing the attitude error estimate; denote the disturbance pulse vectors of 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, denote 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 steering angle of the vehicle 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] In the formula, ω l ,ω r are the left and right wheel speeds of the vehicle's rear axle, 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] In the formula, δ 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 They 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 a 3×3 unit matrix.

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

[0105] Those skilled in the art will appreciate that embodiments of the present invention may be provided as methods, systems, or computer program products. Therefore, the present invention may take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Moreover, 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 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 flowchart and / or block diagram, as well as the combination of processes and / or blocks in the flowchart and / or block diagram, 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 flowchart and / or block diagram. 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 capable of directing a computer or other programmable data processing device to operate 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 A 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 operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process. Figure 1A process or multiple processes and / or boxes Figure 1 The steps for the functions specified 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, and various changes can be made within the knowledge scope of ordinary technicians in this field without departing from the purpose of the present invention.

[0110] Many other changes and modifications may be made without departing from the concept 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 a ground vehicle 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 two driving conditions, straight driving and turning, to train the deep neural network. The deep neural network predicts the lateral and vertical speeds of the vehicle in real time based on MEMS-IMU measurements, wheel speedometer measurements, and steering angle sensor measurements, and provides the confidence of the prediction in the form of variance; S3: Motion constraint construction: The vehicle motion constraint model is constructed by combining the lateral and vertical speeds predicted by the deep neural network as well as the rear axle wheel speed and front wheel steering angle; S4: Error state Kalman filter correction: Based on the preliminary posture estimation and motion constraints, the accumulated error in the MEMS-IMU posture solution is corrected by applying the error state Kalman filter algorithm to obtain the corrected vehicle posture.

2. A method for enhancing inertial positioning of a ground vehicle considering motion constraints according to claim 1, characterized in that: The step S1 specifically includes: The vehicle linear motion information and angular motion information are measured by MEMS-IMU. The vehicle's posture is deduced based on the vehicle's initial posture and 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 nominal posture recursion equation of the vehicle in discrete time 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 In the formula, 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 to the quaternion; ω m ,ω b Respectively represent the angular rate measurement value and its bias; During the actual movement of the vehicle, the error state variable to be estimated 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 recursive equation for the error state discrete time is established as: δp k+1 =δp k +δv k Δt In the formula, the subscripts k and k+1 represent two adjacent moments, and the corresponding time interval is Δt; R, R T Respectively represent the rotation matrix from the body coordinate system to the navigation coordinate system and its transpose; 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 velocity error estimation; ω m ,ω b Respectively represent the angular rate measurement value and its bias; The disturbance pulse vector representing the attitude error estimate; denote the disturbance pulse vectors of acceleration bias estimation and angular velocity bias estimation respectively.

3. The method for enhancing inertial positioning of a ground vehicle considering motion constraints according to claim 1, characterized in that: The step S2 specifically includes: According to the average yaw rate measured by MEMS-IMU within the time window Determine whether the vehicle is in a straight-ahead or turning condition, and classify the data measured by MEMS-IMU, wheel speedometer, and steering angle sensor. The calculation of is as follows: In the formula, k and W are the start time and window size of the time window respectively, Δt is the time interval between two adjacent moments, and ω z (i) is the yaw angular velocity measured by the MEMS-IMU at time i; When it is greater than the threshold value τ, it is turning, otherwise it is going straight, and τ is set according to the data and experience; The classified MEMS-IMU measurements, wheel speed meter measurements and front wheel steering sensor measurements are input into the deep neural network, and the deep neural network outputs the predicted lateral and vertical speeds of the vehicle and the variance representing the prediction confidence; the input of the deep neural network is a one-dimensional tensor of size N×9, which consists of the acceleration, angular velocity, left and right wheel speeds of the rear axle and the average steering angle of the front wheel for N consecutive epochs; the main structure of the deep neural network is three end-to-end convolutional layers, the convolution kernel of the convolutional layer is 3, the expansion coefficient is 1, the activation function between the convolutional layers is ReLU, and the Dropout coefficient between each layer is 0.5; the output of the convolutional layer is transformed in dimension through the fully connected layer, and finally the lateral and vertical speeds of the vehicle and the corresponding variance are output; 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 input, v i 、v i are the output of the deep neural network and its corresponding label value, In the variance definition 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 back propagation 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 inertial positioning of a ground vehicle considering motion constraints according to claim 1, characterized in that: The step S3 specifically includes: MEMS-IMU raw measurements have acceleration measurement bias a b and angular rate measurement bias ω b , which leads to cumulative errors when using MEMS-IMU to measure 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 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; Respectively represent the rotation matrix from the body coordinate system to the navigation coordinate system and its transpose; 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 velocity error estimation; ω m ,ω b Respectively represent the angular rate measurement value and its bias; The disturbance pulse vector representing 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, denote velocity random walk and angle random walk respectively; Represent the power spectral density of the dynamic bias of the accelerometer and gyroscope respectively; The lateral and vertical velocities predicted by the deep neural network, the rear axle wheel speed, and the front wheel steering angle of the vehicle 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: In the formula, ω 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: In the formula, δ 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 They 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.

5. The method for enhancing inertial positioning of a ground vehicle considering motion constraints according to claim 1, 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 unit matrix; The lateral and vertical velocities, the wheel speed of the rear axle, and the front wheel steering angle predicted by the deep neural network together constitute the motion constraints, which can periodically correct the accumulated errors of MEMS-IMU inertial positioning and obtain a more accurate vehicle posture.

Images