CN111076718B - Autonomous navigation positioning method for subway train - Google Patents

Autonomous navigation positioning method for subway train Download PDF

Info

Publication number
CN111076718B
CN111076718B CN201911315384.5A CN201911315384A CN111076718B CN 111076718 B CN111076718 B CN 111076718B CN 201911315384 A CN201911315384 A CN 201911315384A CN 111076718 B CN111076718 B CN 111076718B
Authority
CN
China
Prior art keywords
speed
zero
train
attitude
matrix
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201911315384.5A
Other languages
Chinese (zh)
Other versions
CN111076718A (en
Inventor
林云志
何小飞
牟玉涛
周砚
南非
白子杰
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Aerospace Times Electronic Technology Co ltd
Beijing Aerospace Times Optical Electronic Technology Co Ltd
China Railway Electrification Engineering Group Co Ltd
Original Assignee
Aerospace Times Electronic Technology Co ltd
Beijing Aerospace Times Optical Electronic Technology Co Ltd
China Railway Electrification Engineering Group Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Aerospace Times Electronic Technology Co ltd, Beijing Aerospace Times Optical Electronic Technology Co Ltd, China Railway Electrification Engineering Group Co Ltd filed Critical Aerospace Times Electronic Technology Co ltd
Priority to CN201911315384.5A priority Critical patent/CN111076718B/en
Publication of CN111076718A publication Critical patent/CN111076718A/en
Application granted granted Critical
Publication of CN111076718B publication Critical patent/CN111076718B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Manufacturing & Machinery (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)

Abstract

The invention relates to an autonomous navigation positioning method of a subway train, which belongs to the technical field of navigation and solves the autonomous positioning problem of the subway train; carrying out inertia calculation according to the angular velocity and acceleration data measured by the inertia assembly; detecting whether the train is at zero speed; if yes, entering a zero-speed correction state to correct the attitude of the inertia assembly; and if not, entering a motion constraint state, and carrying out filtering correction on the inertia resolving data. The invention adopts the inertial navigation technology to enhance the autonomous navigation capability of the subway train, eliminates the historical accumulated error in the stop stage of the subway train through the zero-speed correction technology, and adopts the train motion constraint technology in the running process of the train to keep the train to have higher positioning precision in the running stage.

Description

Autonomous navigation positioning method for subway train
Technical Field
The invention relates to the technical field of navigation, in particular to an autonomous navigation positioning method for a subway train.
Background
The operation of the subway train needs to obtain self accurate position information and speed information for controlling the train operation. In the past, an autonomous navigation positioning mode of a speed sensor and a transponder is adopted, but the cost is high and the compatibility is poor.
The subway train can not receive satellite navigation signals, only can use autonomous navigation equipment, and the inertial navigation system has the autonomous navigation characteristic, conforms to a navigation mode without information interaction, does not need other auxiliary equipment, has no compatibility problem, and is suitable for autonomous navigation positioning of the subway train. However, the inertial navigation device has the problem of poor positioning accuracy due to long-time accumulated positioning errors, and needs to be solved in the subway application environment.
Disclosure of Invention
In view of the above analysis, the present invention aims to provide an autonomous navigation positioning method for a subway train, which solves the autonomous positioning problem of the subway train, eliminates the accumulation of positioning errors, and improves the positioning accuracy of autonomous navigation.
The purpose of the invention is mainly realized by the following technical scheme:
the invention discloses an autonomous navigation positioning method of a subway train, which comprises the following steps:
acquiring initial attitude, speed and position information of an inertia assembly installed on a train;
carrying out inertia calculation according to the angular velocity and acceleration data measured by the inertia assembly;
detecting whether the train is at zero speed; if yes, entering a zero-speed correction state to correct the attitude of the inertia assembly; and if not, entering a motion constraint state, and carrying out filtering correction on the inertia resolving data.
Further, the zero speed determination condition is: the change rate of the acceleration of the train is smaller than a set acceleration threshold value, and meanwhile, the change rate of the displacement of the train is smaller than a set displacement threshold value.
Further, the acceleration change rate of the train
Figure BDA0002324225780000021
In the formula (I), the compound is shown in the specification,
Figure BDA0002324225780000022
acceleration f measured for inertial component in set time rangei,jA running average of (d);
rate of change of displacement of the train
Figure BDA0002324225780000023
In the formula (I), the compound is shown in the specification,
Figure BDA0002324225780000024
velocity V calculated for inertia in a set time rangei,jA running average of (d); k is the measuring time, and m and n are the stacking times; x, y, z are the three axes of the navigational coordinate system.
Further, entering a zero-speed correction state, and correcting the attitude of the inertial component, specifically including:
1) detecting the speed increment and judging whether the zero speed state is effective or not;
2) establishing a filter for filtering the acceleration measured in the zero-speed state;
3) solving an attitude matrix for zero-speed correction according to the filtered acceleration;
4) and setting the speed to zero, and updating the attitude matrix subjected to inertial solution into the attitude matrix.
Further, the speed increment Δk,y＝Vk,y-Vk-1,y，Vk,yFor the y-axis speed of the train at time k, when the speed increases by Δk,yWhen the speed increment is smaller than the speed increment threshold, the zero-speed state is valid; otherwise, the zero speed state is invalid, and the speed value V of the current time point is updatedk,y＝Δk,y+Vk-1,yAnd entering a motion constraint state.
Further, the filter is
Figure BDA0002324225780000025
a1,a2,a3,a4,b1,b2,b3Is the set filtering parameter.
Further, the solving the attitude matrix for the zero-speed correction includes:
1) orthogonalizing the filtered acceleration to obtain
Figure BDA0002324225780000031
2) Carrying out coordinate conversion; obtaining the acceleration under a zero-speed carrier system
Figure BDA0002324225780000032
Figure BDA0002324225780000033
In the formula (I), the compound is shown in the specification,
Figure BDA0002324225780000034
for the transition matrix from zero-speed carrier system to carrier system at time k, from
Figure BDA0002324225780000035
Is obtained by calculation, and
Figure BDA0002324225780000036
three-axis angular velocity data under a carrier coordinate system;
3) calculating attitude angle under zero-speed load system
Figure BDA0002324225780000037
Attitude angle
Figure BDA0002324225780000038
In the formula (I), the compound is shown in the specification,
Figure BDA0002324225780000039
Figure BDA00023242257800000310
l is local latitude, omegaieThe rotational angular velocity of the earth is shown, and k is the calculation time; where 0 is the zero time, b0The representation is data under a zero-speed carrier system, n is data under a navigation coordinate system, n0Navigating data under a coordinate system at zero speed;
4) calculating attitude matrix at zero speed
Figure BDA00023242257800000311
The attitude matrix
Figure BDA00023242257800000312
In the formula (I), the compound is shown in the specification,
Figure BDA00023242257800000313
is a conversion matrix from a zero-speed carrier system to a carrier system.
Further, the entering of the motion constraint state performs filtering correction on the inertia calculation data, and includes:
1) establishing an error model form
Figure BDA00023242257800000314
X (t) is a state variable, a (t) is a state transition matrix, w (t) is system noise, y (t) is measurement observed quantity, h (t) is a measurement matrix, and v (t) is measurement noise;
2) iterative filtering is carried out by adopting a kalman device;
3) and correcting inertial data including the attitude matrix, the installation angle and the speed by using the filtered estimation parameters.
Further, the
Figure BDA0002324225780000041
In the formula, phix,φy,φzIs the attitude error, δ Vx,δVy,δVzIs the error in the speed of the vehicle,
Figure BDA0002324225780000042
is the mounting angle error;
the measurement observed quantity
Figure BDA0002324225780000043
Further, a measurement matrix
Figure BDA0002324225780000044
State transition matrix
Figure BDA0002324225780000045
Figure BDA0002324225780000046
fbAs an acceleration output
Figure BDA0002324225780000047
For a transformation matrix from the carrier coordinate system to the navigation coordinate system, [ phi ] - [ phi ]x,φy,φz]。
The invention has the following beneficial effects:
the invention adopts the inertial navigation equipment to enhance the autonomous navigation capability of the subway train, eliminates the historical accumulated error in the stop stage of the subway train through the zero-speed correction technology, and adopts the train motion constraint technology in the running process of the train to keep the train to have higher positioning precision in the running stage.
Drawings
The drawings are only for purposes of illustrating particular embodiments and are not to be construed as limiting the invention, wherein like reference numerals are used to designate like parts throughout.
FIG. 1 is a flowchart of an autonomous navigation positioning method according to an embodiment of the present invention;
FIG. 2 is a schematic diagram of the forward speed of experiment one in the embodiment of the present invention;
FIG. 3 is a graph illustrating the mileage before the first experiment in the embodiment of the present invention;
FIG. 4 is a diagram illustrating the forward speed of experiment two in the embodiment of the present invention;
FIG. 5 is a graph illustrating the forward mileage of experiment two in the example of the present invention.
Detailed Description
The preferred embodiments of the present invention will now be described in detail with reference to the accompanying drawings, which form a part hereof, and which together with the embodiments of the invention serve to explain the principles of the invention.
The embodiment discloses an autonomous navigation positioning method for a subway train, which comprises the following steps as shown in fig. 1:
step S1, acquiring initial attitude, speed and position information of an inertia assembly installed on the train;
acquiring initial attitude, speed and position information of the inertia assembly according to position information acquired from the outside;
specifically, the position information acquired from the outside can be acquired by identifying and positioning the two-dimensional code of the set position coordinate arranged beside the subway rail through a binocular camera installed on the train before the subway train is delivered from the warehouse.
The acquisition of the initial attitude, speed and position information of the inertia assembly installed on the train can also be completed by other methods capable of accurately measuring the initial position information of the subway train before the train is taken out of the warehouse.
Step S2, carrying out inertia calculation according to the angular velocity and acceleration data measured by the inertia assembly of the train;
the inertia assembly comprises a triaxial gyroscope and a triaxial accelerometer, after initial attitude, speed and position information is obtained, triaxial angular speed and acceleration information in the operation process of the subway train are continuously measured, inertial calculation is carried out through inertial navigation mechanical arrangement, and navigation positioning data including speed, position and attitude data in the operation process of the subway train are obtained.
Step S3, detecting whether the train is at zero speed; if yes, entering a zero-speed correction state to correct the attitude of the inertia assembly; and if not, entering a motion constraint state, and carrying out filtering correction on the inertia resolving data.
In the running process of a subway train, zero speed correction can be implemented only by effectively detecting that the speed of the train is zero, the previous zero speed detection utilizes the movement of a data detection carrier of an inertial instrument, so that misjudgment is easily caused, and especially under the disturbance conditions of getting on and off a train and the like, a displacement calculation method is adopted to eliminate instrument disturbance caused by shaking.
Specifically, the zero speed determination condition in this embodiment is: the change rate of the acceleration of the train is smaller than a set acceleration threshold value, and meanwhile, the change rate of the displacement of the train is smaller than a set displacement threshold value.
Further, the acceleration change rate of the train
Figure BDA0002324225780000061
In the formula (I), the compound is shown in the specification,
Figure BDA0002324225780000062
acceleration f measured for inertial component within set time frame (e.g. 1S)i,jA running average of (d);
rate of change of displacement of the train
Figure BDA0002324225780000063
In the formula (I), the compound is shown in the specification,
Figure BDA0002324225780000064
for inertia resolving speed V within a set time frame (e.g. 1S)i,jA running average of (d); k is the measuring time, and m and n are the stacking times; x, y, z are the three axes of the navigational coordinate system.
Preferably, the acceleration threshold is 0.05 ± 0.02 mg; the displacement threshold is 5 +/-1 m.
At the same time satisfying two conditions for determining said zero speed, i.e. deltaf,k,j＜0.05±0.02mg；Δv,k,jIf the speed is less than 5 +/-1 m, the speed is regarded as zero speed, otherwise, the speed is regarded as non-zero speed.
If the zero speed condition is detected, carrying out zero speed correction by adopting a zero speed correction algorithm.
The subway train has short stopping time which is generally not more than 1min, and the attitude correction error is quickly obtained by adopting a least square method.
Specifically, the zero-speed correction algorithm comprises the following steps:
1) detecting the speed increment and judging whether the zero speed state is effective or not;
in order to improve the reliability of zero speed detection, after the zero speed is judged according to the conditions, the speed increment is further detected, and due to the characteristic that the subway train travels on the track, the track direction is taken as the y axis of a coordinate system, so that the train can be considered to have speed change only on the y axis, and can be considered to have 0 speed on the x axis and the z axis. I.e. the speed increase deltak,y＝Vk,y-Vk-1,y，Vk,yFor the y-axis speed of the train at time k, when the speed increases by Δk,yLess than a speed increment threshold (delta)k,yLess than 0.2m/s), the zero speed state is valid; otherwise, the zero speed state is invalid, and the speed value V of the current time point is updatedk,y＝Δk,y+Vk-1,yAnd entering a motion constraint state.
2) Establishing a filter for filtering the acceleration measured in the zero-speed state;
the filter is
Figure BDA0002324225780000071
a1,a2,a3,a4,b1,b2,b3Is the set filtering parameter.
From the results of fitting the filter parameters, at a1＝0.00000375683802，,a2＝0.000011270514059,a3＝0.000011270514059,a4＝0.00000375683802，b1＝-2.93717072844989,b2＝2.87629972347933,b3When the filter is-0.939098940325283, a superior filtering effect can be obtained.
3) Solving an attitude matrix for zero-speed correction according to the filtered acceleration;
the solving of the zero-speed modified attitude matrix comprises:
a. orthogonalizing the filtered acceleration to obtain
Figure BDA0002324225780000072
b. Carrying out coordinate conversion; acceleration under coordinate system of carrier
Figure BDA0002324225780000081
Converted to obtain the acceleration under the zero-speed carrier system
Figure BDA0002324225780000082
Figure BDA0002324225780000083
In the formula (I), the compound is shown in the specification,
Figure BDA0002324225780000084
for the transition matrix from zero-speed carrier system to carrier system at time k, from
Figure BDA0002324225780000085
Is obtained by calculation, and
Figure BDA0002324225780000086
three-axis angular velocity data under a carrier coordinate system;
c. calculating attitude angle under zero-speed load system
Figure BDA0002324225780000087
Attitude angle
Figure BDA0002324225780000088
In the formula (I), the compound is shown in the specification,
Figure BDA0002324225780000089
Figure BDA00023242257800000810
l is local latitude, omegaieThe rotational angular velocity of the earth is shown, and k is the calculation time; where 0 is the zero time, b0The representation is data under a zero-speed carrier system, n is data under a navigation coordinate system, n0And navigating the data under the coordinate system for zero speed.
d. Calculating attitude matrix at zero speed
Figure BDA00023242257800000811
The attitude matrix
Figure BDA00023242257800000812
In the formula (I), the compound is shown in the specification,
Figure BDA00023242257800000813
transformation matrix from navigation coordinate system to zero-speed navigation coordinate system
Figure BDA00023242257800000814
4) Zeroing the velocity and solving the inertia of the attitude matrix
Figure BDA00023242257800000815
Updating to the attitude matrix
Figure BDA00023242257800000816
If the detection result is the non-zero speed, a filtering correction method of a motion constraint state is adopted to solve the problem of positioning error drift of the subway train in the running process, and by means of the running characteristics of the train, the train only has the constraint that the forward speed, the lateral speed and the vertical speed are zero, and the filtering correction is carried out on the autonomous navigation algorithm, so that the dynamic positioning precision is improved.
Specifically, the filtering correction method for the motion constraint state includes:
1) establishing an error model form
Figure BDA00023242257800000817
X (t) is a state variable, a (t) is a state transition matrix, w (t) is system noise, y (t) is measurement observed quantity, h (t) is a measurement matrix, and v (t) is measurement noise;
specifically, the
Figure BDA0002324225780000091
In the formula, phix,φy,φzIs the attitude error, δ Vx,δVy,δVzIs the error in the speed of the vehicle,
Figure BDA0002324225780000092
is the mounting angle error;
the measurement observed quantity
Figure BDA0002324225780000093
The measurement matrix
Figure BDA0002324225780000094
State transition matrix
Figure BDA0002324225780000095
Figure BDA0002324225780000096
fbAs an acceleration output
Figure BDA0002324225780000097
For a transformation matrix from the carrier coordinate system to the navigation coordinate system, [ phi ] - [ phi ]x,φy,φz]。
System noise
Figure BDA0002324225780000098
Measuring noise
Figure BDA0002324225780000099
2) Iterative filtering is carried out by adopting a kalman device;
in this embodiment, a known kalman filter may be used to perform iterative filtering on the state variable according to the measurement observation, and any iterative filtering algorithm that can implement this process does not affect the protection scope of the present invention.
3) And correcting inertial data including the attitude matrix, the installation angle and the speed by adopting the filtered estimation parameters so as to improve the accuracy of the position and speed data of navigation positioning output by inertial calculation.
Specifically, the attitude matrix is corrected:
Figure BDA0002324225780000101
and (3) correcting the installation angle:
Figure BDA0002324225780000102
and (3) speed correction: v + δ V;
by adopting the autonomous navigation positioning method for the subway of the embodiment, the position information is calculated by recursion according to the corrected speed information in real subway environment measurement, and the first test result is shown in fig. 2, 3 and table 1, and the second test result is shown in fig. 4, 5 and table 2.
TABLE 1 Experimental one-mile error test results
Figure BDA0002324225780000103
Table 2 experimental two-mile error test results
Up run (Standard) Down (standard) Uplink (test) Go down (test) Up run (error) Go down (error)
Between stations 1 1491.252 1485.180 1490.23 1486.73 -1.022 1.55
Between stations 2 1394.5 1394.472 1393.54 1395.43 -0.96 0.96
Between stations 3 1775.116 1777.149 1774.15 1778.17 -0.97 1.02
In conclusion, compared with the prior art, the autonomous navigation capability of the subway train is enhanced by the inertial navigation device, the historical accumulated errors are eliminated in the stop stage of the subway train through the zero-speed correction technology, and the train motion constraint technology is adopted in the running process of the train, so that the train is kept to have higher positioning accuracy in the running stage.
The above description is only for the preferred embodiment of the present invention, but the scope of the present invention is not limited thereto, and any changes or substitutions that can be easily conceived by those skilled in the art within the technical scope of the present invention are included in the scope of the present invention.

Claims (8)

1. An autonomous navigation positioning method for a subway train is characterized by comprising
Acquiring initial attitude, speed and position information of an inertia assembly installed on a train;
carrying out inertia calculation according to the angular velocity and acceleration data measured by the inertia assembly;
detecting whether the train is at zero speed; if yes, entering a zero-speed correction state to correct the attitude of the inertia assembly; if not, entering a motion constraint state, and carrying out filtering correction on the inertia resolving data;
the entering of the motion constraint state carries out filtering correction on the inertia resolving data, and comprises the following steps:
1) establishing an error model form
Figure FDA0002677170020000011
X (t) is a state variable, a (t) is a state transition matrix, w (t) is system noise, y (t) is measurement observed quantity, h (t) is a measurement matrix, and v (t) is measurement noise;
2) iterative filtering is carried out by adopting a kalman device;
3) correcting inertial data including an attitude matrix, a mounting angle and a speed by using the filtered estimation parameters;
measuring matrix
Figure FDA0002677170020000012
State transition matrix
Figure FDA0002677170020000013
Figure FDA0002677170020000014
L is the local latitude, and L is the local latitude,
ωiethe rotational angular velocity of the earth;
fbas an acceleration output
Figure FDA0002677170020000015
Figure FDA0002677170020000016
For a transformation matrix from the carrier coordinate system to the navigation coordinate system, [ phi ] - [ phi ]x,φy,φz]，φx,φy,φzIs the attitude error.
2. The autonomous navigation positioning method according to claim 1, wherein the zero velocity is determined by: the change rate of the acceleration of the train is smaller than a set acceleration threshold value, and meanwhile, the change rate of the displacement of the train is smaller than a set displacement threshold value.
3. The autonomous navigational positioning method of claim 2,
acceleration rate of the train
Figure FDA0002677170020000021
In the formula (I), the compound is shown in the specification,
Figure FDA0002677170020000022
Figure FDA0002677170020000023
acceleration f measured for inertial component in set time rangei,jA running average of (d);
rate of change of displacement of the train
Figure FDA0002677170020000024
In the formula (I), the compound is shown in the specification,
Figure FDA0002677170020000025
Figure FDA0002677170020000026
velocity V calculated for inertia in a set time rangei,jA running average of (d); k is the measuring time, and m and n are the stacking times; x, y, z are the three axes of the navigational coordinate system.
4. The autonomous navigation positioning method according to claim 1, wherein entering a zero-velocity correction state to correct the attitude of the inertial component, specifically comprises:
1) detecting the speed increment and judging whether the zero speed state is effective or not;
2) establishing a filter for filtering the acceleration measured in the zero-speed state;
3) solving an attitude matrix for zero-speed correction according to the filtered acceleration;
4) and setting the speed to zero, and updating the attitude matrix subjected to inertial solution into the attitude matrix.
5. The autonomous navigational positioning method of claim 4,
the speed increment Δk,y＝Vk,y-Vk-1,y，Vk,yFor the y-axis speed of the train at time k, when the speed increases by Δk,yWhen the speed increment is smaller than the speed increment threshold, the zero-speed state is valid; otherwise, the zero speed state is invalid, and the speed value V of the current time point is updatedk,y＝Δk,y+Vk-1,yAnd entering a motion constraint state.
6. The autonomous navigational positioning method of claim 4,
the filter is
Figure FDA0002677170020000031
a1,a2,a3,a4,b1,b2,b3For filtering parameters of settingsAnd (4) counting.
7. The autonomous navigational positioning method of claim 1,
solving the attitude matrix for the zero-speed correction includes:
1) orthogonalizing the filtered acceleration to obtain
Figure FDA0002677170020000032
2) Carrying out coordinate conversion; obtaining the acceleration under a zero-speed carrier system
Figure FDA0002677170020000033
Figure FDA0002677170020000034
In the formula (I), the compound is shown in the specification,
Figure FDA0002677170020000035
for the transition matrix from zero-speed carrier system to carrier system at time k, from
Figure FDA0002677170020000036
Is obtained by calculation, and
Figure FDA0002677170020000037
Figure FDA0002677170020000038
three-axis angular velocity data under a carrier coordinate system;
3) calculating attitude angle under zero-speed load system
Figure FDA0002677170020000039
Attitude angle
Figure FDA00026771700200000310
In the formula (I), the compound is shown in the specification,
Figure FDA00026771700200000311
Figure FDA00026771700200000312
l is local latitude, omegaieThe rotational angular velocity of the earth is shown, and k is the calculation time; where 0 is the zero time, b0The representation is data under a zero-speed carrier system, n is data under a navigation coordinate system, n0Navigating data under a coordinate system at zero speed;
4) calculating attitude matrix at zero speed
Figure FDA00026771700200000313
The attitude matrix
Figure FDA00026771700200000314
In the formula (I), the compound is shown in the specification,
Figure FDA00026771700200000315
Figure FDA00026771700200000316
is a conversion matrix from a zero-speed carrier system to a carrier system.
8. The autonomous navigational positioning method of claim 7, wherein the autonomous navigational positioning method is further characterized by
Figure FDA0002677170020000041
In the formula, phix,φy,φzIs the attitude error, δ Vx,δVy,δVzIs the error in the speed of the vehicle,
Figure FDA0002677170020000042
is the mounting angle error;
the measurement observed quantity
Figure FDA0002677170020000043
CN201911315384.5A 2019-12-18 2019-12-18 Autonomous navigation positioning method for subway train Active CN111076718B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911315384.5A CN111076718B (en) 2019-12-18 2019-12-18 Autonomous navigation positioning method for subway train

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911315384.5A CN111076718B (en) 2019-12-18 2019-12-18 Autonomous navigation positioning method for subway train

Publications (2)

Publication Number Publication Date
CN111076718A CN111076718A (en) 2020-04-28
CN111076718B true CN111076718B (en) 2021-01-15

Family

ID=70316222

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911315384.5A Active CN111076718B (en) 2019-12-18 2019-12-18 Autonomous navigation positioning method for subway train

Country Status (1)

Country Link
CN (1) CN111076718B (en)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112248940B (en) * 2020-08-27 2022-03-29 清华大学 Sensor arrangement resolving method and system for vehicle body attitude control

Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101476894A (en) * 2009-02-01 2009-07-08 哈尔滨工业大学 Vehicle-mounted SINS/GPS combined navigation system performance reinforcement method
CN104599498A (en) * 2014-05-05 2015-05-06 腾讯科技（深圳）有限公司 Vehicle information acquiring method and device
CN105021192A (en) * 2015-07-30 2015-11-04 华南理工大学 Realization method of combined navigation system based on zero-speed correction
CN106525071A (en) * 2016-10-26 2017-03-22 上海与德信息技术有限公司 Method and device for monitoring motion state
CN106908759A (en) * 2017-01-23 2017-06-30 南京航空航天大学 A kind of indoor pedestrian navigation method based on UWB technology
CN107228664A (en) * 2017-05-02 2017-10-03 太原理工大学 Mining gyrolevel SINS attitude algorithm and zero speed correcting method
CN108055606A (en) * 2017-12-11 2018-05-18 联想(北京)有限公司 The information cuing method and electronic equipment of a kind of electronic equipment
CN109036445A (en) * 2017-06-12 2018-12-18 杭州萤石网络有限公司 The method of adjustment and moving camera of the source of sound yield value of microphone MIC sensor
CN109297485A (en) * 2018-08-24 2019-02-01 北京航空航天大学 A kind of personal inertial navigation height accuracy method for improving in interior based on height from observation algorithm
CN109855617A (en) * 2019-02-28 2019-06-07 深圳市元征科技股份有限公司 A kind of vehicle positioning method, vehicle locating device and terminal device
CN110057381A (en) * 2019-03-20 2019-07-26 深圳市元征科技股份有限公司 A kind of zero velocity update method and system of navigation system
CN110207704A (en) * 2019-05-21 2019-09-06 南京航空航天大学 A kind of pedestrian navigation method based on the identification of architectural stair scene intelligent

Patent Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101476894A (en) * 2009-02-01 2009-07-08 哈尔滨工业大学 Vehicle-mounted SINS/GPS combined navigation system performance reinforcement method
CN104599498A (en) * 2014-05-05 2015-05-06 腾讯科技（深圳）有限公司 Vehicle information acquiring method and device
CN105021192A (en) * 2015-07-30 2015-11-04 华南理工大学 Realization method of combined navigation system based on zero-speed correction
CN106525071A (en) * 2016-10-26 2017-03-22 上海与德信息技术有限公司 Method and device for monitoring motion state
CN106908759A (en) * 2017-01-23 2017-06-30 南京航空航天大学 A kind of indoor pedestrian navigation method based on UWB technology
CN107228664A (en) * 2017-05-02 2017-10-03 太原理工大学 Mining gyrolevel SINS attitude algorithm and zero speed correcting method
CN109036445A (en) * 2017-06-12 2018-12-18 杭州萤石网络有限公司 The method of adjustment and moving camera of the source of sound yield value of microphone MIC sensor
CN108055606A (en) * 2017-12-11 2018-05-18 联想(北京)有限公司 The information cuing method and electronic equipment of a kind of electronic equipment
CN109297485A (en) * 2018-08-24 2019-02-01 北京航空航天大学 A kind of personal inertial navigation height accuracy method for improving in interior based on height from observation algorithm
CN109855617A (en) * 2019-02-28 2019-06-07 深圳市元征科技股份有限公司 A kind of vehicle positioning method, vehicle locating device and terminal device
CN110057381A (en) * 2019-03-20 2019-07-26 深圳市元征科技股份有限公司 A kind of zero velocity update method and system of navigation system
CN110207704A (en) * 2019-05-21 2019-09-06 南京航空航天大学 A kind of pedestrian navigation method based on the identification of architectural stair scene intelligent

Also Published As

Publication number Publication date
CN111076718A (en) 2020-04-28

Similar Documents

Publication Publication Date Title
CN107588769B (en) Vehicle-mounted strapdown inertial navigation, odometer and altimeter integrated navigation method
CN111156994B (en) INS/DR & GNSS loose combination navigation method based on MEMS inertial component
CN102422166B (en) Movement condition detection device
CN109471144B (en) Multi-sensor tightly combined train combined positioning method based on pseudo range/pseudo range rate
US6292751B1 (en) Positioning refinement algorithm
CN107560612B (en) Method and device for determining the angular position of a vehicle
CN109186597B (en) Positioning method of indoor wheeled robot based on double MEMS-IMU
US20030216865A1 (en) Inertial navigation system for mobile objects with constraints
CN108931244B (en) Inertial navigation error suppression method and system based on train motion constraint
EP1530024A1 (en) Motion estimation method and system for mobile body
CN109974697A (en) A kind of high-precision mapping method based on inertia system
CN109870173A (en) A kind of track correct method of the submarine pipeline inertial navigation system based on checkpoint
JP2010019703A (en) Positioning device for mobile body
CN108051839B (en) Vehicle-mounted three-dimensional positioning device and three-dimensional positioning method
JP2009019992A (en) Position detection device and position detection method
CN108345021B (en) Doppler radar assisted GPS/INS vehicle speed measurement method
CN106153069B (en) Attitude rectification device and method in autonomous navigation system
CN107219542B (en) GNSS/ODO-based robot double-wheel differential positioning method
CN108195374A (en) For the integrated navigation system of track automatic measurement vehicle and integrated navigation calculation method
KR20110022953A (en) Apparatus and method for compenating angular velocity error for robot
CN105444764A (en) Attitude measurement method based on assistance of speedometer of vehicle
CN111220151B (en) Inertia and milemeter combined navigation method considering temperature model under load system
CN111076718B (en) Autonomous navigation positioning method for subway train
JP2001510890A (en) Method for detecting the rotational state of an autonomous mobile unit and autonomous mobile unit
CN110567456B (en) BDS/INS combined train positioning method based on robust Kalman filtering

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant