An MEMS track positioning method assisted by an infrared optoelectronic sensor

Abstract

The present invention discloses a method for track positioning of a MEMS system assisted by an infrared optoelectronic sensor. First, a distributed optical measurement platform is built through the infrared optoelectronic sensor. The position and velocity information of the vehicle at this time are calculated by using the time difference of the interruption of the infrared optoelectronic sensor and the pre-calibrated sub-module position relationship on the test track. Then, taking the differences between the position and velocity calculated by the MEMS system and the position and velocity information calculated by the distributed optical system as the basic observation information, the extended Kalman filter is used for error estimation, and the MEMS error is feedback-corrected by using the fused error information. This method introduces optoelectronic sensor interruption speed measurement to perform data fusion with the MEMS system through the extended Kalman filter algorithm, and can better suppress the random error and cumulative error of low-cost MEMS without relying on other external information, and is applicable to urban high-precision track positioning systems in scenarios where weak signals, high dynamics, etc. are prone to satellite loss of lock.

Description

Technical Field

[0001] The present invention relates to the technical fields of infrared sensor induction technology, multi-sensor data fusion, and track positioning technology, and particularly relates to a MEMS (Micro-Electro Mechanical System, an inertial sensor system based on microelectromechanics) track positioning method assisted by an infrared optoelectronic sensor. Background Art

[0002] Most rail trains adopt communication-based train control systems, and high-precision track positioning technology is one of the key technologies of train control systems. Whether the positioning result is accurate directly affects the operation efficiency and accuracy of trains. Currently, the methods for obtaining track positioning mainly include beacon positioning, speed measurement positioning method, query responder method, cable loop positioning technology, axle counting positioning, global satellite positioning method, and wireless spread spectrum positioning. The cable loop positioning technology divides the rail into different sections, and a sending and receiving device is added at the beginning and end of each section to form an information transmission loop. This method has low accuracy. The speed measurement positioning technology based on an odometer is a relatively common urban track positioning method. This method samples periodic pulses through an installed encoder to calculate the moving distance. The errors mainly come from two aspects: counting errors, such as idling, sliding, and creep, and wheel diameter wear. Train positioning based on speed measurement can use an acceleration sensor and a gyroscope to measure the acceleration of the train in three-dimensional space and then calculate the train running speed through integration, or it can be measured by the Doppler radar speed measurement method. The disadvantage is that there is an accumulated error. Using low-cost MEMS devices will bring higher random noise and accumulated errors, and the Doppler phenomenon is not obvious at low speeds and the accuracy is not high. Therefore, on the premise that the accuracy of a single sensor is limited, a multi-sensor information fusion method should be adopted to correct the positioning result through information feedback. Due to the characteristics of the urban track environment where satellite signals are easily blocked and there is strong electromagnetic interference, satellite signals are sometimes lost, and it is difficult to use global satellite positioning technology to correct the accumulated errors brought by the odometer or inertial positioning method. Summary of the Invention

[0003] Problems to be Solved by the Invention: Aiming at the problem that the application of a single sensor in track positioning has limited accuracy and the use of satellite signals as the information source for multi-sensor information fusion is easily lost due to environmental interference, in order to improve the positioning accuracy and the robustness of the system, the present invention proposes a MEMS track positioning method assisted by an infrared optoelectronic sensor.

[0004] Technical Solutions: To achieve the above object, the technical solutions adopted by the present invention are as follows

[0005] A MEMS track positioning method assisted by an infrared optoelectronic sensor includes the following steps:

[0006] (1) Calculate the position and velocity information of the vehicle carrier by using the time difference of the interruption of the infrared optoelectronic sensor and the pre-calibrated position relationship of the sub-modules on the test track;

[0007] (2) Construct the observable and the observation equation;

[0008] (3) Linearize the observation equation and use the extended Kalman filter to estimate the random error and cumulative error of the MEMS;

[0009] (4) Correct the error output by the MEMS and output the corrected position and velocity information of the vehicle carrier.

[0010] In the above step (1), the equations for calculating the position and velocity information of the vehicle carrier by using the time difference of the interruption of the infrared optoelectronic sensor and the pre-calibrated position relationship of the sub-modules on the test track are as follows:

[0011] x m ·m = x n ·(m - 1), n >> m

[0012] Δx m，n = x n / m

[0013] Ax i = (i - 1)·Δx m，n = [x n ·(i - 1)] / m, i ∈ [2, m]

[0014]

[0015] where x m is the distance between two adjacent infrared optoelectronic emitters on one side of the vehicle carrier, m + 1 is the number of emitters installed at equal intervals on one side of the vehicle carrier, x n is the distance between two adjacent infrared optoelectronic receivers on one side of the track, n + 1 is the number of receivers installed at equal intervals on one side of the track, Δx m，n is the minimum position resolution, is the i-th velocity information, t i+1 and t i are the time information of the adjacent interruption records of the infrared optoelectronic sensor in the front and back times.

[0016] In the above step (2), the observable is

[0017]

[0018] v E 、v N 、v U respectively represent the velocity components obtained by the MEMS solution in the northeast celestial coordinate system; P L 、Pλ , P h is the corresponding latitude, longitude, and altitude information under this system; v x , v y , v z is the velocity component measured by the distributed optical system and the velocity information obtained after conversion to the n-system; P x , P y , P z is the position information of the displacement measured by this system after conversion to the n-system.

[0019] In the said step (2), the observation equation is

[0020] z k = H k · x k + n k

[0021] where H k is the measurement matrix at time k, satisfying the matrix form H k = [0 3×6 I 6 0 6×6 , n k is the measurement noise matrix of the infrared optoelectronic sensor at time k, and x k is the state vector of the system at time k.

[0022] In the said step (3), the discretized system motion equation is

[0023]

[0024] where the state transition matrix

[0025]

[0026] The noise control matrix

[0027]

[0028] The noise distribution matrix

[0029] w = [w g w a w cg w ca T

[0030] w cg , w ca are the process white noises of the gyroscope and accelerometer respectively, and w g , w a are the bias white noises of the gyro and accelerometer respectively, and x k-1 is the state vector of the system at time k - 1,​ is the derivative of the system state vector at time k with respect to time.

[0031] In step (4), the correction method for the position and velocity information errors output by the MEMS system is indirect feedback correction. Using the attitude error, velocity error, three-axis position error, gyroscope zero bias, and accelerometer zero bias of the system as state variables, and the estimated error amount as the parameters in the MEMS system mechanical arrangement equation, the corrected position and velocity information of the MEMS system is output.

[0032] Beneficial effects: Compared with the prior art, the present invention introduces an infrared optoelectronic sensor as an information source to provide position and velocity information, corrects the problems of long-term error accumulation and accuracy divergence of MEMS devices through the extended Kalman filtering method, improves the signal output frequency and positioning accuracy of the information source by improving the distribution method of the infrared optoelectronic sensor, realizes multi-information source fusion orbit positioning without relying on external information sources such as satellite signals. At the same time, the present invention uses microelectromechanical inertial components and uses infrared sensors to provide fusion information to suppress the random noise and cumulative errors of low-cost MEMS devices, ensuring orbit positioning accuracy while reducing technical costs. Compared with the transponder query method with a relatively similar principle, the infrared sensor has higher interruption accuracy, is less susceptible to environmental interference, has a shorter response time, and can measure a higher upper limit value of speed. Compared with other methods, it also improves the accuracy and robustness of the orbit positioning system. Description of the Drawings

[0033] Figure 1 is the method flow chart of the embodiment of the present invention;

[0034] Figure 2 is the installation schematic diagram of the infrared optoelectronic sensor of the present invention;

[0035] Figure 3 is the infrared optoelectronic sensor t of the present invention 2 at time;

[0036] Figure 4 is the infrared optoelectronic sensor t of the present invention m+1 at time. Detailed Embodiments

[0037] The following further clarifies the present invention in conjunction with specific embodiments. It should be understood that these embodiments are only used to illustrate the present invention and not to limit the scope of the present invention. After reading the present invention, various equivalent forms of modification of the present invention by those skilled in the art all fall within the scope defined by the appended claims of this application.

[0038] Embodiment 1: Refer to Figures 1 - 4 As shown in Figure 1As shown in the figure, a MEMS track positioning method assisted by an infrared optoelectronic sensor disclosed in an embodiment of the present invention mainly includes the following steps:

[0039] S1: Calculate the position and speed information of the carrier by using the time difference of the interruption of the infrared optoelectronic sensor and the pre-calibrated position relationship of the sub-modules on the test track;

[0040] S1-1: The installation method of the infrared optoelectronic sensor on the carrier and the track is as Figure 2 shown. On one side of the track, n + 1 receivers are installed equidistantly at a spacing of x n On one side of the carrier, m + 1 transmitters are installed equidistantly at a spacing of x m The spacing on the carrier side and the spacing on the track side satisfy the relationship:

[0041] x m ·m = x n ·(m - 1), n >> m

[0042] When the carrier has not moved, the optical axes of the first transmitter on the carrier and the first receiver on the track coincide, and the optical axes of the (m + 1)-th transmitter on the carrier and the m-th receiver on the track coincide. The remaining receivers on the track cannot receive the infrared signal. At this time, the difference in the optical axes between the second transmitter on the carrier and the second receiver on the track is

[0043] Δx m，n = x n - x m

[0044] x n is a preset value, then

[0045] Δx m，n = x n / m

[0046] Then the difference in the optical axes between the i-th transmitter on the carrier and the i-th receiver on the track at this time is

[0047] Δx i = (i - 1)·Δx m，n = [x n ·(i - 1)] / m

[0048] When the carrier starts to move, all the receivers on the track no longer receive the infrared signal. When the first receiver on the track just stops receiving the infrared signal, record this moment as t 1 , this moment is the starting time node of the carrier's movement. Until a certain moment t 2 , the carrier moves to the position where the optical axes of the second transmitter on the carrier and the second receiver on the track coincide, as Figure 3 shown. At this time, the moving distance of the carrier is Δx m，n, from which the first speed information is obtained as

[0049]

[0050] At this time, the difference between the optical axes of the third transmitter on the vehicle and the third receiver on the orbit is Δx m，n , and the differences of the remaining optical axes are all reduced by Δx m，n , the vehicle continues to move, and all the receivers on the orbit no longer receive infrared signals until a certain moment t 3 , the vehicle moves to the position where the optical axes of the third transmitter on the vehicle and the third receiver on the orbit coincide, from which the second speed information is obtained as

[0051]

[0052] The vehicle continues to move, repeating the signal reception state of the receiver during the above-mentioned period from t 2 to t 3 until a certain moment t m+1 , the vehicle moves to the position where the optical axes of the (m + 1)-th transmitter on the vehicle and the (m + 1)-th receiver on the orbit coincide. As Figure 4 shown, at this time, the optical axes of the first transmitter on the vehicle and the second receiver on the orbit exactly coincide, from which the m-th speed information is obtained as

[0053]

[0054] At this moment, the signal reception state of the receiver on the orbit returns to the moment when the vehicle starts to move. When the vehicle continues to move, it will repeat the signal reception state of the receiver during the above-mentioned period from t 1 to t m+1 . The output of speed and position information is cycled in the spatial dimension with the interval x n between two adjacent receivers on the orbit as the period, and m speed and position information are output in each cycle.

[0055] S2: Construct observables and observation equations;

[0056] S2-1: The observables are:

[0057]

[0058] v E 、v N 、v U respectively represent the velocity components obtained by MEMS solution in the northeast celestial coordinate system; P L 、P λ 、P h are the corresponding latitude, longitude, and altitude information in this coordinate system; v x 、v y 、v zThe velocity components measured for the distributed optical system, and the velocity information obtained after transformation to the n-frame; P x 、P y 、P z are the position information of the displacement measured by the system after transformation to the n-frame.

[0059] S2-2: The observation equation is:

[0060] z k =H k ·x k +n k

[0061] where H k is the measurement matrix at time k, satisfying the matrix form H k =[0 3×6 I 6 0 6×6 , n k is the measurement noise matrix of the infrared optoelectronic sensor at time k, and x k is the state vector of the system at time k.

[0062] S3: Linearize the observation equation and use the extended Kalman filter to estimate the random error and cumulative error of the MEMS;

[0063] S3-1: Use the angular velocity information measured by the MEMS gyroscope to calculate the direction cosine matrix of the carrier from the n-frame to the b-frame (body coordinate system) to obtain the attitude information through transformation; then measure the specific force through the built-in accelerometer and obtain the position and velocity information through navigation solution. Since the MEMS devices used have low cost, poor stability, and large noise, the influence of the earth's angular velocity of rotation, the entrained angular velocity caused by vehicle movement, and the velocity oar effect can be ignored in the calculations of this paper. Then, taking the n-frame as the reference frame, the attitude differential equation ignoring the earth's rotation is:

[0064]

[0065] In the formula, (ω×) represents the skew-symmetric matrix form of the angular rate, represents the direction cosine matrix rotating from the b-frame to the n-frame, represents the angular rate output by the gyroscope, is where represents the earth's angular velocity of rotation, represents the earth's entrained angular velocity. Similarly, the velocity differential equation can be approximated as:

[0066]

[0067] where vn represents the velocity of the n - system download body, g n represents the local gravitational acceleration in the n - system, f b represents the specific force output by the accelerometer. Considering that the velocity component along the celestial direction is almost zero, the position update equation can be expressed as:

[0068]

[0069] where L, λ, h represent latitude, longitude, and altitude respectively; R M and R N represent the principal radii of curvature of the meridian and prime vertical respectively. Assume that the sampling time interval of the MEMS is T s , then the discrete update equation can be expressed as:

[0070]

[0071]

[0072]

[0073] Thus, the data measured by the gyroscope and accelerometer can be resolved to obtain the velocity and position information output by the MEMS device.

[0074] S3 - 2: Select the geocentric coordinate system i as the inertial reference frame, the northeast - celestial coordinate system n as the navigation coordinate system, and the vehicle body coordinate system b as the right - front - up coordinate system. Establish a 15 - dimensional state - vector motion model as:

[0075] x = [φ δv n δp b g b a T

[0076] where φ is the attitude error vector, δv n is the velocity error vector, δp is the position error vector, b g is the zero - bias vector of the three - axis gyroscope, b a is the zero - bias vector of the three - axis accelerometer. The zero - biases of the gyroscope and accelerometer both conform to the first - order Markov process, that is:

[0077]

[0078] where w cg 、w ca are the process white noises of the gyroscope and accelerometer respectively.

[0079] S3 - 3: The motion equation of the system can be expressed as:

[0080] ​

[0081] The discretized system motion equation is as follows:

[0082]

[0083] where the state transition matrix

[0084]

[0085] noise control matrix

[0086]

[0087] noise distribution matrix

[0088] w = [w g w a w cg w ca T

[0089] w cg and w ca are the process white noises of the gyroscope and accelerometer respectively, and w g and w a are the bias white noises of the gyro and accelerometer respectively.

[0090] S3-4: According to the system motion equation in S3-2 and the observation equation in S2-2, the Kalman filter of the fusion system is as follows:

[0091] x k / k-1 = Φ k / k-1 · x k-1

[0092]

[0093]

[0094] x k = x k / k-1 + K k (z k - H k x k / k-1 )

[0095] P k = (I - K k H k )P k / k-1

[0096] where the system process noise and measurement noise w k-1 and n k at the (k - 1)th moment are uncorrelated and both follow a Gaussian white noise distribution with a mean of zero, and the variances are Q​k , R k . Denote x k / k-1 as the optimal state estimate of k with respect to the (k - 1)th moment, P k / k-1 as the predicted variance matrix of k with respect to the (k - 1)th moment, and P k as the covariance matrix at the kth moment.

[0097] S4: Indirect feedback correction is performed on the position and velocity information errors output by the MEMS system. Using the attitude error, velocity error, three-axis position error, gyroscope zero bias, and accelerometer zero bias of the system as state variables, according to the covariance matrix P calculated by the extended Kalman filter k , correct the parameters in the mechanical arrangement equation of the MEMS system and output the corrected position and velocity information of the MEMS system.

[0098] It should be noted that the above embodiments are not used to limit the protection scope of the present invention. Equivalent transformations or substitutions made on the basis of the above technical solutions all fall within the protection scope of the claims of the present invention.

Claims

1. An MEMS orbit positioning method assisted by an infrared optoelectronic sensor, characterized in that, it includes the following steps: (1), Using the time difference of the interruption of the infrared optoelectronic sensor and the pre-calibrated position relationship of the sub-modules on the test track to calculate the position and speed information of the carrier; (2), Constructing the observed quantity and the observation equation; (3), Linearizing the observation equation and using the extended Kalman filter to estimate the random error and cumulative error of the MEMS; (4), Correcting the error output by the MEMS and outputting the corrected position and speed information of the carrier; Among them, in the step (1), the equation for calculating the position and speed information of the carrier by using the time difference of the interruption of the infrared optoelectronic sensor and the pre-calibrated position relationship of the sub-modules on the test track is: x m ·m = x n ·(m - 1), n >> m Δx m,n = x n / m Δx i =(i - 1)·Δx m,n =[x n ·(i - 1)] / m, i ∈ [2, m] At the initial moment, the photoelectric receiver is located at the starting point, where x m is the distance between two adjacent infrared photoelectric transmitters on one side of the vehicle carrier, m + 1 is the number of transmitters installed at equal intervals on one side of the vehicle carrier, x n is the distance between two adjacent infrared photoelectric receivers on one side of the track, n + 1 is the number of receivers installed at equal intervals on one side of the track, and the four values are preset and adjusted according to the actual installation situation. Δx m,n is the minimum position resolution, which is calculated from the preset value. is the i-th speed information, t i+1 and t i are the moment information recorded by the infrared receiver interruption system in two adjacent times before and after; in the step (2), the observable quantity is v E 、v N 、v U respectively represent the velocity components obtained from MEMS solution in the northeast celestial coordinate system; P L 、P λ 、P h are the corresponding latitude, longitude, and altitude information in this coordinate system; v x 、v y 、v z are the velocity components measured by the distributed optical system, and the velocity information obtained after conversion to the n-system, the local geographic navigation system; P x 、P y 、P z are the position information of the displacement measured by this system after conversion to the n-system, In the step (2), the observation equation is z k = H k ·x k + n k where H k is the measurement matrix at time k, satisfying the matrix form H k = [0 3×6 I 6 0 6×6 , n k is the measurement noise matrix of the infrared optoelectronic sensor at time k, and x k is the state vector of the system at time k; In the step (3), the discretized system motion equation is where the state transition matrix noise control matrix noise distribution matrix w = [w g w a w cg w ca T ​ w cg and w ca are the process white noises of the gyroscope and accelerometer respectively, and w g and w a are the bias white noises of the gyro and accelerometer respectively. x k-1 is the state vector of the system at time k - 1, and is the derivative of the system state vector with respect to time at time k; In the step (4), the correction method for the position and speed information error output by the MEMS system is indirect feedback correction. Taking the attitude error, speed error, three-axis position error, gyroscope zero bias and accelerometer zero bias of the system as state variables, and the estimated error quantity as the parameters in the mechanical arrangement equation of the MEMS system, the corrected position and speed information of the MEMS system is output.

Images