阅读说明：本技术 基于抗差卡尔曼滤波的bds/ins组合列车定位方法 (BDS/INS combined train positioning method based on robust Kalman filtering ) 是由 陈光武 樊子燕 石建强 程鉴皓 司涌波 邢东峰 杨菊花 李文元 于 2019-09-03 设计创作，主要内容包括：本发明公开了一种基于抗差卡尔曼滤波的BDS/INS组合列车定位方法,包括：检测列车运行的零速场景,得到所述零速场景相对应的约束条件；基于所述零速场景和相应的约束条件,获取误差模型；基于所述零速场景选择相应的抗差卡尔曼滤波器；将获取的所述误差模型的误差观测信息经选择的抗差卡尔曼滤波器修正解算后,得到修正的导航数据,基于所述导航数据对列车进行定位。以实现在多种传感器融合时,提高定位可靠性和精度的优点。(the invention discloses a BDS/INS combined train positioning method based on robust Kalman filtering, which comprises the following steps: detecting a zero-speed scene of train operation to obtain a constraint condition corresponding to the zero-speed scene; acquiring an error model based on the zero-speed scene and a corresponding constraint condition; selecting a corresponding robust Kalman filter based on the zero-speed scene; and correcting and resolving the obtained error observation information of the error model through a selected robust Kalman filter to obtain corrected navigation data, and positioning the train based on the navigation data. The method has the advantages of improving the positioning reliability and precision when various sensors are fused.)

1. A BDS/INS combined train positioning method based on robust Kalman filtering is characterized by comprising the following steps:

Detecting a zero-speed scene of train operation to obtain a constraint condition corresponding to the zero-speed scene;

acquiring an error model based on the zero-speed scene and a corresponding constraint condition;

selecting a corresponding robust Kalman filter based on the zero-speed scene;

Correcting and resolving the obtained error observation information of the error model by a selected robust Kalman filter to obtain corrected navigation data,

and positioning the train based on the navigation data.

2. the robust kalman filter based BDS/INS combination train positioning method of claim 1, wherein the stall scenario comprises:

first zero-speed scenario: the train is stationary relative to the ground;

Second zero-speed scenario: the train is in a motion state, but the speed component in the direction vertical to the ground and the motion direction vertical to the track is 0 based on the carrier coordinate system;

Or

the third zero-speed scenario: the train makes uniform linear motion relative to the ground.

3. The BDS/INS combined train positioning method based on robust Kalman filtering according to claim 1, wherein the detecting the zero-speed scene of train operation comprises:

detecting a second zero-speed scene based on the condition that the train is in a zero-speed state within a set time period;

And detecting the first zero-speed scene or the third zero-speed scene based on the speed of the detected train.

4. the method for positioning the BDS/INS combination train according to claim 3, wherein the detecting the second stall-speed scenario based on the condition that the train is in the stall-speed state within the set time period comprises:

Detecting train presence (t)₀,t_s) Whether the time is in the zero speed state meets the following conditions:

f (t) is g, ω (t) is 0, where t is₀And t_sRepresenting time, f (t) is the specific force value of the train at time t, and ω (t) is the angular velocity of the train at time tValue, g is the acceleration of gravity;

And if the condition is not met, the scene is a second zero-speed scene.

5. the robust kalman filter based BDS/INS combination train positioning method of claim 3, wherein the detecting the first or third stall-speed scenario based on the detected train speed comprises:

acquiring the output speed of a wheel speed sensor;

if the output speed is 0, the scene is a first zero speed scene;

And if the output speed is not 0, determining that the scene is a third zero-speed scene.

6. the robust kalman filter based BDS/INS combination train positioning method of claim 1, wherein the error equation comprises:

An attitude error equation, a velocity error equation, or a position error equation.

7. The robust kalman filter based BDS/INS combination train positioning method of claim 6, wherein the attitude error equation is:

wherein the content of the first and second substances,is the derivative vector of the attitude error;The projection of the rotational angular velocity of the earth in an n system is obtained;is the projection of the rotation angular velocity of n relative to e in n;are respectively asAn error of (2);Is an attitude angle vector; epsilonⁿis the equivalent gyro drift of the n system.

8. the robust kalman filter based BDS/INS combination train positioning method of claim 6, wherein the speed error equation is:

wherein the content of the first and second substances,A vector of derivatives of the velocity error; f. ofⁿis the projection of specific force in n system; vⁿIs a velocity vector; delta Vⁿa velocity error vector;is an attitude angle vector;the projection of the rotational angular velocity of the earth in an n system is obtained;Is the projection of the rotation angular velocity of n relative to e in n;are respectively asan error of (2);For accelerometer drift, Vⁿrepresenting speed, δ Vⁿrepresents Vⁿthe error of (2).

9. the robust kalman filter based BDS/INS combination train positioning method of claim 6, wherein the position error equation is:

wherein L, lambda and h are respectively the longitude, the latitude and the height of the train; δ L, δ λ, δ h are longitude, latitude and altitude errors;Derivatives of δ L, δ λ, δ h, respectively; r_M R_NRespectively, the radius of curvature, V, along the unit circle of the earth's fourth quadrant and the meridian circle_E、V_N、V_Uthe speeds in the three directions of the northeast and the northeast in the northeast coordinate system are respectively.

10. The BDS/INS combined train positioning method based on the robust Kalman filter as claimed in claim 1, wherein the robust Kalman filter is:

Real-time adjustment of observation error vector variance by adopting robust estimation on the basis of standard Kalman filteringThe size of (2).

Technical Field

the invention relates to the field of train positioning, in particular to a BDS/INS combined train positioning method based on robust Kalman filtering.

Background

Since the 21 st century, the informatization of a railway signal system, particularly a train operation control system plays a crucial role in improving the railway transportation efficiency, reducing the operation and maintenance cost and guaranteeing the railway transportation safety, and the autonomous positioning of a train based on a global satellite navigation system is an important part of the informatization of the train operation control system. At present, the BDS can provide a navigation Positioning service with complete functions for Asia-Pacific regions, and after being comprehensively built in 2020, the BDS can provide accurate Positioning, navigation and time service (PNT) covering the world.

the Inertial Navigation System (INS) becomes an important component for realizing the combined positioning of the BDS/INS along with the rapid development of inertial devices and the complementary characteristics of the BDS system. Based on the requirements of future development of railways in China on autonomous positioning of trains and the broad prospect of railway application of the Beidou satellite navigation system and the continuous improvement of the new technical levels of an inertial navigation technology, an embedded electronic system technology and the like, the method has important practical significance and application value for researching a feasible and efficient railway station train autonomous positioning method, and provides key technical support for all-weather continuous autonomous positioning of trains and development of existing line and even high-speed railway train positioning technologies. The main stream train positioning method in the traditional train control system mainly comprises three methods: firstly, a positioning method based on a speed sensor (including a wheel axle sensor, a Doppler radar, a speed measuring motor and the like) is adopted to calculate the real-time position of the train in an integral mode, secondly, ground point type response equipment is used for updating the current position of the train, and thirdly, a track circuit is used for detecting the interval occupation of the train. The traditional positioning methods have defects, rely on trackside ground equipment, have high construction and maintenance costs, or are based on motion estimation, require a train to be in a motion state and have initialization information and error correction, and cannot completely realize train autonomous positioning. The domestic scholars and organizations have accumulated a certain amount of data source combination mode, combination positioning method, map matching algorithm and digital track map construction in the autonomous train positioning, the current navigation positioning technology is rapidly developed, and the research of reliable and efficient autonomous train positioning system still has huge development space from the requirement of the current railway application field for autonomous train positioning.

the Inertial Navigation System (INS) is widely applied with the advantages of all-weather work, difficulty in being interfered by external environment and the like, a Micro Electro Mechanical System (MEMS) becomes a preferred device of the inertial navigation system in recent years due to the advantages of low cost, small volume, easiness in integration, low power consumption and the like, an Inertial Measurement Unit (IMU) based on the MEMS can measure three-axis gyro information and acceleration information of carrier motion, the carrier attitude can be obtained through attitude calculation, and the inertial navigation is realized. However, the gyroscope has the temperature drift characteristic, the drift is serious after long-time operation, and the integral operation can generate accumulated errors; accelerometers are susceptible to carrier vibration and acceleration of motion. When the sensors work independently, the sensors can cause large deviation due to respective limitations and cannot be used for measuring postures independently, and after the sensors are fused, the problems of much external interference and poor reliability and precision exist.

disclosure of Invention

The invention aims to provide a BDS/INS combined train positioning method based on robust Kalman filtering to achieve the advantages of improving positioning reliability and precision when various sensors are integrated.

In order to achieve the purpose, the embodiment of the invention adopts the technical scheme that:

a BDS/INS combined train positioning method based on robust Kalman filtering comprises the following steps:

Detecting a zero-speed scene of train operation to obtain a constraint condition corresponding to the zero-speed scene;

acquiring an error model based on the zero-speed scene and a corresponding constraint condition;

Selecting a corresponding robust Kalman filter based on the zero-speed scene;

Correcting and resolving the obtained error observation information of the error model by a selected robust Kalman filter to obtain corrected navigation data,

and positioning the train based on the navigation data.

As a specific implementation manner of the embodiment of the present invention, the zero-speed scene includes:

First zero-speed scenario: the train is stationary relative to the ground;

Second zero-speed scenario: the train is in a motion state, but the speed component in the direction vertical to the ground and the motion direction vertical to the track is 0 based on the carrier coordinate system;

Or

The third zero-speed scenario: the train makes uniform linear motion relative to the ground.

as a specific implementation manner of the embodiment of the present invention, the detecting a zero-speed scene of train operation includes:

detecting a second zero-speed scene based on the condition that the train is in a zero-speed state within a set time period;

and detecting the first zero-speed scene or the third zero-speed scene based on the speed of the detected train.

As a specific implementation manner of the embodiment of the present invention, the detecting a second stall scenario based on a condition that the train is in a stall state within a set time period includes:

Detecting train presence (t)₀,t_s) Whether the time is in the zero speed state meets the following conditions:

f (t) is g, ω (t) is 0, where t is₀and t_srepresenting time, f (t) is a specific force value of the train at the time t, omega (t) is an angular velocity value of the train at the time t, and g is gravity acceleration;

And if the condition is not met, the scene is a second zero-speed scene.

As a specific implementation manner of the embodiment of the present invention, the detecting the first zero speed scene or the third zero speed scene based on the speed of the train includes:

Acquiring the output speed of a wheel speed sensor;

If the output speed is 0, the scene is a first zero speed scene;

And if the output speed is not 0, determining that the scene is a third zero-speed scene.

as a specific implementation manner of the embodiment of the present invention, the error equation includes:

An attitude error equation, a velocity error equation, or a position error equation.

as a specific implementation manner of the embodiment of the present invention, the attitude error equation is:

wherein the content of the first and second substances,Is the derivative vector of the attitude error;The projection of the rotational angular velocity of the earth in an n system is obtained;Is the projection of the rotation angular velocity of n relative to e in n;are respectively asAn error of (2);Is an attitude angle vector; epsilonⁿIs the equivalent gyro drift of the n system.

As a specific implementation manner of the embodiment of the present invention, the speed error equation is:

wherein the content of the first and second substances,a vector of derivatives of the velocity error; f. ofⁿIs the projection of specific force in n system; vⁿis a velocity vector; delta Vⁿa velocity error vector;is an attitude angle vector;the projection of the rotational angular velocity of the earth in an n system is obtained;is the projection of the rotation angular velocity of n relative to e in n;Are respectively asAn error of (2);for accelerometer drift, VⁿRepresenting speed, δ Vⁿto representVⁿthe error of (2).

as a specific implementation manner of the embodiment of the present invention, the position error equation is:

wherein L, lambda and h are respectively the longitude, the latitude and the height of the train; δ L, δ λ, δ h are longitude, latitude and altitude errors;derivatives of δ L, δ λ, δ h, respectively; r_M R_Nrespectively, the radius of curvature, V, along the unit circle of the earth's fourth quadrant and the meridian circle_E、V_N、V_Uthe speeds in the three directions of the northeast and the northeast in the northeast coordinate system are respectively.

As a specific implementation manner of the embodiment of the present invention, the robust kalman filter is:

Real-time adjustment of observation error vector variance by adopting robust estimation on the basis of standard Kalman filteringthe size of (2).

the embodiment of the invention has the following beneficial effects:

According to the embodiment of the invention, the corresponding error model and the filter are obtained according to the detected zero-speed scene of train operation, and then the navigation data is corrected after the error observation information of the error model is processed by the corresponding filter, so that the proportion of the observed value of each sensor is adjusted, the influence of the gross error observation value on the estimation result is effectively reduced, and the accuracy and the reliability of train positioning are improved.

The technical solution of the present invention is further described in detail by the accompanying drawings and embodiments.

Drawings

FIG. 1 is a flow chart of a BDS/INS combined train positioning method based on robust Kalman filtering according to an embodiment of the present invention;

FIG. 2 is a schematic diagram of a hardware structure used in the method for positioning a BDS/INS combined train based on the robust Kalman filtering according to the embodiment of the present invention;

FIG. 3 is a flow chart of the zero-speed scene detection according to the embodiment of the present invention;

Fig. 4 is a flowchart of a simulation experiment according to an embodiment of the present invention.

Detailed Description

the preferred embodiments of the present invention will be described in conjunction with the accompanying drawings, and it will be understood that they are described herein for the purpose of illustration and explanation and not limitation.

as shown in fig. 1, a BDS/INS combined train positioning method based on robust kalman filtering includes:

s101: detecting a zero-speed scene of train operation to obtain a constraint condition corresponding to the zero-speed scene;

in a specific application scene, a positioning system adopted by the positioning method comprises a satellite navigation system, such as a BDS Beidou satellite navigation system, an IMU inertial measurement unit and an ODO speed sensor;

when the zero-speed scene of train operation is detected, the zero-speed scene of train operation is detected through the output information of the IMU inertial measurement unit and the ODO speed sensor and by combining the constraint condition of the zero-speed scene.

As shown in fig. 2, the system mainly comprises an IMU inertial navigation sensor (3DM-AHRS300A attitude reference system), a BDS Beidou satellite receiver, an ODO velocity sensor and a PC terminal; the software part mainly comprises data acquisition software and algorithm editing software (MATLAB). The accelerometer and the gyroscope are integrated in a 3DM-AHRS300A attitude and heading reference system.

S102: acquiring an error model based on the zero-speed scene and a corresponding constraint condition;

s103: selecting a corresponding robust Kalman filter based on the zero-speed scene;

s104: correcting and resolving the obtained error observation information of the error model by a selected robust Kalman filter to obtain corrected navigation data,

S105: and positioning the train based on the navigation data.

as an optional implementation manner of the embodiment of the present invention, the zero-speed scenario includes:

First zero-speed scenario: the train is stationary relative to the ground;

Second zero-speed scenario: the train is in a motion state, but the speed component in the direction vertical to the ground and the motion direction vertical to the track is 0 based on the carrier coordinate system;

or

the third zero-speed scenario: the train makes uniform linear motion relative to the ground.

In a particular application scenario, the user may,

the whole train advancing process is divided into three zero-speed scenes according to the train running states (static, uniform linear motion and variable speed motion):

(1) the first zero speed scenario (the train is stationary relative to the ground) has the following constraints:

Wherein, V_E、V_N、V_Uthe speeds in the three directions of the northeast are respectively.

(2) In the second zero-speed scenario (the train is in a moving state, but the speed component in the direction perpendicular to the ground and the direction perpendicular to the track movement is 0 based on the carrier coordinate system), the constraint conditions are as follows:

wherein, V_by、V_bzThe velocity components in the direction perpendicular to the ground and perpendicular to the direction of orbital motion, respectively.

(3) a third zero speed scene (the third zero speed scene is a special case of the second zero speed scene, and the train makes uniform linear motion relative to the ground), and the constraint conditions are as follows:

Wherein the content of the first and second substances,The speed derivatives of the train along the Y axis of the driving direction and the speed derivatives of the carrier system perpendicular to the X axis and the Z axis of the driving direction are respectively based on the carrier coordinate system.

as an optional implementation manner of the embodiment of the present invention, the detecting a zero-speed scene of train operation includes:

Detecting a second zero-speed scene based on the condition that the train is in a zero-speed state within a set time period;

and detecting the first zero-speed scene or the third zero-speed scene based on the speed of the detected train.

as a specific implementation manner of the embodiment of the present invention, the detecting a second stall scenario based on a condition that the train is in a stall state within a set time period includes:

Detecting train presence (t)₀,t_s) Whether the time is in the zero speed state meets the following conditions:

f (t) is g, ω (t) is 0, where t is₀and t_srepresenting time, f (t) is a specific force value of the train at the time t, omega (t) is an angular velocity value of the train at the time t, and g is gravity acceleration;

And if the condition is not met, the scene is a second zero-speed scene.

as an optional implementation manner of the embodiment of the present invention, the detecting the first zero speed scene or the third zero speed scene based on the speed of the train includes:

acquiring the output speed of a wheel speed sensor;

if the output speed is 0, the scene is a first zero speed scene;

And if the output speed is not 0, determining that the scene is a third zero-speed scene.

in a specific application scenario, as shown in figure 3,

For the first and third stall scenarios, the train is at (t)₀,t_s) The condition of zero speed state is as follows:

f(t)＝g,ω(t)＝0，

the second stall scene can be distinguished from the three stall scenes according to the above conditions, but the first stall scene and the third stall scene cannot be distinguished according to the above conditions, so the output of the wheel speed sensor ODO is introduced as the detected switching value, and if the output speed is 0, the first stall scene is determined, and if the output speed is not 0, the third stall scene is determined.

As an optional implementation manner of the embodiment of the present invention, the error equation includes:

An attitude error equation, a velocity error equation, or a position error equation.

as an optional implementation manner of the embodiment of the present invention, the attitude error equation is:

Wherein the content of the first and second substances,is the derivative vector of the attitude error;The projection of the rotational angular velocity of the earth in an n system is obtained;is the projection of the rotation angular velocity of n relative to e in n;Are respectively asAn error of (2);is an attitude angle vector; epsilonⁿis the equivalent gyro drift of the n system.

as an optional implementation manner of the embodiment of the present invention, the speed error equation is:

Wherein the content of the first and second substances,a vector of derivatives of the velocity error; f. ofⁿis the projection of specific force in n system; vⁿis a velocity vector; delta VⁿA velocity error vector;is an attitude angle vector;The projection of the rotational angular velocity of the earth in an n system is obtained;Is the projection of the rotation angular velocity of n relative to e in n;are respectively asan error of (2);For accelerometer drift, VⁿRepresenting speed, δ Vⁿrepresents Vⁿthe error of (2).

As an optional implementation manner of the embodiment of the present invention, the position error equation is:

wherein L, lambda and h are respectively the longitude, the latitude and the height of the train; δ L, δ λ, δ h are longitude, latitude and altitude errors;Derivatives of δ L, δ λ, δ h, respectively; r_M R_NRespectively, the radius of curvature, V, along the unit circle of the earth's fourth quadrant and the meridian circle_E、V_N、V_Uthe speeds in the three directions of the northeast and the northeast in the northeast coordinate system are respectively.

as an optional implementation manner of the embodiment of the present invention, the robust kalman filter is:

Real-time adjustment of observation error vector variance by adopting robust estimation on the basis of standard Kalman filteringThe size of (2).

in a particular application scenario, the user may,

The Kalman filter is an anti-difference Kalman filter, and the specific algorithm is as follows:

the stochastic system state space model is:

In the formula phi_k/k-1Is a state transition matrix from the time k-1 to the time k; x_kIs a state vector; z_kIs an observation vector; w_kis the systematic error vector; v_kis an observation error vector; h_kIs a coefficient matrix; w_k，V_kthe following properties are satisfied:

E[W_k]＝0；E[V_k]＝0；

Cov[W_k,W_j]＝0，k≠j；Cov[V_k,V_j]＝0，k≠j；Cov[W_k,V_j]＝0，

The classical kalman filter algorithm is described as:

Wherein the content of the first and second substances,Respectively are state vectors at the moment k and corresponding posterior variance matrixes;respectively a state one-step predicted value at the k moment and a corresponding covariance matrix; k_kThe filter gain at time k.

In order to restrain the influence of observation errors, the method adopts the robust estimation principle to adjust in real time on the basis of standard Kalman filteringThe estimated value of the robust kalman filter at the k-th time may be expressed as:

wherein the content of the first and second substances,is a matrix containing equivalent observation weightsis expressed as:

observed value Z_kis equivalent weight matrix ofA Huber weight function may be employed:

wherein, P_iis an equivalence weight element; k is a constant, typically 2.5-3.0; v_iis Z_kNormalized residual of (2); c is a constant, typically taken to be 1.5.

the system state vector is:

The process noise vector is:

W_k＝[w_ωx w_ωy w_ωz w_fx w_fy w_fz]^T，

Wherein, w_ωx、w_ωy、w_ωz、w_fx、w_fy、w_fzthe noise of the gyroscope and the accelerometer under the carrier system can be regarded as white gaussian noise with zero mean value.

For the first zero-speed scenario, there are:

The observation matrix is:

H＝[0³I³ 0³ 0³ 0³]，

Wherein I is an identity matrix.

For the second zero-speed scenario, there are:

due to the fact thatfully differentiating it to obtain:

is provided with[Vⁿ]_×is Vⁿthe above formula is:

the observation matrix is obtained as:

H＝[0^2×3 FΜ₁ FΜ₂ 0^2×3 0^2×3]，

For the third zero-velocity scenario, there are

wherein, V_bxFor the speed component of the train in the direction of travel。

The observation matrix is obtained by the same method:

H＝[0³ FΜ₁ FΜ₂ 0³ 0³]。

The embodiment of the invention adopts a zero-speed correction technology, can independently correct errors without depending on external equipment or being interfered by external signals, and further inhibits the divergence of system speed and attitude calculation errors;

when the embodiment of the invention is used for zero-speed detection, the ODO speed sensor is used for assisting in detecting the zero-speed scene of the train, so that the method is more convenient and faster, and the calculated amount of the algorithm is reduced;

The embodiment of the invention adopts the robust estimation fusion Kalman filtering algorithm, can identify abnormal observed values, adaptively adjusts the proportion of each sensor observed value by utilizing an equivalent weight function, and effectively reduces the influence of gross error observed values on an estimation result so as to improve the accuracy and the reliability of train positioning. The flow chart design of the simulation experiment is shown in FIG. 4.

Finally, it should be noted that: although the present invention has been described in detail with reference to the foregoing embodiments, it will be apparent to those skilled in the art that changes may be made in the embodiments and/or equivalents thereof without departing from the spirit and scope of the invention. Any modification, equivalent replacement, or improvement made within the spirit and principle of the present invention should be included in the protection scope of the present invention.