Odometer sideslip online compensation integrated navigation method based on FPGA hardware acceleration

Abstract

The invention provides a speedometer sideslip online compensation integrated navigation method based on FPGA (Field Programmable Gate Array) hardware acceleration. Relates to the technical field of train navigation positioning. According to the SINS / DR integrated navigation system, an IMU inertial measurement unit is integrated in an SINS inertial navigation module and specifically comprises a three-axis gyroscope and a three-axis accelerometer, in SINS / DR integrated navigation, mapping from sensor data of the gyroscope, the accelerometer, an odometer and the like to a slip mode is achieved by adopting a Sheng differential equation model, and the SINS / DR integrated navigation system is obtained. Two speedometers are installed on non-steering wheels on the two sides respectively, a speedometer speed error model is established to compensate and correct the actual speed, and acceleration of a Kalman filtering algorithm is achieved through an FPGA.

Description

Technical Field

[0001] The invention relates to the technical field of train navigation and positioning, and in particular to an odometer sideslip online compensation combined navigation method based on FPGA hardware acceleration. Background Art

[0002] The present invention relates to an online compensation method for odometer sideslip based on FPGA hardware acceleration in a combined navigation application, and belongs to the field of navigation and positioning technology. With the rapid development of modern navigation technology, strapdown inertial navigation system (SINS) is widely used in vehicles, unmanned driving, railways and other fields due to its high autonomy and real-time performance. Since SINS is limited by the drift and noise of inertial sensors, its positioning error will accumulate rapidly over time, so a mode of combining SINS and odometer (Odometer, ODO) is often used for navigation and positioning. The SINS system is fixedly installed relative to the vehicle body, and its attitude matrix can be used to convert the mileage information measured in the vehicle coordinate system to the navigation coordinate system in real time, and then positioning and solving are performed, effectively suppressing the rapid accumulation of SINS system errors. However, if the driving road condition is bad, the odometer is prone to slippage or sliding failure, and the dead reckoning (DR) error becomes larger due to inaccurate modeling, thereby reducing the precision of the combined navigation system. Traditionally, when the odometer slips or slides, the integrated navigation system often relies only on SINS and discards the odometer data. Although SINS can provide accurate position estimation in a short time, the error of the inertial sensor itself will accumulate over time due to the noise and drift effect, resulting in a significant decrease in the accuracy of the integrated system. Therefore, a method for online monitoring of the slip state of the odometer using a Neural Ordinary Differential Equations network model is proposed, and the error caused by slip is compensated by the control system, thereby improving the positioning accuracy and adaptability of the entire integrated navigation system.

[0003] As the requirements of integrated navigation systems for real-time performance and high precision continue to increase, traditional software calculation methods can no longer meet the needs of high-frequency data processing and complex calculations. Therefore, hardware acceleration of the Kalman filter algorithm has become an effective way to improve system performance. By adopting hardware acceleration technologies such as graphics processing units (GPUs), application-specific integrated circuits (ASICs), and field programmable gate arrays (FPGAs), the calculation efficiency of the Kalman filter can be significantly improved, and the stability and robustness of the integrated navigation system in complex environments can be enhanced. Among them, GPUs are widely used in fields such as image processing, but their high power consumption and resource consumption are not suitable for scenarios such as navigation systems that require high energy efficiency. Although ASICs provide the best performance, their long development cycle and high design complexity limit their application in systems with high requirements for rapid iteration and customization. In contrast, FPGAs have achieved a relatively balanced advantage between performance, power consumption, and design flexibility, and can simultaneously undertake multiple functions such as signal acquisition, data conversion, storage, input / output, and provide efficient information processing auxiliary modules. Therefore, a Kalman filter algorithm acceleration method based on FPGA is proposed, which is mainly divided into matrix operation optimization and pipeline structure design. By optimizing the sparse matrix and improving the Cholesky decomposition algorithm, which has problems such as slow speed and high resources when inverting the matrix, the data with large data scale such as the decomposed lower triangular matrix elements are stored in the external memory, which reduces the dependence of the matrix inversion operation on the FPGA's internal BRAM, improves the operation speed, optimizes its hardware implementation structure, and reduces hardware resource consumption. Summary of the invention

[0004] The purpose of the present invention is to solve the above-mentioned problem and to provide an odometer sideslip online compensation integrated navigation method based on FPGA hardware acceleration.

[0005] To achieve the above object, the technical solution adopted by the present invention is:

[0006] According to one aspect of the present invention, an odometer sideslip online compensation integrated navigation method based on FPGA hardware acceleration is provided, including an IMU inertial measurement unit integrated inside a SINS inertial navigation module, the IMU inertial measurement unit including a three-axis gyroscope and a three-axis accelerometer, characterized in that, in the SINS / DR integrated navigation, a neural ordinary differential equation model is used to realize the mapping from sensor data such as a gyroscope, an accelerometer and an odometer to a slip mode, two odometers are respectively installed on non-steering wheels on both sides, an odometer speed error model is established to compensate and correct the actual speed, and then the Kalman filter algorithm is accelerated by FPGA, and the specific contents are as follows:

[0007] Step 1: The IMU inertial measurement module outputs the three-axis accelerometer and three-axis gyroscope data, and its mean and variance are used as the downsampling layer of the neural ordinary differential equation, which is calculated as follows:

[0008]

[0009] Where k is the training sample number, N is the number of sampling points, the system preset slip recognition frequency is 1Hz, and the sensor data sampling frequency is 125Hz, so N can be set to 125 here. i Raw data collected for the inertial measurement unit:

[0010] IMU i =[acc x ,acc y ,acc z ,gyr x ,gyr y ,gyr z ] T

[0011] where acc j ,gyr j (j=x, y, z) are the acceleration and angular velocity data of the X-axis, Y-axis, and Z-axis directions in the carrier coordinate system collected by the inertial measurement unit, respectively;

[0012] Step 2: The integrated navigation system includes two odometers, which measure the mileage information of the left and right non-steering wheels respectively. The feature vector based on the odometer is defined as the differential mean of the odometer:

[0013]

[0014] where v L , v R They are the speed data output by the left and right odometers respectively;

[0015] In the actual vehicle driving process, the downsampling layer of the neural ordinary differential equation is determined to be 9 dimensions:

[0016]

[0017] Step 3: The speed output of the odometer can be expressed in the navigation coordinate system as

[0018]

[0019]

[0020] in Indicates the speed of the odometer on the Y axis in the carrier coordinate system, is the attitude transformation matrix from the carrier coordinate system to the navigation coordinate system, which can be obtained by SINS navigation solution.

[0021] Furthermore, the error compensation principle of the odometer includes single-side slip and double-side slip;

[0022] When the vehicle slips on one side, the following analysis is based on the slip on the right side, assuming that the speed error caused by the slip is δv R , the actual speed and speed error caused by the right wheel slip are as follows:

[0023]

[0024] At this time, the speed error caused by directly replacing the left odometer with the original odometer speed is:

[0025]

[0026] When the error is not zero, a one-sided slip occurs. At this time, the control system can be used to actively control the wheel speeds on both sides to be equal.

[0027] When one-side slip occurs and the speeds of the wheels on both sides have not been corrected, the rotational angular velocity of the vehicle between two adjacent moments is

[0028]

[0029] where d L is the wheelbase of the non-steering wheels on both sides of the vehicle. At this time, the vehicle speed is compensated as

[0030]

[0031] Since SINS can maintain high accuracy in a short time, the rotation angular velocity ω of the vehicle D The gyroscope Z-axis measurement value ω can be approximated Z The vehicle speed compensation formula is as follows:

[0032]

[0033] Similarly, when the left wheel slips, the vehicle speed can be compensated by the following formula:

[0034]

[0035] When the vehicle skids on both sides, since the SINS is constantly corrected by the odometer speed when the vehicle is in good driving condition, it can maintain a high accuracy in a short period of time. Therefore, the speed of the vehicle in the navigation coordinate system is as follows:

[0036]

[0037] At this time, a threshold can be set for the speed of the vehicle Constrain the speed of SINS:

[0038]

[0039] This reduces the impact of SINS velocity accumulation error on positioning accuracy.

[0040] Furthermore, the ODO and SINS are not installed at the same position on the vehicle, that is, the odometer coordinate system m and the vehicle coordinate system b do not overlap, and there is a small installation error angle, denoted by α = [α θ α γ α ψ ] T , each component represents the pitch installation error angle, roll installation error angle and heading installation error angle, so there is an error matrix from the b system to the m system:

[0041]

[0042] Further, including:

[0043] Step 1: During the actual driving of the vehicle, the odometer has a scale coefficient error δK D , the odometer speed after considering the odometer scale coefficient error is expressed as:

[0044]

[0045] Step 2: Consider the attitude error of SINS solution Then the odometer speed in the navigation coordinate system is expressed as:

[0046]

[0047] Among them C ij (i,j=1,2,3) is the attitude matrix the various elements of

[0048] Step 3: From the velocity equation in step 2, it can be seen that the roll angle installation error will not affect the measurement accuracy of the odometer, as shown in the following formula:

[0049]

[0050] Furthermore, a simplified SINS algorithm and error equation are adopted to realize FPGA-based hardware acceleration, as follows:

[0051] Step 1: The simplified strapdown attitude update algorithm is:

[0052]

[0053] in

[0054]

[0055] Indicates t m The quaternion of the attitude transformation at the moment, It is from t m-1 Time to t m The quaternion change of attitude at the moment, Δθ m is the gyroscope in the time period [t m-1 ,t m ] The angular increment and modulus Δθ of the output m =|Δθ m |;

[0056] Step 2: The simplified strapdown velocity update equation is:

[0057]

[0058] in

[0059]

[0060] t m SINS speed at the moment, For quaternion The corresponding attitude matrix, Δv m is the accelerometer in the time period [t m-1 ,t m ] is the specific force increment outputted within the system. In practice, the specific force output is directly multiplied by the sampling interval for approximation.

[0061] Step 3: The simplified strapdown position update equation is:

[0062]

[0063] In the formula: In meters;

[0064] Step 4: The simplified SINS system error equation is as follows:

[0065]

[0066] in: and They are the gyroscope angular rate white noise and the accelerometer specific force white noise respectively; and are the first-order Markov process errors of the gyroscope and accelerometer, respectively, as follows:

[0067]

[0068] Where: τ gi and τ ai (i = x, y, z) is the relevant time constant; and is a first-order Markov process exciting white noise.

[0069] Furthermore, in the integrated navigation system, the SINS and ODO data are fused using the Kalman filter algorithm, as follows:

[0070] Step 1: The Kalman filter state space model of the integrated navigation system is:

[0071]

[0072] Where F is the state transfer matrix of the Kalman filter, G and w are the noise driving matrix and noise matrix, H and V are the measurement matrix and measurement white noise respectively;

[0073] Step 2: Define the 18-dimensional state vector of the integrated navigation system for Kalman filter estimation, as follows:

[0074]

[0075] Step 3: According to the Kalman filter principle, iterative updates are performed to estimate the error in real time, and the error quantities estimated by the Kalman filter are fed back to the corresponding variables to obtain compensated navigation information.

[0076] Furthermore, the improved Cholesky decomposition is used for the matrix inversion operation, and the implementation process is divided into three modules: matrix decomposition, lower triangular matrix inversion, and triangular matrix multiplication, as follows:

[0077] Step 1: Use Cholesky decomposition to decompose the symmetric positive definite matrix A to obtain L, D and L H Right now

[0078] A=LDL H

[0079] in

[0080]

[0081]

[0082] According to the matrix operation rules, we can get:

[0083]

[0084] Step 2: Invert the lower triangular matrix L to obtain the inverse matrix B = L -1

[0085]

[0086] Step 3: Calculate the inverse matrix D of the diagonal matrix D -1

[0087]

[0088] Step 4: Calculate the inverse matrix A of A -1

[0089] A -1 =(LDL H ) -1 =(L -1 ) H D -1 L -1 .

[0090] Compared with the prior art, the present invention has the following beneficial effects:

[0091] The invention provides an odometer sideslip online compensation combined navigation method based on FPGA hardware acceleration, which is easy to use, maintain, extend and popularize.

[0092] The invention proposes an odometer sideslip online compensation integrated navigation method based on FPGA hardware acceleration, which can monitor the odometer slip in real time online and perform state identification and compensation correction, thereby improving the integrated navigation accuracy.

[0093] The present invention proposes an odometer sideslip online compensation integrated navigation method based on FPGA hardware acceleration, which uses a total of nine original collected data of accelerometer, gyroscope and odometer as the downsampling layer of the neural ordinary differential equation model, and three vehicle driving slip states as the feedforward layer, thereby enhancing the coupling of model input and output and improving the accuracy of the system in identifying the odometer operating state.

[0094] The invention proposes an odometer sideslip online compensation integrated navigation method based on FPGA hardware acceleration. The simplified SINS algorithm is used in the integrated navigation system of low-precision gyroscope. SINS and ODO use Kalman filtering algorithm for data fusion, which further optimizes and reduces the computational complexity of the integrated navigation algorithm.

[0095] The invention proposes an odometer sideslip online compensation integrated navigation method based on FPGA hardware acceleration, which selects an improved Cholesky decomposition algorithm to implement the matrix inversion process. The hardware implementation structure is relatively simple, the complexity is low, it is suitable for high-order operations, and the stability and parallelism are good. FPGA is used to implement hardware acceleration, thereby improving the real-time performance and accuracy of the integrated navigation system. BRIEF DESCRIPTION OF THE DRAWINGS

[0096] Figure 1 The present invention is a diagram of the odometer slipping state identified by the Godly Ordinary Differential Equation model;

[0097] Figure 2 It is a SINS algorithm block diagram of the present invention;

[0098] Figure 3 is a diagram showing the relationship between the vehicle space angle positions of the present invention;

[0099] Figure 4 It is the overall block diagram of the integrated navigation system of the present invention;

[0100] Figure 5 It is a hardware design diagram of the triangular decomposition in the improved Cholesky decomposition of the present invention. DETAILED DESCRIPTION

[0101] In order to make the technical means, creative features, objectives and effects achieved by the present invention easy to understand, the present invention is further explained below in conjunction with specific implementation methods.

[0102] like Figure 1 As shown, the present invention provides an odometer sideslip online compensation integrated navigation method based on FPGA hardware acceleration, wherein an IMU inertial measurement unit is integrated inside the SINS inertial navigation module, specifically including a three-axis gyroscope and a three-axis accelerometer. In the SINS / DR combined navigation, a neural ordinary differential equation model is used to realize the mapping from sensor data such as gyroscopes, accelerometers and odometers to the slip mode. Two odometers are respectively installed on the non-steering wheels on both sides, and an odometer speed error model is established to compensate and correct the actual speed. The acceleration of the Kalman filter algorithm is realized by FPGA.

[0103] The specific steps include:

[0104] Step 1: The IMU inertial measurement module outputs the three-axis accelerometer and three-axis gyroscope data, and its mean (MV) and variance (VAR) are used as the downsampling layer of the neural ordinary differential equation, which is calculated as follows:

[0105]

[0106] Where k is the training sample number, N is the number of sampling points, the system preset slip recognition frequency is 1Hz, and the sensor data sampling frequency is 125Hz, so N can be set to 125 here. i Raw data collected for the inertial measurement unit:

[0107] IMU i =[acc x ,acc y ,acc z ,gyr x ,gyr y ,gyr z ] T

[0108] where acc j ,gyr j (j=x, y, z) are the acceleration and angular velocity data of the X-axis, Y-axis and Z-axis directions in the carrier coordinate system collected by the inertial measurement unit.

[0109] Step 2: The integrated navigation system includes two odometers, which measure the mileage information of the left and right non-steering wheels respectively. The feature vector based on the odometer is defined as the differential mean of the odometer:

[0110]

[0111] where v L , v R They are the speed data output by the left and right odometers respectively.

[0112] In the actual driving process of the vehicle, the X-axis gyroscope signal corresponds to the angular velocity of the vehicle body in the pitch direction, which cannot well reflect the angle information of the vehicle slipping. Therefore, the downsampling layer of the neural ordinary differential equation is determined to be 9 dimensions:

[0113]

[0114] Step 3: The speed output of the odometer can be expressed in the navigation coordinate system as

[0115]

[0116] in It represents the speed of the odometer on the Y axis in the carrier coordinate system (i.e. the actual vehicle speed). is the attitude transformation matrix from the carrier coordinate system to the navigation coordinate system, which can be obtained by SINS navigation solution.

[0117] The odometer error compensation principle is mainly divided into single-sided slip and double-sided slip.

[0118] When the vehicle slips on one side, the following analysis is based on the slip on the right side, assuming that the speed error caused by the slip is δv R , then the actual speed and speed error caused by the right wheel slip are

[0119]

[0120] If the original odometer speed is used to directly replace the left odometer, the speed error is

[0121]

[0122] It can be seen from the above formula that when the speeds of the wheels on both sides are equal, that is, there is no slippage, the speed error is zero. When the error is not zero, it means that one side is slipping. At this time, the control system can be used to actively control the speeds of the wheels on both sides to be equal.

[0123] When one-side slip occurs and the speeds of the wheels on both sides have not been corrected, the rotational angular velocity of the vehicle between two adjacent moments is

[0124]

[0125] where d L is the wheelbase of the non-steering wheels on both sides of the vehicle. At this time, the vehicle speed is compensated to

[0126]

[0127] Since SINS can maintain high accuracy in a short time, the rotation angular velocity ω of the vehicle D The gyroscope Z-axis measurement value ω can be approximated Z Replace

[0128]

[0129] Similarly, when the left wheel slips, the vehicle speed can be compensated by the following formula:

[0130]

[0131] When the vehicle skids on both sides, since the SINS is constantly corrected by the odometer speed when the vehicle is in good driving condition, it can maintain a high accuracy in a short time. Therefore, the speed of the vehicle in the navigation coordinate system can be obtained:

[0132]

[0133] At this time, a threshold can be set for the speed of the vehicle Constrain the speed of SINS:

[0134]

[0135] This reduces the impact of SINS velocity accumulation error on positioning accuracy.

[0136] ODO and SINS are both fixed on the vehicle, but their installation positions are inconsistent, that is, the odometer coordinate system m and the vehicle coordinate system b do not coincide, and there is a small installation error angle, denoted by α = [α θ α γ α ψ ] T , each component represents the pitch installation error angle, roll installation error angle and heading installation error angle, so there is an error matrix from the b system to the m system:

[0137]

[0138] include:

[0139] Step 1: During the actual driving of the vehicle, the odometer has a scale coefficient error δK D , the odometer speed after considering the odometer scale coefficient error can be expressed as

[0140]

[0141] Step 2: Consider the attitude error of SINS solution Then the odometer speed in the navigation coordinate system can be expressed as:

[0142]

[0143] Among them C ij (i,j=1,2,3) is the attitude matrix elements of.

[0144] Step 3: From the velocity equation in step 2, we can get that the roll angle installation error will not affect the measurement accuracy of the odometer, so we can get

[0145]

[0146] Low-precision gyroscopes cannot be sensitive to the Earth's rotation information, and the SINS update algorithm is complex and computationally intensive. Simplified SINS algorithms and error equations are adopted to reduce the amount of computation, thereby achieving FPGA-based hardware acceleration, including:

[0147] Step 1: The simplified strapdown attitude update algorithm is

[0148]

[0149] in

[0150]

[0151] Indicates t m The quaternion of the attitude transformation at the moment, It is from t m-1 Time to t m The quaternion change of attitude at the moment, Δθ m is the gyroscope in the time period [t m-1 ,t m ] The angular increment and modulus Δθ of the output m =|Δθ m |. Low-precision gyroscopes use angular rate output sampling, which only needs to be multiplied by the sampling interval T s =t m -t m-1 , which can be approximately transformed into an angular increment.

[0152] Step 2: The simplified strapdown velocity update equation is

[0153]

[0154] in

[0155]

[0156] t m SINS speed at the moment, For quaternion The corresponding attitude matrix, Δv m is the accelerometer in the time period [t m-1 ,t m ] is the specific force increment output within the range. In practice, the specific force output is directly multiplied by the sampling interval for approximation.

[0157] Step 3: The simplified strapdown position update equation is

[0158]

[0159] In the formula: The unit is meters.

[0160] Step 4: The simplified SINS system error equation is as follows:

[0161]

[0162] in: and They are the gyroscope angular rate white noise and the accelerometer specific force white noise respectively; and are the first-order Markov process errors of the gyroscope and accelerometer, respectively, as follows:

[0163]

[0164] Where: τ gi and τ ai (i = x, y, z) is the relevant time constant; and is a first-order Markov process exciting white noise.

[0165] Compared with the random constant model, the first-order Markov model can avoid the over-convergence of the filter after a long period of combined filtering, which will lead to the deterioration of the filter's anti-interference performance. When there is a large random constant component in the SINS error, the influence of the random constant error can be eliminated through the filter's inertial sensor error feedback correction, thereby reducing the modeling dimension and filtering calculation amount.

[0166] In the integrated navigation system, SINS and ODO data are usually fused using the KF algorithm. The error equation provides a basic framework for the filtering process by describing the linear relationship between various error sources, which effectively promotes the dynamic estimation and compensation of system errors.

[0167] include:

[0168] Step 1: The Kalman filter state space model of the integrated navigation system is

[0169]

[0170] Where F is the state transfer matrix of the Kalman filter, G and w are the noise driving matrix and noise matrix, H and V are the measurement matrix and measurement white noise respectively.

[0171] Step 2: Define the 18-dimensional state vector of the integrated navigation system for Kalman filter estimation

[0172]

[0173] Step 3: According to the Kalman filter principle, iterative updates are performed to estimate the error in real time. The error values ​​estimated by the Kalman filter are fed back to the corresponding variables to obtain the compensated navigation information.

[0174] For the matrix inversion operation, the improved Cholesky decomposition is used to divide the implementation process into three modules: matrix decomposition, lower triangular matrix inversion and triangular matrix multiplication.

[0175] Step 1: Use Cholesky decomposition to decompose the symmetric positive definite matrix A to obtain L, D and LH Right now

[0176]

[0177] in

[0178]

[0179] According to the matrix operation rules, we can get:

[0180]

[0181] Step 2: Invert the lower triangular matrix L to obtain the inverse matrix B = L -1

[0182]

[0183] Step 3: Calculate the inverse matrix D of the diagonal matrix D -1

[0184]

[0185] Step 4: Calculate the inverse matrix A of A -1

[0186] A -1 =(LDL H ) -1 =(L -1 ) H D -1 L -1

[0187] like Figure 1 As shown, the odometer sideslip online compensation combined navigation method based on FPGA hardware acceleration studied in the present invention, the designed neural ordinary differential equation model is used to identify the odometer slip state, the nine dimensions of the downsampling layer are the mean and variance of the Y / Z axis of the accelerometer and gyroscope and the original collected data of the speed difference mean output of the left and right non-steering wheels measured by the odometer, and the feedforward layer is based on the three states of the odometer sideslip during the vehicle driving process, namely right slip, left slip and bilateral slip.

[0188] like Figure 2 As shown, the odometer sideslip online compensation integrated navigation method based on FPGA hardware acceleration studied in the present invention, the main calculation block diagram of the SINS update algorithm is as follows Figure 3 As shown, it is t m-1 The posture array of the moment speed Position P m-1 And from t m-1 Time to t m Gyroscope angle increment sampling Δθ at timemi , accelerometer velocity increment sampling Δv mi As input, solve to obtain t m The posture array of the moment speed and position P m Output.

[0189] like Figure 3 As shown, the odometer sideslip online compensation combined navigation method based on FPGA hardware acceleration studied by the present invention, the attitude of the vehicle is usually represented by three attitude angles: heading angle ψ, pitch angle θ and roll angle γ. These three angles are determined by the rotation relationship between the vehicle carrier coordinate system and the navigation coordinate system. The complex angular position relationship between the two coordinate systems can be regarded as a composite of a finite number of basic rotations. The rotation of the vehicle from the navigation coordinate system to the carrier coordinate system can be regarded as a composite of three independent rotations.

[0190] like Figure 4 As shown in the figure, the odometer sideslip online compensation combined navigation method based on FPGA hardware acceleration studied in the present invention, SINS first calculates the attitude angle of the carrier according to the three-axis gyroscope data, and then calculates the speed under pure inertia in combination with the three-axis accelerometer information; at the same time, the odometer information and attitude angle data are used to calculate the odometer speed, and the speed difference between the two is used as the observed value for Kalman filtering. The MV of the Y / Z axis data collected by the accelerometer and gyroscope is converted into k and VAR k , MD of odometer data collection k As the downsampling layer of the neural ordinary differential equation model, the sideslip state recognition output by the feedforward layer compensates the velocity error, estimates the error involved in the integrated navigation system, compensates the SINS, and obtains the final navigation result.

[0191] like Figure 5 As shown, the odometer sideslip online compensation integrated navigation method based on FPGA hardware acceleration studied in the present invention is used to calculate the diagonal elements d of the diagonal matrix D rr The unit is the PED unit. Since the d rr There are only N of them, so the number of PED units is 1, which is used to calculate the element I of the lower triangular matrix L. ir The unit is a PEL unit, which needs to calculate the I ir The number is N 2 / 2, the size of the matrix N and the internal resources of the chip determine the number of PELs. After obtaining L and D of the improved Cholesky decomposition, the vector multiplication and accumulation operation is used to solve the inverse operation of the lower triangular matrix, realizing the fast solution of the matrix inverse operation.

[0192] It will be apparent to those skilled in the art that the invention is not limited to the details of the exemplary embodiments described above and that the invention can be implemented in other specific forms without departing from the spirit or essential features of the invention. Therefore, the embodiments should be considered exemplary and non-limiting in all respects, and the scope of the invention is defined by the appended claims rather than the foregoing description, and it is intended that all variations falling within the meaning and scope of the equivalent elements of the claims be included in the invention. Any reference numeral in a claim should not be considered as limiting the claim to which it relates.

[0193] In addition, it should be understood that although the present specification is described according to implementation modes, not every implementation mode contains only one independent technical solution. This description of the specification is only for the sake of clarity. Those skilled in the art should regard the specification as a whole. The technical solutions in each embodiment may also be appropriately combined to form other implementation modes that can be understood by those skilled in the art.

Claims

1. An odometer sideslip online compensation integrated navigation method based on FPGA hardware acceleration, including an IMU inertial measurement unit integrated inside a SINS inertial navigation module, the IMU inertial measurement unit including a three-axis gyroscope and a three-axis accelerometer, characterized in that: In SINS / DR integrated navigation, the neural ordinary differential equation model is used to realize the mapping from sensor data such as gyroscopes, accelerometers and odometers to the slip mode. Two odometers are installed on the non-steering wheels on both sides. The error of the odometer scale coefficient is corrected to the actual output speed in the navigation coordinate system. The odometer speed error model is established to compensate and correct the actual speed. The matrix inversion operation in the Kalman filter algorithm is decomposed by improved Cholesky decomposition, and the matrix multiplication is decomposed into multiple stages. The parallel architecture of FPGA is used to calculate multiple matrix elements at the same time to accelerate the filtering algorithm. The specific contents are as follows: Step 1: The IMU inertial measurement module outputs the three-axis accelerometer and three-axis gyroscope data, and its mean and variance are used as the downsampling layer of the neural ordinary differential equation, which is calculated as follows: Where k is the training sample number, N is the number of sampling points, the system preset slip recognition frequency is 1Hz, and the sensor data sampling frequency is 125Hz, so N can be set to 125 here; Raw data collected for the inertial measurement unit: in (j= x, y, z) are the acceleration and angular velocity data of the X-axis, Y-axis, and Z-axis directions in the carrier coordinate system collected by the inertial measurement unit; Step 2: The integrated navigation system includes two odometers, which measure the mileage information of the left and right non-steering wheels respectively. The feature vector based on the odometer is defined as the differential mean of the odometer: in They are the speed data output by the left and right odometers respectively; In the actual vehicle driving process, the downsampling layer of the neural ordinary differential equation is determined to be 9 dimensions: Step 3: The speed output of the odometer can be expressed in the navigation coordinate system as , in Indicates the speed of the odometer on the Y axis in the carrier coordinate system, is the attitude transformation matrix from the carrier coordinate system to the navigation coordinate system, which can be obtained by SINS navigation solution.

2. The odometer sideslip online compensation integrated navigation method based on FPGA hardware acceleration according to claim 1 is characterized by: The error compensation principle of the odometer includes single-side slip and double-side slip; When the vehicle slips on one side, the following analysis is based on the slip on the right side. The speed error caused by the slip is assumed to be , the actual speed and speed error caused by the right wheel slip are as follows: At this time, the speed error caused by directly replacing the left odometer with the original odometer speed is: When the error is not zero, a one-sided slip occurs. At this time, the control system can be used to actively control the wheel speeds on both sides to be equal. When one-side slip occurs and the speeds of the wheels on both sides have not been corrected, the rotational angular velocity of the vehicle between two adjacent moments is in is the wheelbase of the non-steering wheels on both sides of the vehicle. At this time, the vehicle speed is compensated as Since SINS can maintain high accuracy in a short time, the rotation angular velocity of the vehicle The value measured by the gyroscope Z axis can be approximated The vehicle speed compensation formula is as follows: Similarly, when the left wheel slips, the vehicle speed can be compensated by the following formula: When the vehicle skids on both sides, since the SINS is constantly corrected by the odometer speed when the vehicle is in good driving condition, it can maintain a high accuracy in a short period of time. Therefore, the speed of the vehicle in the navigation coordinate system is as follows: At this time, a threshold can be set for the speed of the vehicle Constrain the speed of SINS: This reduces the impact of SINS velocity accumulation error on positioning accuracy.

3. The odometer sideslip online compensation integrated navigation method based on FPGA hardware acceleration according to claim 1 is characterized in that: The odometer and SINS are not installed in the same position on the vehicle, that is, the odometer coordinate system m and the vehicle coordinate system b do not coincide, and there is a small installation error angle. , each component represents the pitch installation error angle, roll installation error angle and heading installation error angle, so there is an error matrix from the b system to the m system: 。 4. The odometer sideslip online compensation integrated navigation method based on FPGA hardware acceleration according to claim 1 is characterized by: The odometer scale coefficient error is corrected to the actual output speed in the navigation coordinate system. The specific contents are as follows: Step 1: The odometer has a scale factor error during the actual driving of the vehicle , the odometer speed after considering the odometer scale coefficient error is expressed as: Step 2: Consider the attitude error of SINS solution , then the odometer speed in the navigation coordinate system is expressed as: in is the attitude matrix the various elements of Step 3: From the velocity equation in step 2, it can be seen that the roll angle installation error will not affect the measurement accuracy of the odometer, as shown in the following formula: 。 5. The odometer sideslip online compensation integrated navigation method based on FPGA hardware acceleration according to claim 1 is characterized by: A simplified SINS algorithm and error equation are adopted to realize FPGA-based hardware acceleration, as follows: Step 1: The simplified strapdown attitude update algorithm is: in express The quaternion of the attitude transformation at the moment, is from Time has come The quaternion change of the attitude at the moment, is the gyroscope in the time period The angular increment and modulus of the internal output ; Step 2: The simplified strapdown velocity update equation is: in for SINS speed at the moment, For quaternion The corresponding posture array, is the accelerometer in the time period The specific force increment of the internal output is approximated by directly multiplying the specific force output by the sampling interval in practice; Step 3: The simplified strapdown position update equation is: In the formula: , in meters; Step 4: The simplified SINS system error equation is as follows: in: and They are the gyroscope angular rate white noise and the accelerometer specific force white noise respectively; and are the first-order Markov process errors of the gyroscope and accelerometer, respectively, as follows: Where: and is the correlation time constant; and is a first-order Markov process exciting white noise.

6. The odometer sideslip online compensation integrated navigation method based on FPGA hardware acceleration according to claim 1 is characterized by: In the integrated navigation system, SINS and ODO data are fused using the Kalman filter algorithm, as follows: Step 1: The Kalman filter state space model of the integrated navigation system is: in is the state transfer matrix of the Kalman filter, and Noise driven matrix and noise matrix, and are the measurement matrix and the measurement white noise respectively; Step 2: Define the 18-dimensional state vector of the integrated navigation system for Kalman filter estimation, as follows: Step 3: According to the Kalman filter principle, iterative updates are performed to estimate the error in real time, and the error quantities estimated by the Kalman filter are fed back to the corresponding variables to obtain compensated navigation information.

7. The odometer sideslip online compensation integrated navigation method based on FPGA hardware acceleration according to claim 1 is characterized by: For the matrix inversion operation in the Kalman filter algorithm, the improved Cholesky decomposition is used to divide the implementation process into three modules: matrix decomposition, lower triangular matrix inversion and triangular matrix multiplication. The matrix multiplication is decomposed into multiple stages. The parallel architecture of FPGA is used to calculate multiple matrix elements at the same time to accelerate the Kalman filter algorithm. The details are as follows: Step 1: Convert the symmetric positive definite matrix Using Cholesky decomposition, we get , and Right now in According to the matrix operation rules, we can get: Step 2: For the lower triangular matrix The inverse matrix can be obtained by inverting Step 3: Calculate the diagonal matrix The inverse matrix Step 4: Calculation The inverse matrix 。

Images