Gyroscope-fused multi-sensor train speed and distance measurement algorithm method

Abstract

The invention discloses a gyroscope-fused multi-sensor train speed and distance measurement algorithm method. The method comprises the following steps: S1, mounting a gyroscope on a train; s2, after the signal system judges that the train enters the slipping or idling state, gradient data are measured through a gyroscope, and train acceleration data are obtained through an accelerometer; s3, the acceleration without the slope influence is obtained through the slope data and the acceleration data, and then the speed and positioning of the train are corrected; and S4, performing secondary supervision on data measured by the speedometer and the accelerometer through the gyroscope in a non-idle slip scene of the train. According to the invention, under the abnormal slipping or idling state, the precision of the measured value of the accelerometer is enhanced, so that the usability of the positioning function of the whole system during idling and slipping is improved.

Description

Technical Field

[0001] The invention relates to a train speed and distance measurement algorithm method integrating multiple sensors of a gyroscope. Background Art

[0002] In the current combination of speedometer and acceleration positioning, the accelerometer cannot determine the slope acceleration alone. It needs to be combined with the speedometer measurement value to perform differential calculation when there is no slipping. Then it is estimated when entering the slipping state. However, since the impact of slipping on the speedometer is earlier than the slipping determination, there is an error in the slope acceleration calculation, and since the error will continue to accumulate and amplify over time, it will trigger the safety boundary under certain circumstances, such as causing the train to lose its position due to excessive speed uncertainty. For example: when the slip conditions of different axles are inconsistent, the slope values ​​of different axles are different, resulting in the subsequent speed measurement values ​​of different axles being too different and losing position.

[0003] The accurate positioning of trains in subway train signal systems is very important, especially since subway systems usually operate in high-density urban areas with very small intervals between vehicles. Accurate signal systems can ensure a safe distance between trains and prevent collisions and other accidents.

[0004] A more common system positioning and speed measurement system architecture is a speed sensor based on axle rotation and an accelerometer to ensure the continuity of the positioning function. Note: When the train is idling and slipping, the axle rotation sensor cannot accurately locate the train, and thus the safe positioning of the train cannot be achieved.

[0005] However, the widely used one-dimensional accelerometers cannot distinguish between the acceleration caused by the slope and the acceleration caused by the running, and need to deal with the installation error during the life cycle. Currently, the method of comparing with the speedometer measurement value is used to determine the current slope and installation error, and the differential method is used to estimate the slope effect when slipping. However, the slope estimation algorithm will trigger the safety boundary-oriented safety response in some scenarios, affecting availability.

[0006] Therefore, a multi-sensor train speed and distance measurement algorithm method integrating a gyroscope is provided. Summary of the invention

[0007] The purpose of the present invention is to overcome the existing defects and provide a multi-sensor train speed and distance measurement algorithm method integrating a gyroscope, which enhances the accuracy of the measurement value of the accelerometer in abnormal slipping or idling conditions, thereby improving the availability of the overall system positioning function during idling and slipping.

[0008] The technical solution to achieve the above purpose is:

[0009] A multi-sensor train speed and distance measurement algorithm method integrating a gyroscope, comprising:

[0010] Step S1, installing a gyroscope on the train;

[0011] Step S2, after the signal system determines that the train enters a slipping or idling state, the slope data is measured by a gyroscope, and the train acceleration data is obtained by a speedometer and an accelerometer;

[0012] Step S3, obtaining the acceleration excluding the influence of the slope through the slope data and the acceleration data, and then correcting the speed and positioning of the train;

[0013] Step S4: When the train is in a non-idling slipping scenario, the data measured by the accelerometer is secondarily supervised by the gyroscope.

[0014] Preferably, in step S1, the gyroscope is a single-axis MEMS gyroscope.

[0015] Preferably, in step S2, after the signal system determines that the train has entered a slipping or idling state, a gyroscope and an accelerometer are used for fusion calculation based on the following assumptions to perform reasonable position and speed compensation:

[0016] After the nth cycle, the output of the train acceleration sensor is a n , and use a first-order low-pass filter on it, that is:

[0017] A(n)=K a *a n +(1-K a )A(n-1);

[0018] In the formula, K a is the acceleration filter coefficient;

[0019] At this time, the angular velocity output by the train gyroscope is:

[0020] ω n =Δθ / Δt;

[0021] Where Δθ is the change in train slope, Δt is the time interval;

[0022] And use a first-order low-pass digital filter on it, that is:

[0023] W(n)=K ω *ω n +(1-K ω )W(n-1);

[0024] In the formula, K ω is the angular velocity filter coefficient;

[0025] The angular velocity is integrated to obtain the deviation angle, and the slope is calculated based on it, that is:

[0026]

[0027] In the formula, θ 0 is the initial slope, T App For the processing cycle;

[0028] Then according to the calculation, the acceleration of the train at this time can be reduced to:

[0029] A mea (n) = A(n) - θ(n)*g;

[0030] Where g is the acceleration due to gravity, 9.80 m / s 2 .

[0031] Preferably, in step S3, the speed at this time is integrated according to the calculated acceleration:

[0032]

[0033] Where V 0 is the initial velocity;

[0034] The position at this time can be calculated by integrating the driving speed:

[0035]

[0036] In the formula, S 0 is the initial position.

[0037] Preferably, in step S4, the data measured by the speedometer and the accelerometer are supervised twice by a gyroscope, that is:

[0038] Acceleration of the speedometer:

[0039] A wheelx (n)=[V wheelx (n)-V wheelx (n-1)] / T Application_Cycle ;

[0040] Where V wheelx is the speed of the speedometer, T Application_Cycle is the operation cycle;

[0041] Accelerometer acceleration:

[0042] A means (n) = (1-1 / p)*A means (n-1)+1 / p*A raw (n);

[0043] Where p is the acceleration filter factor, A raw(n) for;

[0044] The difference is the acceleration of the slope:

[0045] A gradex (n) = A difx (n) = (1-1 / p)*A difx (n-1)+1 / p*(A means (n)-

[0046] A wheelx (n));

[0047] In the formula, A gradex (n) is the acceleration of the slope, A difx (n) is the difference between the velocity meter and the accelerometer;

[0048] Then, the slope angle θ n The calculation formula is as follows:

[0049] tan(θ n )=A gradex (n) / g;

[0050] Using the inverse tangent function, we get:

[0051] θ n =arctan(A gradex (n) / g);

[0052] At this time, gyroscope supervision is used, theoretically θ n The rate of change is proportional to the measurement value of the gyroscope, that is:

[0053] θ n -θ n-1 =K W *W(n)*T App ;

[0054] In the formula, K W is the correction factor.

[0055] The beneficial effects of the present invention are as follows: the present invention utilizes the original speedometer, accelerometer, and newly added gyroscope to enhance the accuracy of the accelerometer's measurement value in an abnormal slipping or idling state, thereby improving the availability of the overall system positioning function during idling slipping, thereby avoiding the need to re-establish the positioning process, improving the overall subway operation efficiency, and in normal circumstances without slipping, achieving mutual supervision among the three to ensure the reliability of the enhanced slipping algorithm. BRIEF DESCRIPTION OF THE DRAWINGS

[0056] Figure 1 It is a flow chart of a train speed and distance measurement algorithm method integrating multiple sensors of a gyroscope according to the present invention;

[0057] Figure 2 It is a schematic diagram of the installation of a gyroscope when a one-dimensional curve is used in the present invention. DETAILED DESCRIPTION

[0058] The technical solution of the present invention will be described clearly and completely below in conjunction with the accompanying drawings. In the description of the present invention, it should be noted that the terms "center", "up", "down", "left", "right", "vertical", "horizontal", "inside", "outside" and the like indicate directions or positional relationships based on the directions or positional relationships shown in the accompanying drawings, which are only for the convenience of describing the present invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific direction, be constructed and operated in a specific direction, and therefore cannot be understood as a limitation on the present invention. In addition, the terms "first", "second", and "third" are used for descriptive purposes only and cannot be understood as indicating or implying relative importance.

[0059] The present invention will be further described below in conjunction with the accompanying drawings.

[0060] like Figure 1 As shown, a multi-sensor train speed and distance measurement algorithm method integrating a gyroscope includes:

[0061] Step S1, installing a gyroscope on the train.

[0062] In the embodiment, the gyroscope adopts a single-axis MEMS gyroscope, which has the unique advantages of small size, light weight, low cost and mass production. Assuming that the motion trajectory of the train is a one-dimensional curve as shown in the figure, in which the train moves forward along the X axis and pitches along the Z axis, the installation method of the gyroscope is as follows: Figure 2 shown.

[0063] Step S2, after the signal system determines that the train has entered a slipping or idling state, the slope data is measured by a gyroscope, and the train acceleration data is obtained by a speedometer and an accelerometer.

[0064] In the embodiment, after the signal system determines that the train has entered a slipping or idling state, the gyroscope and the accelerometer are used for fusion calculation based on the following assumptions to perform reasonable position and speed compensation:

[0065] After the nth cycle, the output of the train acceleration sensor is a n , and use a first-order low-pass filter on it, that is:

[0066] A(n)=K a *a n +(1-K a )A(n-1);

[0067] In the formula, K a is the acceleration filter coefficient;

[0068] At this time, the angular velocity output by the train gyroscope is:

[0069] ω n =Δθ / Δt;

[0070] Where Δθ is the change in train slope, Δt is the time interval;

[0071] And use a first-order low-pass digital filter on it, that is:

[0072] W(n)=K ω *ω n +(1-K ω )W(n-1);

[0073] In the formula, K ω is the angular velocity filter coefficient;

[0074] The angular velocity is integrated to obtain the deviation angle, and the slope is calculated based on it, that is:

[0075]

[0076] In the formula, θ 0 is the initial slope, T App For the processing cycle;

[0077] Then according to the calculation, the acceleration of the train at this time can be reduced to:

[0078] A mea (n) = A(n) - θ(n)*g;

[0079] Where g is the acceleration due to gravity, 9.80 m / s 2 .

[0080] Step S3, obtaining the acceleration excluding the influence of the slope through the slope data and the acceleration data, and then correcting the speed and positioning of the train.

[0081] In the embodiment, the speed at this time is integrated according to the calculated acceleration:

[0082]

[0083] Where V 0 is the initial velocity;

[0084] The position at this time can be calculated by integrating the driving speed:

[0085]

[0086] In the formula, S 0 is the initial position.

[0087] Step S4: When the train is in a non-idling slipping scenario, the data measured by the accelerometer is secondarily supervised by the gyroscope.

[0088] In the embodiment, the data measured by the speed meter and the accelerometer are supervised twice by the gyroscope, namely:

[0089] Acceleration of the speedometer:

[0090] A wheelx (n)=[V wheelx (n)-V wheelx (n-1)] / T Application_Cycle ;

[0091] Where V wheelx is the speed of the speedometer, T Application_Cycle is the operation cycle;

[0092] Accelerometer acceleration:

[0093] A means (n) = (1-1 / p)*A means (n-1)+1 / p*A raw (n);

[0094] Where p is the acceleration filter factor, A raw (n) for;

[0095] The difference is the acceleration of the slope:

[0096] A gradex (n) = A difx (n) = (1-1 / p)*A difx (n-1)+1 / p*(A means (n)-

[0097] A wheelx (n));

[0098] In the formula, A gradex (n) is the acceleration of the slope, A difx (n) is the difference between the velocity meter and the accelerometer;

[0099] Then, the slope angle θ n The calculation formula is as follows:

[0100] tan(θ n )=A gradex (n) / g;

[0101] Using the inverse tangent function, we get:

[0102] θ n =arctan(Agradex (n) / g);

[0103] At this time, gyroscope supervision is used, theoretically θ n The rate of change is proportional to the measurement value of the gyroscope, that is:

[0104] θ n -θ n-1 =K W *W(n)*T App ;

[0105] In the formula, K W is the correction factor.

[0106] The above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit the same. Although the present invention has been described in detail with reference to the above embodiments, those skilled in the art should understand that the technical solutions described in the above embodiments may still be modified, or some or all of the technical features may be replaced by equivalents. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A multi-sensor train speed and distance measurement algorithm method integrating a gyroscope, characterized in that: include: Step S1, installing a gyroscope on the train; Step S2, after the signal system determines that the train enters a slipping or idling state, the slope data is measured by a gyroscope, and the train acceleration data is obtained by an accelerometer; Step S3, obtaining the acceleration excluding the influence of the slope through the slope data and the acceleration data, and then correcting the speed and positioning of the train; Step S4: When the train is in a non-idling slipping scenario, the data measured by the speedometer and the accelerometer are secondarily supervised by the gyroscope.

2. According to the multi-sensor train speed and distance measurement algorithm method integrating gyroscopes in claim 1, it is characterized in that: In step S1, the gyroscope adopts a single-axis MEMS gyroscope.

3. The train speed and distance measurement algorithm method of a multi-sensor gyroscope integrated according to claim 1 is characterized in that: In step S2, after the signal system determines that the train has entered a slipping or idling state, the gyroscope and accelerometer are used for fusion calculation based on the following assumptions to perform reasonable position and speed compensation: After the nth cycle, the output of the train acceleration sensor is a n , and use a first-order low-pass filter on it, that is: A(n)=K a *a n +(1-K a )A(n-1); In the formula, K a is the acceleration filter coefficient; At this time, the angular velocity output by the train gyroscope is: oh n =Δθ / Δt; Where Δθ is the change in train slope, Δt is the time interval; And use a first-order low-pass digital filter on it, that is: W(n)=K ω *ω n +(1-K ω )W(n-1); In the formula, K ω is the angular velocity filter coefficient; The angular velocity is integrated to obtain the deviation angle, and the slope is calculated based on it, that is: Where θ0 is the initial slope, T App For the processing cycle; Then according to the calculation, the acceleration of the train at this time can be reduced to: A mea (n)=A(n)-θ(n)*g; Where g is the acceleration due to gravity, 9.80 m / s 2 .

4. The method for train speed and distance measurement using a multi-sensor gyroscope-integrated algorithm according to claim 3 is characterized in that: In step S3, the speed at this time is integrated according to the calculated acceleration: Where V0 is the initial velocity; The position at this time can be calculated by integrating the driving speed: Where S0 is the initial position.

5. The train speed and distance measurement algorithm method of a multi-sensor gyroscope integrated according to claim 4 is characterized in that: In step S4, the data measured by the speed meter and the accelerometer are supervised twice by the gyroscope, that is: Acceleration of the speedometer: A wheelx (n)=[V wheelx (n)-V wheelx (n-1)] / T Application_Cycle ; Where V wheelx is the speed of the speedometer, T Application_Cycle is the operation cycle; Accelerometer acceleration: A means (n)=(1-1 / p)*A means (n-1)+1 / p*A raw (n); Where p is the acceleration filter factor, A raw (n) for; The difference is the acceleration of the slope: A gradex (n)=A difx (n)=(1-1 / p)*A difx (n-1)+1 / p*(A means (n)- A wheelx (n)); In the formula, A gradex (n) is the acceleration of the slope, A difx (n) is the difference between the velocity meter and the accelerometer; Then, the slope angle θ n The calculation formula is as follows: <h2 style=";text-align:left;direction:ltr">tan(θ<h2 style=";text-align:left;direction:ltr"> n <h2 style=";text-align:left;direction:ltr"> A)=<h2 style=";text-align:left;direction:ltr"> gradex <h2 style=";text-align:left;direction:ltr"> (n) / g; Using the inverse tangent function, we get: θ n =arctan(A gradex (n) / g); At this time, gyroscope supervision is used, theoretically θ n The rate of change is proportional to the measurement value of the gyroscope, that is: i n -θ n-1 =K W *W(n)*T App ; In the formula, K W is the correction factor.

Images