Train inertial navigation method and inertial navigation equipment based on multi-signal fusion

Abstract

The present invention discloses a train inertial navigation method and inertial navigation equipment based on multi-signal fusion, and belongs to the technical field of train navigation and positioning. In the train inertial navigation method, the inertial device collects inertial signals, the mileage device collects pulse signals of corresponding wheels, and the positioning device collects wireless signals. When the wireless signal is valid, the present invention generates navigation data of the train according to the wireless signal and the inertial signal, and updates the effective diameter of the wheel according to the pulse signal. When the wireless signal fails, the present invention predicts the wheel motion state according to the wheel sliding data, generates the wheel speed according to the effective diameter of the navigation wheel and the pulse signal, and then extracts the mileage data, and generates the train navigation data according to the mileage signal and the inertial signal, so as to avoid the measurement error of different wheels affecting the accuracy of the navigation data.

Description

Technical Field

[0001] The present invention relates to the technical field of train navigation and positioning, and in particular to a train inertial navigation method and inertial navigation equipment based on multi-signal fusion. Background Art

[0002] Uninterrupted navigation and positioning are required during the travel of the train. When traveling in mountain tunnels and other places, satellite signals are limited and the accuracy of navigation cannot be guaranteed. The prior art integrates inertial navigation technology into GPS / mileage data to improve positioning accuracy. For example, Chinese Patent Publication No. CN108196289B discloses a combined positioning method for trains under satellite signal limited conditions. When satellite positioning is effective, the method uses the position information provided by the satellite navigation system to correct the error of the inertial navigation system. When satellite positioning fails, the wheel sensor / inertial navigation system is used for combined positioning, so that the train combined navigation system can provide positioning information with a certain accuracy. When the wheel sensor / inertial navigation system is used in this patent, the train is regarded as a particle, and the accuracy of the positioning information is limited. Chinese Patent Application No. CN114923479A discloses an error compensation method for inertial / mileage combined navigation. The observation equation of inertial / mileage combined navigation is established based on the observed quantity of inertial / mileage combined navigation, and Kalman filtering is performed based on the state equation and the observation equation to achieve accurate estimation of various errors of the inertial navigation system, and the inertial / mileage combined navigation is compensated according to the error estimated by the inertial navigation system. The actual working conditions of trains are complex, and the motion states of different wheels vary greatly. The existing error estimation is greatly affected by the motion state of the wheels. Therefore, the existing technology needs to be further improved. Summary of the invention

[0003] In order to solve the defects of the above-mentioned prior art, the present invention proposes a train inertial navigation method and inertial navigation equipment based on multi-signal fusion, which collects the pulse signal of the wheel when the wireless signal is valid, and predicts the wheel motion state according to the wheel pulse signal when the wireless signal fails, so as to avoid the accuracy of navigation data affected by measurement errors of different wheels.

[0004] The technical solution of the present invention is achieved in this way:

[0005] A train inertial navigation method based on multi-signal fusion includes the following steps:

[0006] Step 1: Install an inertial device and a positioning device on the train body, install mileage devices on multiple wheels of the train, and the inertial device generates attitude compensation data and equipment compensation data;

[0007] Step 2: The train travels along the track, the inertial device collects inertial signals to generate instantaneous angular velocity and instantaneous acceleration, the mileage device collects pulse signals from the corresponding wheels, and the positioning device collects wireless signals;

[0008] Step 3: Obtaining a first speed and a first coordinate of the train according to the attitude compensation data, the equipment compensation data, and multiple sets of instantaneous angular velocities and instantaneous accelerations;

[0009] Step 4: If the wireless signal is valid, go to step 5, otherwise go to step 8;

[0010] Step 5: Generate the second speed and second coordinate of the train according to the wireless signal, generate the wheel speed according to the pulse signal and the effective diameter of the wheel, and then obtain the third speed of the train in combination with the instantaneous rotation matrix of the wheel;

[0011] Step 6: construct a first state matrix based on the second speed and the second coordinate, generate a first filter model according to the first state matrix, predict an estimated value of the first state matrix according to the first filter model, and obtain navigation data of the train according to the estimated value;

[0012] Step 7: extract the heading angle of the instantaneous rotation matrix, generate the sliding data of each wheel according to the heading angle, the second speed and the third speed, select one of the wheels as the navigation wheel according to the sliding data of the multiple wheels, update the effective diameter of the navigation wheel, and return to step 2;

[0013] Step 8: Generate the wheel speed according to the effective diameter of the navigation wheel and the pulse signal, and then combine it with the instantaneous rotation matrix of the train to obtain the third speed and third coordinate of the train;

[0014] Step 9: Construct a second state matrix based on the third speed and the third coordinate, generate a second filtering model according to the second state matrix, predict an estimated value of the second state matrix according to the second filtering model, obtain navigation data of the train according to the estimated value, and return to step 2.

[0015] In the present invention, in step 1, the inertial device collects the angular velocity A when the train is stationary. 1 and acceleration B 1 , then generate the attitude compensation data D 1 , apply multiple excitation angular velocities to the inertial device in turn, and the inertial device collects angular velocity A 2 , generate device compensation data D 2 .

[0016] In the present invention, in step 3, according to the posture compensation data D 1 , Equipment compensation data D 2 And multiple sets of instantaneous angular velocities A 3 Generate instantaneous transformation matrix C 2, according to the instantaneous acceleration B 3 and the instantaneous transformation matrix C 2 Get the first speed V of the mobile device 1 , according to the first speed V 1 and the initial coordinates S 0 Generate the first coordinate S 1 .

[0017] In the present invention, in step 5, the wheel revolution number n and wheel speed v are extracted from the pulse signal. 3 =πdn / T 3 , T 3 is the sampling period of the inertial signal, d is the effective diameter of the wheel, and the instantaneous rotation matrix C of the bogie where the wheel is located is collected. 3 , the third speed V 3 = v 3 C 3 .

[0018] In the present invention, in step 6, the first state matrix includes instantaneous angular velocity, instantaneous acceleration, fusion velocity and fusion displacement, and the sampling period of the inertial signal is T 1 , the sampling period of the wireless signal is T 2 , T 2 T 1 If the sampling time t increases by T 2 , calculate the fusion speed and fusion coordinates according to the first speed and the first coordinate and the second speed and the second coordinate, otherwise substitute the first speed and the first coordinate into the fusion speed and the fusion coordinate.

[0019] In the present invention, the measurement value R of the first state matrix is ​​generated according to the instantaneous angular velocity, instantaneous acceleration, fusion velocity and fusion displacement of the sampling time t. t , according to the measured value R t Update the model parameters of the first filtering model and then predict the estimated value R of the first state matrix t '.

[0020] In the present invention, in step 7, the sliding data ρ=[(1-2sinγ 3 L 2 / L 1 )|V 2 |-|V 3 |] / |V 2 |, γ 3 is the rotation angle of the bogie, V 2 is the second speed, L 1 is the distance between the two sets of bogies of the train, L 2 is the distance between the two sets of wheels.

[0021] In the present invention, the prediction period T of each wheel is calculated. 4 The sum of multiple sliding data within, T 4 T 3 The wheel with the smallest sum of sliding data is selected as the navigation wheel, and the navigation wheel is extracted in the prediction period T 4 The sampling time of the minimum internal sliding data is retrieved, and the second speed corresponding to the sampling time is retrieved. The updated effective diameter d=(1-2sinγ 3 L 2 / L 1 )|V 2 |T 3 / (πn).

[0022] An inertial navigation device for implementing the train inertial navigation method based on multi-signal fusion includes: a calibration device, an inertial device, a positioning device, a mileage device, a coordinate solving device, a data analyzing device and a data processing device.

[0023] The calibration device is used to generate attitude compensation data and equipment compensation data;

[0024] The inertial device includes a laser gyro component and an acceleration measurement component, the laser gyro component is used to collect instantaneous angular velocity, and the acceleration measurement component is used to collect instantaneous acceleration;

[0025] The coordinate solving device is used to calculate the first speed and the first coordinate of the train;

[0026] The positioning device is used to collect the second speed and the second coordinate of the train;

[0027] The mileage device is used to collect pulse signals from the wheels;

[0028] The data analysis device is used to generate a third speed according to the effective diameter of the wheel and update the effective diameter of the navigation wheel;

[0029] The data processing device is used to generate a first filtering model and a second filtering model and predict an estimated value of a first state matrix or a second state matrix;

[0030] The data generating device is used to generate navigation data of the train.

[0031] The implementation of the train inertial navigation method and inertial navigation device based on multi-signal fusion of the present invention has the following beneficial effects: the present invention generates the navigation data of the train based on the wireless signal and the inertial signal, and updates the effective diameter of the wheel based on the pulse signal. When the wireless signal fails, the present invention generates the navigation data of the train based on the wheel sliding data and the wheel motion state, and then based on the mileage signal and the inertial signal, so as to avoid the accuracy of the navigation data being affected by the differences in different wheel states and diameters. Furthermore, the present invention updates the effective diameter of the navigation wheel in combination with the turning curvature of the train, so as to avoid the accuracy of the mileage data being affected by wheel wear. In addition, the present invention sets the update period of the filter model in combination with the sampling frequency between different signals, so as to ensure that different navigation signals can be effectively substituted into the filter model. BRIEF DESCRIPTION OF THE DRAWINGS

[0032] Figure 1 The communication principle diagram of the train inertial navigation technology;

[0033] Figure 2 Schematic diagram of different coordinate systems for vehicle inertial navigation technology;

[0034] Figure 3 It is a flow chart of the train inertial navigation method based on multi-signal fusion of the present invention;

[0035] Figure 4 A schematic diagram of updating the first speed and the first coordinate of the present invention;

[0036] Figure 5 A schematic diagram of updating the second speed and the second coordinate of the present invention;

[0037] Figure 6 A schematic diagram of updating the third speed and the third coordinate of the present invention;

[0038] Figure 7 This is a schematic diagram of the train running process of the present invention;

[0039] Figure 8 is a schematic diagram of a train bogie of the present invention;

[0040] Fig. 9 is a schematic diagram of the wheel speed of the train of the present invention;

[0041] Fig.10 A schematic diagram of providing an excitation angular velocity to an inertial device according to the present invention;

[0042] Fig.11 A schematic diagram of a first filtering model preferred in the present invention;

[0043] Fig.12 A block diagram of an inertial navigation device for implementing the train inertial navigation method based on multi-signal fusion according to the present invention;

[0044] Fig.13 This is a schematic diagram of the installation structure of the preferred mileage device of the present invention.

[0045] Reference numerals in the accompanying drawings: telescopic rod 110 , rotating shaft 120 , wheel 130 , elastic component 140 , track 150 , Hall sensor unit 160 , encoder 170 . DETAILED DESCRIPTION

[0046] In order to more clearly understand the purpose, technical solutions and advantages of the present application, the present application is described and illustrated below in conjunction with the accompanying drawings and embodiments.

[0047] like Figure 1 , usually a multi-combination navigation and positioning technology is used to track and locate the train, and the navigation and positioning technology collects wireless signals, pulse signals and inertial signals. When the train is located in mountainous areas, tunnels, etc., the wireless signal may fail. When the wireless signal fails, the present invention analyzes the mileage parameters according to the historical data of the wheels and regenerates the navigation data to avoid the accuracy of the navigation data being affected by the errors of different mileage parameters. Embodiment 1

[0048] like Figure 2 The combined navigation and positioning technology includes three coordinate systems: Earth coordinate system O 0 X 0 Y 0 Z 0 , reference coordinate system O 1 X 1 Y 1 Z 1 , inertial coordinate system O 1 X 2 Y 2 Z 2 And the bogie coordinate system O 3 X 3 Y 3 Z 3 The Earth coordinate system takes the center of the Earth as its origin. 0 , the intersection of the equator and the prime meridian in the equatorial plane is X 0 The positive direction of the axis, pointing to the North Pole, is Y 0 The positive direction of the axis is determined by the right-hand rule. 0 The reference coordinate system takes the center of mass of the train as its origin O 1 , pointing to the east is X 1 The positive direction of the axis is Y, pointing to the north 1 The positive direction of the axis is determined by the right-hand rule. 1 When the inertial device is fixed in the middle of the vehicle body, the origin of the inertial coordinate system is the same as the reference coordinate system, and the three sets of mutually perpendicular sensitive elements of the inertial device are respectively X 2 , Y2 , Z 2 The bogie coordinate system takes the bogie center point as the origin O 3 , with the wheel axis as X 3 Axis positive direction, with the wheel connecting rod as Y 3 Axis positive direction, with the steering axis as Z 3 The positive direction of the axis. When the train is stationary, the inertial reference system is transformed to the reference coordinate system by the reference transformation matrix. When the train is moving, the inertial reference system is transformed to the reference coordinate system by the instantaneous transformation matrix. When the train is turning, the bogie reference system is transformed to the reference coordinate system by the instantaneous rotation matrix.

[0049] like Figures 3 to 9 As shown, the train inertial navigation method based on multi-signal fusion of the present invention includes the following steps.

[0050] Step 1: Install an inertial device and a positioning device on the train body, and install mileage devices on multiple wheels of the train. The inertial device generates attitude compensation data and equipment compensation data. The initial coordinates S of the starting point are collected before the train starts. 0 When the train is stationary, the inertial device collects the angular velocity A 1 and acceleration B 1 , find the base transformation matrix C from the inertial coordinate system to the reference coordinate system 1 , generate attitude compensation data D 1 At this time, the inertial device is in a non-excited state, and the measurement error of the inertial measurement device includes the initial attitude error. Apply multiple excitation angular velocities to the inertial device in sequence, and the inertial device is in an excited state. At this time, the measurement error of the inertial measurement device includes the device output error. Collect the angular velocity A of the inertial measurement device 2 , generate device compensation data D 2 The second embodiment further describes the method for generating compensation data.

[0051] Step 2: The train travels along the track, the inertial device collects inertial signals, generates instantaneous angular velocity and instantaneous acceleration, the mileage device collects pulse signals of the corresponding wheels, and the positioning device collects wireless signals. In the motion state, the measurement error of the inertial device at each sampling moment includes the initial attitude error and the device output error. The wireless signal is obtained, for example, through the global navigation satellite system or the Beidou satellite navigation system. The pulse signal is obtained, for example, through a magnetic encoder / optical encoder.

[0052] Step 3: Obtain the first velocity and first coordinate of the train based on the attitude compensation data, the equipment compensation data, and multiple sets of instantaneous angular velocities and instantaneous accelerations. Specifically, the instantaneous transformation matrix C 2 is the transformation matrix from the inertial coordinate system to the reference coordinate system at any sampling time. Figure 4 , according to the instantaneous acceleration B3 and the instantaneous transformation matrix C 2 Get the acceleration B of the train in the reference coordinate system 3 ', acceleration B 3 The time integral of ' is the first velocity V 1 , the first speed V 1 The time integral and initial coordinate S 0 The sum is the first coordinate S 1 The method for calculating the first coordinate is further described in the third embodiment.

[0053] Step 4: If the wireless signal is valid, go to step 5, otherwise go to step 8. The wireless signal uses a three-point positioning method, which requires at least three groups of positioning satellites, and also requires a group of positioning satellites to solve the problem of time synchronization. If the number of available positioning satellites in the wireless signal is greater than or equal to four groups, the wireless signal is valid, otherwise the wireless signal is invalid.

[0054] Step 5: Generate the second speed and second coordinate of the train according to the wireless signal, generate the wheel speed according to the pulse signal and the effective diameter of the wheel, and then combine the instantaneous rotation matrix of the wheel to obtain the third speed of the train. In the present invention, the second speed, the second coordinate and the third speed are three-dimensional vectors in the reference coordinate system, and the wheel speed is a scalar. Figure 5 , measure the time difference between sending and receiving each set of ranging signals in the wireless signal, calculate the pseudo-range between the corresponding positioning satellite and the train, and then determine the second coordinate of the train in combination with the satellite coordinates. Differentiate the second coordinate to obtain the second speed. Figure 6 , extract the wheel revolutions from the pulse signal and calculate the wheel speed. Collect the instantaneous rotation matrix of the bogie where the wheel is located, convert the wheel speed into a three-dimensional third speed based on the bogie deflection angle, integrate the third speed and add the initial coordinate to obtain the third coordinate.

[0055] Step 6: Construct a first state matrix based on the second speed and the second coordinate, generate a first filter model according to the first state matrix, predict an estimated value of the first state matrix according to the first filter model, and obtain the navigation data of the train according to the estimated value. The first state matrix includes a measured value and an estimated value. The present invention generates a first filter model through the estimated value of the previous sampling number and the measured value of the current sampling number, and then predicts the estimated value of the current sampling number. The first filter model can be a Kalman filter model, as shown in Example 3. The first filter model can also be other prediction models, which is not limited by the present invention.

[0056] Step 7: Extract the heading angle of the instantaneous rotation matrix, generate the slip data of each wheel according to the heading angle, the second speed and the third speed, select one wheel as the navigation wheel according to the slip data of multiple wheels, update the effective diameter of the navigation wheel, and return to step 2. Affected by factors such as turning and vibration, different wheels have different degrees of slip or idling, which leads to low accuracy of the third speed. Fig. 9 , the wheel is in the partial sampling period T 3 The present invention predicts the theoretical wheel speed of the wheel and predicts the slip data according to the speed difference. The wheel with the smallest slip data in the preset time period is used as the navigation wheel, and the third speed obtained by the corresponding mileage device is used as the positioning data. At the same time, in order to avoid the change of wheel diameter caused by factors such as wear and tear affecting the accuracy of the data, the effective diameter is updated in real time, as described in the fourth embodiment.

[0057] Step 8: Generate wheel speed according to the effective diameter of the navigation wheel and the pulse signal, and then combine the instantaneous rotation matrix of the train to obtain the third speed and third coordinate of the train. The method for calculating the wheel speed of the navigation wheel is as described in step 5, which will not be repeated here.

[0058] Step 9: construct a second state matrix based on the third speed and the third coordinate, generate a second filter model according to the second state matrix, predict an estimated value of the second state matrix according to the second filter model, obtain the navigation data of the train according to the estimated value, and return to step 2. The method for generating the second filter model is described in step 6. Extract the fused displacement in the estimated value of the second state data, which is the navigation data of the train at the current sampling time. Embodiment 2

[0059] This embodiment further discloses the method of generating posture compensation data and device compensation data in step 1.

[0060] Step 101: The inertial device collects angular velocity A 1 and acceleration B 1 In the inertial coordinate system O 1 X 2 Y 2 Z 2 The angular velocity of the train at rest is , element value ω 1x ,ω 1y ,ω 1z They are the three uniaxial angular velocity components. , element value a 1x 、a 1y 、a 1z They are the three uniaxial acceleration components respectively.

[0061] Step 102: Calculate the reference transformation matrix. Since the mobile device is in a stationary state, the angular velocity in the reference coordinate system is , U is the latitude of the inertial measurement device. The orbit to be measured is much smaller than the diameter of the earth. It is usually assumed that the dimension of the mobile device remains unchanged during the monitoring process. ie is the angular velocity of the Earth's rotation. Angular velocity A 1 is caused by the rotation of the earth, and there is a matrix equation: , C 1 is the reference transformation matrix. Acceleration B 1 is caused by the earth's gravity, and the acceleration in the reference coordinate system is , g is the acceleration due to gravity. There is a matrix equation: By combining the above matrix equations, we can find the reference transformation matrix C 1 .

[0062] Step 103: Calculate the posture compensation data. 1 is a three-row and three-column matrix, , and then according to α 1 =arctan(C 31 / C 32 ), β 1 =arccos(C 33 ), γ 1 = -arctan(C 13 / C 23 ) Determine the initial pitch angle α 1 , roll angle β 1 , heading angle γ 1 . Attitude compensation data D 1 is the initial three-axis attitude matrix, that is The inertial coordinate system rotates along the Z axis by an angle γ 1 , and then rotate along the Y axis by a rolling angle β 1 , and then rotate along the X axis by a pitch angle α 1 Get the reference coordinate system.

[0063] Step 104: Calculate equipment compensation data. When the train is stationary, multiple excitation angular velocities are applied to the inertial measurement device in sequence, which are , , . Reference Fig.10 The three sets of excitation axes drive the inertial measurement device to rotate at a constant speed in three directions, and the excitation angular velocity is ω 0 , collect the output values ​​ω of the three uniaxial acceleration components of the inertial measurement device in turn 2x ,ω 2y ,ω 2z . Equipment compensation data D 2is the deviation between the excitation acceleration and the output value, the device compensation data is in X 1 The element value of the direction is ω 0 / ω 2x , in Y 1 The element value of the direction is ω 0 / ω 2y , in Z 1 The element value of the direction is ω 0 / ω 2z . . Embodiment 3

[0064] This embodiment further discloses a method for solving the first coordinate in step 3. The core of solving the first coordinate is to convert the inertial coordinate system O 1 X 2 Y 2 Z 2 The instantaneous angular velocity and acceleration under the reference coordinate system O are solved 1 X 1 Y 1 Z 1 .

[0065] First, according to the attitude compensation data D 1 Generate quaternion q 1 ,q 1 ,q 2 ,q 3 .q 0 =cos(α 1 / 2)cos(β 1 / 2)cos(γ 1 / 2)+sin(α 1 / 2)sin(β 1 / 2)sin(γ 1 / 2), q 1 =sin(α 1 / 2)cos(β 1 / 2)cos(γ 1 / 2)-cos(α 1 / 2)sin(β 1 / 2)sin(γ 1 / 2), q 2 =cos(α 1 / 2)sin(β 1 / 2)cos(γ 1 / 2)+sin(α 1 / 2)cos(β 1 / 2)sin(γ 1 / 2), q 3 =cos(α 1 / 2)cos(α1 / 2)sin(γ 1 / 2)-sin(β 1 / 2)sin(α 1 / 2)cos(γ 1 / 2).

[0066] Instantaneous transformation matrix .

[0067] Collect instantaneous angular velocity A 3 , ,ω 3x ,ω 3y ,ω 3z They are the output values ​​of the three uniaxial acceleration components during the measurement process. The equipment compensates the data D 2 and multiple sets of instantaneous angular velocities A 3 Calculate the instantaneous angular velocity measurement value, instantaneous angular velocity measurement value = The updated quaternion matrix satisfies , regenerate the instantaneous transformation matrix C based on the updated quaternion 2 .

[0068] Instantaneous acceleration in inertial coordinate system , the acceleration in the reference coordinate system .

[0069] Along X 1 Axis speed , along Y 1 Axis speed , along Z 1 Axis speed . First speed V 1 = Along X 1 Axis coordinates , along Y 1 Axis coordinates , along Z 1 Axis coordinates . Initial coordinates , the first coordinate . t is the sampling duration.

[0070] Continue to collect the instantaneous angular velocity A at the next sampling moment 3 , update the instantaneous transformation matrix C 2 and the first speed V 1 and the first coordinate S 1 After each sampling period T 1 , the relationship between the inertial coordinate system where the instantaneous angular velocity is located and the reference coordinate system changes. The instantaneous transformation matrix C 2If there is a change, it is necessary to iteratively update the quaternion based on the quaternion of the current instantaneous transformation matrix and continue to solve the first velocity and the first coordinate. Embodiment 4

[0071] like Fig.11 This embodiment further discloses a method for obtaining the navigation data of the train according to the estimated value of the state matrix in step 6. The sampling period of the inertial signal is T 1 , the sampling period of the wireless signal is T 2 , T 2 T 1 An integer multiple of .

[0072] Step 601: Construct a state matrix. If the sampling time t increases by T 2 , the mean of the first speed and the second speed is substituted into the fusion speed, and the matrix form of the fusion speed is [v 0x v 0y v 0z ] T , the mean of the first coordinate and the second coordinate is substituted into the fusion coordinate, and the matrix form of the fusion coordinate is [s 0x s 0y s 0z ] T Otherwise, the sampling time t increases by T 1 , substitute the fusion velocity and fusion coordinate according to the first velocity and the first coordinate. The state matrix is ​​the matrix form of the Kalman filter state vector, which includes instantaneous angular velocity, instantaneous acceleration, fusion velocity and fusion displacement. State matrix = [ω 3x ω 3y ω 3z a 3x a 3y a 3z v 0x v 0y v 0z s 0x s 0y s 0z ] T In the process of calculating the first speed, the attitude compensation data and the equipment compensation data of the present invention compensate the error value of the inertial device. In order to simplify the calculation, the state matrix does not include the error value of the inertial device.

[0073] Step 602: Generate a first filter model. The first filter model is an asynchronous Kalman filter model. In the first filter model, R t '= F t-1 R t-1 ''+ G t ;P t '= F t-1 P t-1''F t-1 T Q t Among them, R t ' is the estimated value of the state matrix of sampling time t, F t-1 is the state transfer matrix of sampling time t-1, R t-1 '' is the second estimation of the state matrix of sampling time t-1. t P is the control input data, which expresses the influence of the train motion control signal on the estimated value of the state matrix. The control input data consists of the control input matrix and the control input vector. t ' is the covariance matrix of the state matrix estimate of sampling time t, P t ' is the covariance matrix of the quadratic estimate of the state matrix of sampling time t-1, Q t is the process noise variance matrix of sampling time t.

[0074] Step 603: Set initial filtering parameters. Determine the estimated value of the state matrix R at t=0 based on the previous measurement data. 0 ', according to the uncertainty of the estimated value of the state matrix, preset P 0 ', if the setting of the state matrix estimate is relatively certain, a smaller P can be selected 0 '; On the contrary, a relatively large diagonal matrix can be set to express high uncertainty. Set the random perturbation noise to represent Q t .

[0075] Step 604: Obtain the navigation data of the train. Calculate the estimated value of the state matrix R of the sampling time t step by step according to the first filtering model. t '.

[0076] Step 605: Extract the estimated value R of the state data t 'Fusion displacement in [s 0x s 0y s 0z ] T , the fused displacement is the navigation data of the train at the current sampling moment.

[0077] Step 606: Update the first filter model. Calculate the Kalman gain, Kalman gain K t =P t 'H t T (H t P t 'H t T ) -1 .H t is the measurement matrix, which maps the true state to the measurement space. tis the measurement noise variance matrix. The state matrix measurement value R is generated according to the measurement results of the inertial device and the positioning device at time t t Iteratively calculate the state matrix quadratic estimate R of sampling time t t '' = R t '+ K t (R t -H t R t-1 '). Then iteratively calculate the covariance matrix P of the quadratic estimate of the state matrix of sampling time t t '' = (IK t H t ) t ', I is the identity matrix. Return to step 604 to regenerate the estimated value of the state matrix.

[0078] Furthermore, the present invention can use the same method as the first filtering model to construct the second filtering model in step 9. The sampling period of the pulse signal is T 3 If the sampling time t increases by T 3 , the average of the first and third speeds is substituted into the fusion speed, and the average of the first and third coordinates is substituted into the fusion coordinates. Otherwise, the sampling time t is increased by T 1 , substitute the fusion velocity and fusion coordinate according to the first velocity and the first coordinate. Embodiment 5

[0079] This embodiment further discloses a method for calculating the third speed and updating the wheel diameter.

[0080] The wheel revolution number n is extracted from the pulse signal of the mileage device. Specifically, the mileage device includes a turntable fixed to the wheel, and the turntable has n uniformly distributed 2 A group of sensor elements, each time a sensor element is passed, a high level pulse signal is generated. The sampling period of the mileage device is T 3 , T 3 The number of discrete high levels of the pulse signal within a certain time period is n 1 , number of wheel laps n=n 1 / n 2 .

[0081] Calculate the wheel speed v of the wheel corresponding to the odometer 3 The wheel diameter d is preset, and the distance traveled by the wheel is πdn. The preset wheel diameter d is usually 0.5m. In the subsequent sampling cycles, the wheel diameter is gradually updated. Wheel speed v 3 =πdn / T 3 . Limited by the mileage device, v 3 It is a scalar, and the direction of the wheel speed is always along the forward direction of the wheel.

[0082] Generates the third velocity. Figure 7 The train consists of two sets of bogies. During the turning process, the posture of the bogies is different from that of the train. 3 Direction axis, X 3 Direction axis, Z 3 The direction shaft is connected to the train. 3 Roll angle of the axis β 3 , X 3 The pitch angle of the axis α 3 , Z 3 Steering angle of the axis γ 3 , generate the instantaneous rotation matrix C 3 The third speed of the train is a three-dimensional vector, which is transformed into v by the instantaneous rotation matrix. 3 Transformed into the third speed of the train in the reference coordinate system. V 3 = v 3 C 3 According to the transformation order of the bogie coordinate system, the instantaneous rotation matrix

[0083] .

[0084] Calculate the wheel slip data. The slip data is the speed loss of the wheel slip, that is, the difference between the theoretical wheel speed and the third speed. Figure 7 , Figure 8 , extract the steering angle γ of the bogie according to the instantaneous rotation matrix 3 , that is, Z 3 Steering angle of the axis γ 3 The curvature of the train is 2sinγ 3 / L 1 , L 1 is the distance between the two sets of bogies of the train. The theoretical wheel speed is (1-2sinγ 3 L 2 / L 1 )|V 2 |, V 2 is the second speed, L 2 is the distance between the two sets of wheels. If the current wheel is located on the side of the bogie close to the rotation center, L 2 > 0, if the wheel is located on the side of the bogie away from the rotation center, L 2 <0. Sliding data ρ=[(1-2sinγ 3 L 2 / L 1 )|V 2 |-|V 3 |] / |V 2 |, V 3 The third speed.

[0085] Determine the navigation wheel. The train has multiple sets of wheels, and calculate the prediction period T 4 The sum of multiple sliding data of each wheel, T 4 T 3 The smaller the sum of the sliding data is, the better the wheel is in the sampling period T. 4 The fewer the number of slips, the higher the reliability of the third speed collected by the wheel. The wheel with the smallest sum of slip data is selected as the navigation wheel.

[0086] Update the wheel diameter of the navigation wheel. Prediction period T 4 The inner navigation wheel may also slip during some sampling periods, resulting in speed loss, and the corresponding wheel speed is significantly different from the second speed. 4 The plurality of sets of sliding data in the above example are used to extract the minimum sampling time of the sliding data, and retrieve the second speed V corresponding to the sampling time. 2 , the updated d = (1-2sinγ 3 L 2 / L 1 )|V 2 |T 3 / (πn). Embodiment 6

[0087] like Fig.11 and Fig.12 As shown, an inertial navigation device for implementing the train inertial navigation method based on multi-signal fusion of the present invention includes: a calibration device, an inertial device, a positioning device, a mileage device, a coordinate solving device, a data analyzing device and a data processing device, and also includes a data input device and a data generating device. The calibration device is used to generate attitude compensation data and equipment compensation data. The calibration device, for example, includes three sets of motors for providing an excitation angular velocity ω 0 . The calibration device can also collect the initial coordinates of the train. The inertial device includes a laser gyro component and an acceleration measurement component, the laser gyro component is used to collect instantaneous angular velocity, and the acceleration measurement component is used to collect instantaneous acceleration. The coordinate solution device is used to calculate the first speed and first coordinate of the train. The positioning device is used to collect the second speed and second coordinate of the train. The positioning device can have a wireless communication module for receiving wireless signals from positioning satellites. The mileage device is used to collect pulse signals from wheels. Fig.12The preferred installation structure of the mileage device is disclosed. The lower part of the telescopic rod 110 is connected to the rotating shaft 120, and the rotating shaft 120 is installed with the wheel 130. The upper part of the telescopic rod 110 is provided with an elastic component 140, and the elastic component 140 is connected to the steering gear. The elastic component 140 ensures that the wheel 130 after wear is continuously supported on the track 150. The rotating shaft 120 of the wheel 130 is provided with a Hall sensor unit 160, and the (magnetic / optical) encoder 170 is fixed on the side of the telescopic rod 110. When the wheel 130 rotates, the Hall sensor unit 160 passes through the encoder 170, and the encoder generates a pulse signal.

[0088] The data analysis device is used to generate a third speed according to the effective diameter of the wheel and update the effective diameter of the navigation wheel. The data processing device is used to generate a first filter model and a second filter model and predict the estimated value of the first state matrix or the second state matrix. The data input device is used to collect the power signal of the train, and the power signal includes the output power, braking parameters, load and other control input data of the train motor. The present invention can generate a control input vector according to the power signal and then obtain the control input data. The control input data is used as an external input disturbance to iteratively update the first filter model or the second filter model. The data generation device is used to generate navigation data of the train.

[0089] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included in the protection scope of the present invention.

Claims

1. A train inertial navigation method based on multi-signal fusion, characterized in that: The following steps are involved: Step 1: Install an inertial device and a positioning device on the train body, install mileage devices on multiple wheels of the train, and the inertial device generates attitude compensation data and equipment compensation data; Step 2: The train runs along the track, the inertial device collects inertial signals to generate instantaneous angular velocity and instantaneous acceleration, the mileage device collects pulse signals from the corresponding wheels, and the positioning device collects wireless signals; Step 3: Obtaining a first speed and a first coordinate of the train according to the attitude compensation data, the equipment compensation data, and multiple sets of instantaneous angular velocities and instantaneous accelerations; Step 4: If the wireless signal is valid, go to step 5, otherwise go to step 8; Step 5: Generate the second speed and second coordinate of the train according to the wireless signal, generate the wheel speed according to the pulse signal and the effective diameter of the wheel, and then obtain the third speed of the train in combination with the instantaneous rotation matrix of the wheel; Step 6: construct a first state matrix based on the second speed and the second coordinate, generate a first filter model according to the first state matrix, predict an estimated value of the first state matrix according to the first filter model, and obtain navigation data of the train according to the estimated value; Step 7: extract the heading angle of the instantaneous rotation matrix, generate the sliding data of each wheel according to the heading angle, the second speed and the third speed, select one of the wheels as the navigation wheel according to the sliding data of the multiple wheels, update the effective diameter of the navigation wheel, and return to step 2; Step 8: Generate the wheel speed according to the effective diameter of the navigation wheel and the pulse signal, and then combine the instantaneous transformation matrix of the train to obtain the third speed and third coordinate of the train; Step 9: Construct a second state matrix based on the third speed and the third coordinate, generate a second filtering model according to the second state matrix, predict an estimated value of the second state matrix according to the second filtering model, obtain navigation data of the train according to the estimated value, and return to step 2.

2. The train inertial navigation method based on multi-signal fusion according to claim 1 is characterized in that: In step 1, when the train is stationary, the inertial device collects angular velocity A1 and acceleration B1, and then generates attitude compensation data D1. Multiple excitation angular velocities are applied to the inertial device in sequence, and the inertial device collects angular velocity A2 to generate equipment compensation data D2.

3. The train inertial navigation method based on multi-signal fusion according to claim 2 is characterized in that: In step 3, an instantaneous transformation matrix C2 is generated based on the posture compensation data D1, the equipment compensation data D2 and multiple sets of instantaneous angular velocities A3, the first speed V1 of the mobile device is obtained based on the instantaneous acceleration B3 and the instantaneous transformation matrix C2, and the first coordinate S1 is generated based on the first speed V1 and the initial coordinate S0.

4. The train inertial navigation method based on multi-signal fusion according to claim 1 is characterized in that: In step 5, the wheel revolution number n is extracted from the pulse signal, the wheel speed v3=πdn / T3, T3 is the sampling period of the inertial signal, d is the effective diameter of the wheel, the instantaneous rotation matrix C3 of the bogie where the wheel is located is collected, and the third speed V3= v3C3.

5. The train inertial navigation method based on multi-signal fusion according to claim 1 is characterized in that: In step 6, the first state matrix includes instantaneous angular velocity, instantaneous acceleration, fused velocity and fused displacement. The sampling period of the inertial signal is T1, and the sampling period of the wireless signal is T2. T2 is an integer multiple of T1. If the sampling time t increases by T2, the fused velocity and fused coordinates are calculated according to the first velocity and the first coordinate and the second velocity and the second coordinate. Otherwise, the first velocity and the first coordinate are substituted into the fused velocity and the fused coordinate.

6. The train inertial navigation method based on multi-signal fusion according to claim 5 is characterized in that: The measurement value R of the first state matrix is ​​generated according to the instantaneous angular velocity, instantaneous acceleration, fusion velocity and fusion displacement of the sampling number t t , according to the measured value R t Update the model parameters of the first filtering model and then predict the estimated value R of the first state matrix t '.

7. The train inertial navigation method based on multi-signal fusion according to claim 4 is characterized in that: In step 7, the sliding data ρ=[(1-2sinγ3L2 / L1)|V2|-|V3|] / |V2|, γ3 is the rotation angle of the bogie, V2 is the second speed, L1 is the distance between the two sets of bogies of the train, and L2 is the distance between the two sets of wheels.

8. The train inertial navigation method based on multi-signal fusion according to claim 7 is characterized in that: Calculate the sum of multiple sliding data of each wheel within the prediction period T4, where T4 is an integer multiple of T3, select the wheel with the smallest sum of sliding data as the navigation wheel, extract the sampling moment when the sliding data of the navigation wheel is the smallest within the prediction period T4, retrieve the second speed corresponding to the sampling moment, and update the effective diameter d=(1-2sinγ3L2 / L1)|V2|T3 / (πn).

9. An inertial navigation device for implementing the train inertial navigation method based on multi-signal fusion as claimed in claim 1, characterized in that: include: Calibration device, inertial device, positioning device, mileage device, coordinate solving device, data analysis device and data processing device, The calibration device is used to generate attitude compensation data and equipment compensation data; The inertial device includes a laser gyro component and an acceleration measurement component, the laser gyro component is used to collect instantaneous angular velocity, and the acceleration measurement component is used to collect instantaneous acceleration; The coordinate solving device is used to calculate the first speed and the first coordinate of the train; The positioning device is used to collect the second speed and the second coordinate of the train; The mileage device is used to collect pulse signals from the wheels; The data analysis device is used to generate a third speed according to the effective diameter of the wheel and update the effective diameter of the navigation wheel; The data processing device is used to generate a first filtering model and a second filtering model and predict an estimated value of a first state matrix or a second state matrix; The data generating device is used to generate navigation data of the train.

Images