CN101915852B - Velocity Measurement Method Based on Stereo Vision - Google Patents

Abstract

The invention discloses a speed measuring method based on stereoscopic vision, which comprises the following steps: s1, collecting the image of the front static marker when the carrier runs, processing the image in the space dimension and the time dimension according to the correlation of the image information in the space and the time dimension to obtain the measured value of the running speed of the carrier

S2, measuring the change rate omega of the gradient phi of the carrier running track and measuring the acceleration a of the carrier running direction_xUsing measured values of speed of operation

Rate of change ω and acceleration a_xAnd establishing a process model and a measurement model for estimating the carrier running speed, and estimating the carrier running speed as a final result by using a Kalman filtering algorithm according to the process model and the measurement model. The method can eliminate the influence of wheel idle and slip on the speed measurement precision; and low-noise and real-time speed measurement can be realized.

Description

Velocity Measurement Method Based on Stereo Vision

technical field

The invention belongs to the technical field of rail transit, in particular to a speed measurement method based on stereo vision.

Background technique

Speed is an important data for train driving and safety control, and its accuracy and reliability determine the running quality of the train. At present, the running speed of the train is mainly measured by directly or indirectly counting the wheels, and then the speed of the train is calculated according to the circumference of the wheels and the number of revolutions of the wheels per unit time. The accuracy of this method is affected by wheel diameter wear and relative slip between wheel and rail. Although some existing compound speed measurement methods have fused the data of some sensors, the measurement data based on the wheel count is still the main basis for speed estimation, so its accuracy is still affected by the wear of the wheel diameter and the relative slip between the wheel and the rail.

Therefore, although the speed measurement method based on wheel counting is simple in principle and easy to implement, its measurement accuracy is easily affected by wheel diameter changes, wheel idling, and slippage. Moreover, the timing of idling and slipping and the resulting errors are difficult to describe mathematically, so their influence cannot be eliminated through complex mathematical processing and fusion filtering.

Contents of the invention

(1) Technical problems to be solved

One of the technical problems to be solved by the present invention is how to eliminate the influence of wheel idling and skidding on the speed measurement accuracy; the other is how to realize low-noise and real-time speed measurement.

(2) Technical solution

In order to solve the problems of the technologies described above, the invention provides a speed measurement method based on stereo vision, comprising the following steps:

S1, implement the video-based speed calculation method: collect the image of the stationary marker in front of the carrier when it is running, and obtain the running speed of the carrier by processing the image in the space and time dimensions according to the correlation of image information in space and time measured value of

变化率ω以及加速度a_x建立进行载体运行速度估计的过程模型和测量模型，然后根据该过程模型和测量模型利用卡尔曼滤波算法估计出载体的运行速度作为最终结果。S2, implement the fusion algorithm: measure the change rate ω of the slope φ of the carrier’s running track, and measure the acceleration a _x in the running direction of the carrier, and use the measured value of the running speed

The rate of change ω and the acceleration a _x establish a process model and a measurement model for estimating the carrier's running speed, and then use the Kalman filter algorithm to estimate the carrier's running speed as the final result according to the process model and measurement model.

Wherein, in step S1, two cameras are used to collect images of markers, the two cameras are placed in parallel to form a binocular stereo vision system, and both cameras are installed on the front of the vehicle, depending on the installation height, they are adjusted so that they are relatively horizontal. The included angle is between 5 degrees and 85 degrees.

Wherein, in step S2, a gyroscope is used to measure the rate of change ω of the gradient φ of the carrier's running track, and an accelerometer is used to measure the acceleration a _x of the carrier's running direction.

Wherein, step S1 further includes the following steps:

S11, at time _t1, video sampling is performed on the stationary marker in front of the carrier when it is running, and images of the same marker are obtained by two cameras at the same time;

S12. Select a feature point P _i in one of the images, where the feature point refers to a point in a pixel area with a relatively obvious boundary, clear contrast, and easy tracking and identification;

S13, use the feature point P _i selected in step S12 to find the matching feature point P′ _i in another image to form a set of matching pairs of the same feature point in the two images {P _i , i=1, 2,..., N} and {P′ _i , i=1, 2,..., N};

S14, according to the position distance b of the two cameras and the matching pair set obtained in step S13, calculate the position x _i of the feature point P _i in the camera coordinate system, and record the feature point P _i and its coordinate position {P _i , x _i };

S15, video sampling is performed at time _t2 ;

S16, also use the set of feature points obtained in step S12 to perform a matching search in the image captured by the same camera at time _t2 , and obtain a set of matching feature points

按步骤S14中的方法计算出步骤S12确定的特征点P_i在时刻t₂在摄像头坐标系下的位置{P_i，x_i′}；S17, use the result obtained in step S16 to perform matching in the image captured by another camera at time _t2 to obtain a corresponding set of matching points

Calculate the position {P i _, x _i '} of the feature point P _i determined in step S12 at the moment _t2 in the camera coordinate system by the method in step S14;

S18, according to the results obtained in steps S14 and S17 and the time interval between two adjacent video samples, calculate the speed of the feature point in the camera coordinate system

S19. Carry out coordinate transformation according to the installed geometric positions of the two cameras to obtain the speed of the feature point in the carrier coordinate system, and the calculated speed is the running speed of the carrier;

作为载体运行速度的测量值。S10, calculate the speed of all feature points at the same time, take the average value as the running speed v of the carrier at that time, and then take the component of the running speed v in the running direction of the carrier

As a measure of the speed at which the carrier is running.

Wherein, step S2 further comprises the following steps:

S21, use the single-axis gyroscope to measure the rate of change ω of the slope φ of the track, and use the single-axis accelerometer to measure the acceleration a _x of the carrier’s running direction, and use the following formulas to describe the measurement models of the single-axis gyroscope and the single-axis accelerometer respectively :

$\left\{\begin{array}{c}\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; \&phi;</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; \&omega;</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; b</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; \&upsi;</span>}\\ \stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; b</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 0</span>}\\ {\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; v</span>}}}_{\mathrm{<span\; class="notranslate">\; x</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; g</span>}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&phi;</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; \&eta;</span>}\end{array}\right.<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span>$

Among them, b is the measurement deviation of the uniaxial gyroscope, υ is the measurement noise of the uniaxial gyroscope, g is the acceleration of gravity, η is the measurement noise of the uniaxial accelerometer, and the Gaussian white noise model is used to represent two kinds of measurement noise: υ～ N(0, Q), η～N(0, R), wherein E[υ′υ]=Q, E[η′η]=R, E[*] represents the expected value, which is obtained statistically according to the actual measurement data, The superscript "'" means transpose operation;

S22, using the measured value obtained in step S1 to express the carrier velocity as

${\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}}^{\mathrm{<span\; class="notranslate">\; m</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; \&epsiv;</span>}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; )</span>$

Where ε is the measurement error, ε～N(0, H), where E[ε′ε]=H, H is statistically obtained according to the actual measurement data, and v _x is the real value of the carrier velocity;

S23, formula (1) is linearized as:

$\left[\begin{array}{c}\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; \&phi;</span>}}\\ \stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; b</span>}}\\ {\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; v</span>}}}_{\mathrm{<span\; class="notranslate">\; x</span>}}\end{array}\right]<span\; class="notranslate">\; =</span>\left[\begin{array}{ccc}\mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; g</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\end{array}\right]\left[\begin{array}{c}\mathrm{<span\; class="notranslate">\; \&phi;</span>}\\ \mathrm{<span\; class="notranslate">\; b</span>}\\ {\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}}\end{array}\right]<span\; class="notranslate">\; +</span>\left[\begin{array}{cc}\mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}\end{array}\right]\left[\begin{array}{c}\mathrm{<span\; class="notranslate">\; \&omega;</span>}\\ {\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}}\end{array}\right]<span\; class="notranslate">\; +</span>\left[\begin{array}{cc}\mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}\end{array}\right]\left[\begin{array}{c}\mathrm{<span\; class="notranslate">\; \&upsi;</span>}\\ \mathrm{<span\; class="notranslate">\; \&eta;</span>}\end{array}\right]<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 3</span>}<span\; class="notranslate">\; )</span>$

${\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}}^{\mathrm{<span\; class="notranslate">\; m</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{ccc}\mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}\end{array}\right]\left[\begin{array}{c}\mathrm{<span\; class="notranslate">\; \&phi;</span>}\\ \mathrm{<span\; class="notranslate">\; b</span>}\\ {\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}}\end{array}\right]<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; \&epsiv;</span>}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 4</span>}<span\; class="notranslate">\; )</span>$

Equations (3) and (4) are the process model and measurement model for speed estimation using the Kalman filter algorithm, respectively. According to the process model and measurement model, the Kalman filter algorithm is used to estimate the running speed of the carrier.

Among them, the process of estimating the running speed of the carrier by using the Kalman filter algorithm according to the process model and the measurement model is as follows:

(1) Using the selected sampling frequency f _s to discretize equations (3) and (4) respectively to obtain a discretization model, wherein the sampling frequency f _s is greater than 10 Hz;

(2) According to the discretization model obtained in (1), and the noise variance of the measurement noise of the uniaxial gyroscope and uniaxial accelerometer obtained by statistics, the Kalman filter is calculated by using the standard steady-state Kalman filter algorithm;

(3) Execute step a at each sampling time: read the measured values of the single-axis accelerometer and single-axis gyroscope, and then input the measured values of step S1 and step a at the same sampling time into the Kalman filter for filtering , to get the output as the final result.

Wherein, the Kalman command of the Matlab software control toolbox is used to implement the standard steady-state Kalman filter algorithm.

Wherein, the sampling frequency f _s is greater than 10 Hz.

Wherein, the sampling frequency f _s is preferably 30 Hz.

Wherein, the carrier is preferably a train.

(3) Beneficial effects

The present invention can produce the following beneficial effects: use dual cameras to form a stereo vision configuration, install them on the front of the carrier, and realize the estimation of the speed by shooting the static scene in front of the carrier; the video-based speed estimation algorithm uses the spatial correlation of the image to perform Positioning, using the temporal correlation of feature points for tracking. By processing adjacent consecutive frames, the velocity of the carrier is calculated. The above measurement methods do not rely on counting the axles, thereby eliminating the influence of wheel idling and slipping on the measurement accuracy. Further, the Kalman (Kalman) filter fusion algorithm is used to realize the fusion of the carrier velocity measurement value and the accelerometer and gyroscope measurement values, realizing low-noise, real-time velocity measurement. In addition, the measuring device is easy to install and easy to maintain.

Description of drawings

Fig. 1 is the method flowchart of the embodiment of the present invention;

Fig. 2 is a system block diagram used in the method of the embodiment of the present invention;

Fig. 3 is a schematic diagram of the installation position and angle of the camera when implementing the method of the embodiment of the present invention;

Fig. 4 is a schematic diagram of the principles of the video-based speed calculation method in the method of the embodiment of the present invention.

Detailed ways

The specific implementation manners of the present invention will be further described in detail below in conjunction with the accompanying drawings and embodiments. The following examples are used to illustrate the present invention, but are not intended to limit the scope of the present invention.

The embodiment of the present invention is described by taking a train as an example. The flow chart of the method of the embodiment of the present invention is shown in FIG. 1 . As shown in Figure 2, the method of the present invention utilizes the system realization that following several devices are formed: 2 CCDs (Charge-coupled Device, charge-coupled device) (or CMOS (Complementary metal-oxide-semiconductor, complementary metal oxide semiconductor) ) camera 1, 2, single-axis accelerometer, single-axis gyroscope. Wherein, the single-axis accelerometer measures the acceleration in the running direction of the train, and the single-axis gyroscope is used to measure the gradient change rate of the train running track. Two cameras are arranged in parallel to form a binocular stereo vision system to complete the real-time capture of the front view of the vehicle. The system also includes two DSP (Digital Signal Processor, digital signal processor), one is video processing DSP, which is responsible for video operation processing, and the other is speed calculation DSP, which is used to run speed measurement algorithm.

As shown in Figure 3, the two cameras are installed on the front of the car. Depending on the installation height, adjust the angle between them relative to the horizontal line to be between 5 degrees and 85 degrees, so as to ensure that the images captured by the cameras do not include the images of the car itself.

The method of the embodiment of the present invention includes two main steps. First, according to the image information collected by the camera, the measured value of the speed is obtained based on the video speed calculation method; second, data fusion is carried out in combination with the accelerometer and the gyroscope, and the Kalman filter is used. An algorithm estimates the train speed.

The speed estimation algorithm based on video is based on the correlation of image information in space and time, and calculates the running speed of the train by performing correlation operations on images in space and time dimensions. The principle description is shown in Figure 3.

In Fig. 4, A, B, C, and D are images of the same object or marker at different cameras and at different times. A and B respectively represent the image of the same marker in the two cameras at time 1; C and D respectively represent the image of the marker in the two cameras at time 2. According to the spatial correlation at the same time, the position of the marker in the camera coordinate system can be calculated, and according to the temporal correlation, the average moving speed of the carrier during the period between time 1 and time 2 can be calculated. When the continuous calculation process is carried out according to the acquisition frequency of the video (usually 30Hz), the instant speed of the carrier can be obtained. The processing steps of the algorithm are as follows:

1. Perform video sampling at time 1 (t ₁ ), and obtain snapshots of the same scene taken by camera 1 and camera 2 at the same time.

2. Find and select the feature points P _i in one of the images (such as the image taken by the camera 1). The feature points are those points in the pixel area with obvious boundaries, clear contrast, and easy to track and identify.

3. Use the feature point P _i selected in step 2 to find the matching feature point P′ _i in another image to form a set of matching pairs of the same feature point in the two images {P _i , i=1, 2, . . . , N} and {P′ _i , i=1, 2, . . . , N}.

4. Calculate the position x _i of the feature point in the camera coordinate system according to the position distance b of the two cameras and the matching set formed in step 3. The calculation of the position _xi adopts the literature [1] (Milan Sonka, Vaclav Hlavac, Roger Boyle: Image Processing, Analysis and Machine Vision, Second Edition, ISBN 0-534-95393-X, Brooks/Cole, pp.460.) The stereo vision algorithm (Stereo vision algorithm) is carried out. Record the feature point P _i and its coordinate position {P _i , x _i }.

5. Perform video sampling at time 2 (t ₂ ).

6. Also use the set of feature points obtained in step 2 to perform a matching search in the image captured by the camera at time 2 to obtain a set of matching feature points

按步骤4中同样方法计算出步骤2确定的特征点P_i在时刻2时在摄像头坐标系下的位置{P_i，x_i′}。7. Use the results in 6 to perform matching in the image captured by another camera at time _t2 to obtain the corresponding set of matching points

Calculate the position {P _i , x _i ′} of the feature point P _i determined in step 2 in the camera coordinate system at time 2 by the same method as in step 4.

8. According to the results obtained in steps 4 and 7 and the time interval between two adjacent video samples, the velocity of the feature point P _i in the camera coordinate system can be calculated

9. Carry out coordinate transformation according to the geometric relationship of the camera installation, and the velocity of the feature point P _i in the train coordinate system can be obtained. Since the scene viewed by the camera is a static scene, the calculated speed is the running speed of the train.

$v_t^i=\begin{array}{c}\left[\begin{array}{cc}\cos \mathrm{\&theta;}& \sin \mathrm{\&theta;}\\ -\sin \mathrm{\&theta;}& \cos \mathrm{\&theta;}\end{array}\right]v_c^i,\end{array}$ See Figure 3 for the definition of θ.

10. Calculate the speed of all feature points, and take the average as the speed of the train at this moment:

$\mathrm{<span\; class="notranslate">\; v</span>}<span\; class="notranslate">\; =</span>\frac{\underset{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\overset{\mathrm{<span\; class="notranslate">\; N</span>}}{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}}{\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}^{\mathrm{<span\; class="notranslate">\; i</span>}}}{\mathrm{<span\; class="notranslate">\; N</span>}}$

(列车坐标系参见图3)作为列车速度的测量值。As the column moves in orbit, we focus on its speed. So take the x-direction component of v

(See Figure 3 for the train coordinate system) as the measured value of the train speed.

11. Go back to step 1 and continue running.

According to the above steps, the train speed calculated by using the video image can be obtained. Since the video processing is affected by the sampling rate of the camera and the processing capability of the DSP, there will be a delay in the calculated speed. In order to improve delay and reduce noise, the speed obtained by video processing is further fused with the accelerometer and gyroscope using the Kalman filter algorithm to achieve instant, low-noise, and accurate speed estimation. The process is as follows:

The gyroscope measures the rate of change ω of the horizontal slope φ of the track. The accelerometer measures the acceleration a _x in the running direction of the train. Considering noise and measurement bias, the measurement models of gyroscope and accelerometer can be described by the following equations respectively.

$\left\{\begin{array}{c}\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; \&phi;</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; \&omega;</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; b</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; \&upsi;</span>}\\ \stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; b</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 0</span>}\\ {\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; v</span>}}}_{\mathrm{<span\; class="notranslate">\; x</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; g</span>}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&phi;</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; \&eta;</span>}\end{array}\right.<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span>$

Among them, in the symbol on the left side of the equation, adding "·" above the variable means to differentiate the variable, b is the measurement deviation of the gyroscope, g is the acceleration of gravity, υ and η are the measurement noises of the two sensors respectively, and we use Gaussian white noise Model,

υ～N(0, Q), η～N(0, R)

Among them, E[υ′υ]=Q, E[η′η]=R, which are statistically obtained according to the actual measurement data.

The train speed is expressed as

${\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}}^{\mathrm{<span\; class="notranslate">\; m</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; \&epsiv;</span>}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; )</span>$

Where ε is the measurement error, ε～N(0, H), E[ε′ε]=H, and v _x is the true value of the train speed.

Since the track slope usually does not exceed 10% (5.7°), equation (1) can be linearized.

$\left[\begin{array}{c}\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; \&phi;</span>}}\\ \stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; b</span>}}\\ {\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; v</span>}}}_{\mathrm{<span\; class="notranslate">\; x</span>}}\end{array}\right]<span\; class="notranslate">\; =</span>\left[\begin{array}{ccc}\mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; g</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\end{array}\right]\left[\begin{array}{c}\mathrm{<span\; class="notranslate">\; \&phi;</span>}\\ \mathrm{<span\; class="notranslate">\; b</span>}\\ {\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}}\end{array}\right]<span\; class="notranslate">\; +</span>\left[\begin{array}{cc}\mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}\end{array}\right]\left[\begin{array}{c}\mathrm{<span\; class="notranslate">\; \&omega;</span>}\\ {\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}}\end{array}\right]<span\; class="notranslate">\; +</span>\left[\begin{array}{cc}\mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}\end{array}\right]\left[\begin{array}{c}\mathrm{<span\; class="notranslate">\; \&upsi;</span>}\\ \mathrm{<span\; class="notranslate">\; \&eta;</span>}\end{array}\right]<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 3</span>}<span\; class="notranslate">\; )</span>$

${\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}}^{\mathrm{<span\; class="notranslate">\; m</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{ccc}\mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}\end{array}\right]\left[\begin{array}{c}\mathrm{<span\; class="notranslate">\; \&phi;</span>}\\ \mathrm{<span\; class="notranslate">\; b</span>}\\ {\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}}\end{array}\right]<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; \&epsiv;</span>}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 4</span>}<span\; class="notranslate">\; )</span>$

Equations (3) and (4) are the process model and measurement model based on visual speed estimation respectively. According to this model, the steady-state Kalman filter algorithm can be used to estimate the running speed of the train. The specific process is as follows:

(1) Select the calculation sampling frequency f _s , and discretize the model equations (3) and (4) to obtain a discretized mathematical model. In order to ensure the measurement accuracy, f _s should be greater than 10Hz, usually choose 30Hz.

(2) Using the discretization model obtained in (1) and the statistically obtained noise variance, the standard steady-state Kalman filter algorithm (such as using the kalman command of the Matlab software control toolbox) is used to calculate the Kalman filter.

(3) At each sampling moment: b, read the measurement data of the acceleration sensor and gyroscope; c, input the measured value calculated by the video-based speed calculation method and the result of b into the kalman filter, and after filtering, get The output of is used as the train speed measurement result.

The method of the present invention can be used not only for measuring the speed of trains but also for other carriers, but because the railway operating environment is relatively simple, it is more suitable for applying the visual method.

The above embodiments are only used to illustrate the present invention, but not to limit the present invention. Those of ordinary skill in the relevant technical field can make various changes and modifications without departing from the spirit and scope of the present invention. Therefore, all Equivalent technical solutions also belong to the category of the present invention, and the scope of patent protection of the present invention should be defined by the claims.

Claims (9)

1. a speed measurement method based on stereo vision, is characterized in that, comprises the following steps: S1, implement the video-based speed calculation method: collect the image of the stationary marker in front of the carrier when it is running, and obtain the running speed of the carrier by processing the image in the space and time dimensions according to the correlation of image information in space and time measured value of

S2, implement the fusion algorithm: measure the change rate ω of the slope φ of the carrier’s running track, and measure the acceleration a _x in the running direction of the carrier, and use the measured value of the running speed

The rate of change ω and the acceleration a _x establish the process model and measurement model of carrier running speed estimation, and then use the Kalman filter algorithm to estimate the carrier running speed as the final result according to the process model and measurement model. 2. the method for measuring speed based on stereo vision as claimed in claim 1, is characterized in that, in step S1, utilizes the image of two cameras to collect markers, and these two cameras are placed in parallel to form a binocular stereo vision system, And the two cameras are all installed on the front of the car, depending on the installation height, adjust the included angle between 5 degrees and 85 degrees relative to the horizontal line. 3. The method for measuring speed based on stereo vision as claimed in claim 1 or 2, wherein in step S2, the rate of change ω of the gradient φ of the carrier track is measured using a gyroscope, and the direction of travel of the carrier is measured using an accelerometer The acceleration a _x . 4. the method for measuring speed based on stereo vision as claimed in claim 3, is characterized in that, step S1 further comprises the following steps: S11, at time _t1, video sampling is performed on the stationary marker in front of the carrier when it is running, and images of the same marker are obtained by two cameras at the same time; S12. Select a feature point P _i in one of the images, where the feature point refers to a point in a pixel area with a relatively obvious boundary, clear contrast, and easy tracking and identification; S13, use the feature point P _i selected in step S12 to find the matching feature point P′ _i in another image to form a set of matching pairs of the same feature point in the two images {P _i , i=1, 2,..., N} and {P′ _i , i=1, 2,..., N}; S14, according to the position distance b of the two cameras and the matching pair set obtained in step S13, calculate the position x _i of the feature point P _i in the camera coordinate system, and record the feature point P _i and its coordinate position {P _i , x _i }; S15, video sampling is performed at time _t2 ; S16, also use the set of feature points obtained in step S12 to perform a matching search in the image captured by the same camera at time _t2 , and obtain a set of matching feature points $\{\stackrel{\&OverBar;}{P_i},i=\mathrm{1,2},...,N\};$ S17, use the result obtained in step S16 to perform matching in the image captured by another camera at time _t2 to obtain a corresponding set of matching points

Calculate the position {P _i , x _i '} of the feature point P _i determined in step S12 in the camera coordinate system at time _t2 according to the method in step S14; S18, according to the results obtained in steps S14 and S17 and the time interval between two adjacent video samples, calculate the speed of the feature point in the camera coordinate system

S19. Carry out coordinate transformation according to the installed geometric positions of the two cameras to obtain the speed of the feature point in the carrier coordinate system, and the calculated speed is the running speed of the carrier; S10, calculate the speed of all feature points at the same time, take the average value as the running speed v of the carrier at that time, and then take the component of the running speed v in the running direction of the carrier

As a measure of the speed at which the carrier is running. 5. the method for measuring speed based on stereo vision as claimed in claim 4, is characterized in that, step S2 further comprises the following steps: S21, use the single-axis gyroscope to measure the rate of change ω of the slope φ of the track, and use the single-axis accelerometer to measure the acceleration a _x of the carrier’s running direction, and use the following formulas to describe the measurement models of the single-axis gyroscope and the single-axis accelerometer respectively : $\left\{\begin{array}{c}\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; \&phi;</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; \&omega;</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; b</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; \&upsi;</span>}\\ \stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; b</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 0</span>}\\ {\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; v</span>}}}_{\mathrm{<span\; class="notranslate">\; x</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; g</span>}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&phi;</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; \&eta;</span>}\end{array}\right.<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span>$ Among them, b is the measurement deviation of the uniaxial gyroscope, υ is the measurement noise of the uniaxial gyroscope, g is the acceleration of gravity, η is the measurement noise of the uniaxial accelerometer, and the Gaussian white noise model is used to represent two kinds of measurement noise: υ～ N(0, Q), η～N(0, R), wherein E[υ′υ]=Q, E[η′η]=R, E[*] represents the expected value, which is obtained statistically according to the actual measurement data, The superscript "'" means transpose operation; S22, using the measured value obtained in step S1 to express the carrier velocity as ${\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}}^{\mathrm{<span\; class="notranslate">\; m</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; \&epsiv;</span>}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; )</span>$ Where ε is the measurement error, ε～N(0, H), where E[ε′ε]=H, H is statistically obtained according to the actual measurement data, and v _x is the real value of the carrier velocity; S23, formula (1) is linearized as: $\left[\begin{array}{c}\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; \&phi;</span>}}\\ \stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; b</span>}}\\ {\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; v</span>}}}_{\mathrm{<span\; class="notranslate">\; x</span>}}\end{array}\right]<span\; class="notranslate">\; =</span>\left[\begin{array}{ccc}\mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; g</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \text{<span class="notranslate"> 0</span>}\end{array}\right]\left[\begin{array}{c}\mathrm{<span\; class="notranslate">\; \&phi;</span>}\\ \mathrm{<span\; class="notranslate">\; b</span>}\\ {\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}}\end{array}\right]<span\; class="notranslate">\; +</span>\left[\begin{array}{cc}\mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}\end{array}\right]\left[\begin{array}{c}\mathrm{<span\; class="notranslate">\; \&omega;</span>}\\ {\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}}\end{array}\right]<span\; class="notranslate">\; +</span>\left[\begin{array}{cc}\mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}\end{array}\right]\left[\begin{array}{c}\mathrm{<span\; class="notranslate">\; \&upsi;</span>}\\ \mathrm{<span\; class="notranslate">\; \&eta;</span>}\end{array}\right]<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 3</span>}<span\; class="notranslate">\; )</span>$ ${\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}}^{\mathrm{<span\; class="notranslate">\; m</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{ccc}\mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}\end{array}\right]\left[\begin{array}{c}\mathrm{<span\; class="notranslate">\; \&phi;</span>}\\ \mathrm{<span\; class="notranslate">\; b</span>}\\ {\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}}\end{array}\right]<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; \&epsiv;</span>}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 4</span>}<span\; class="notranslate">\; )</span>$ Equations (3) and (4) are the process model and measurement model for speed estimation using the Kalman filter algorithm, respectively. According to the process model and measurement model, the Kalman filter algorithm is used to estimate the running speed of the carrier. 6. the method for measuring speed based on stereo vision as claimed in claim 5, is characterized in that, utilizes Kalman filter algorithm to estimate the process of the running speed of carrier according to this process model and measurement model as follows: (1) Using the selected sampling frequency f _s to discretize equations (3) and (4) respectively to obtain a discretization model, wherein the sampling frequency f _s is greater than 10 Hz; (2) According to the discretization model obtained in (1), and the noise variance of the measurement noise of the uniaxial gyroscope and uniaxial accelerometer obtained by statistics, the Kalman filter is calculated by using the standard steady-state Kalman filter algorithm; (3) Execute step a at each sampling time: read the measured values of the single-axis accelerometer and single-axis gyroscope, and then input the measured values of step S1 and step a at the same sampling time into the Kalman filter for filtering , to get the output as the final result. 7. the method for measuring speed based on stereo vision as claimed in claim 6, is characterized in that, utilizes the Kalman command of Matlab software control tool box to realize the steady-state Kalman filter algorithm of described standard. 8. the method for measuring speed based on stereo vision as claimed in claim 6, is characterized in that, described sampling frequency f _s gets 30Hz. 9. The speed measuring method based on stereo vision as claimed in claim 1 or 2, characterized in that, the carrier is a train.

Images