CN111076718A - 一种地铁列车的自主导航定位方法 - Google Patents

Abstract

本发明涉及一种地铁列车的自主导航定位方法，属于导航技术领域，解决地铁列车的自主定位问题，方法包括，获取安装于列车上的惯性组件的初始姿态、速度及位置信息；根据所述惯性组件测量的角速度及加速度数据进行惯性解算；检测列车是否处于零速；是，则进入零速修正状态，对所述惯性组件的姿态进行修正；否，则进入运动约束状态，对惯性解算数据进行滤波修正。本发明采用惯性导航技术增强了地铁列车自主导航能力，并通过零速修正技术，在地铁列车停站阶段消除历史累积误差，在列车运行过程中，采用列车运动约束技术，而保持列车在运行阶段有较高定位精度。

Description

一种地铁列车的自主导航定位方法

技术领域

本发明涉及导航技术领域，尤其是一种地铁列车的自主导航定位方法。

背景技术

地铁列车的运行需获得自身精确的位置信息及速度信息，用于对列车运行控制。以往采用速度传感器+应答器的自主导航定位方式，但成本较高、兼容性较差。

地铁列车由于无法接收卫星导航信号，只能借助自主导航设备，惯性导航***具有自主导航特点，符合无信息交互的导航模式，无需其他辅助设备，不存在兼容性问题，适用于地铁列车的自主导航定位。但惯性导航设备存在长时间定位误差累积，造成定位精度较差的问题，需要在地铁应用环境下解决。

发明内容

鉴于上述的分析，本发明旨在提供一种地铁列车的自主导航定位方法，解决地铁列车的自主定位问题，消除了定位误差累积，提高了自主导航的定位精度。

本发明的目的主要是通过以下技术方案实现的：

本发明公开了一种地铁列车的自主导航定位方法，包括：

获取安装于列车上的惯性组件的初始姿态、速度及位置信息；

根据所述惯性组件测量的角速度及加速度数据进行惯性解算；

检测列车是否处于零速；是，则进入零速修正状态，对所述惯性组件的姿态进行修正；否，则进入运动约束状态，对惯性解算数据进行滤波修正。

进一步地，所述零速的判断条件为：列车的加速度变化率小于设定的加速度阈值，同时列车的位移变化率小于设定的位移阈值。

进一步地，所述列车的加速度变化率

式中，

为在设定时间范围内惯性组件测量的加速度f_i,j的滑动平均；

所述列车的位移变化率

式中，

为在设定时间范围内惯性解算的速度V_i,j的滑动平均；k为测量时刻，m、n为叠加次数；x,y,z为导航坐标系的三个轴向。

进一步地，进入零速修正状态，对所述惯性组件的姿态进行修正，具体包括：

1)检测速度增量，判断零速状态是否有效；

2)建立滤波器，对零速状态下测量的加速度进行滤波；

3)根据滤波后的加速度求解用于零速修正的姿态矩阵；

4)将速度置零，并将惯性解算的姿态矩阵更新为所述姿态矩阵。

进一步地，所述速度增量Δ_k,y＝V_k,y-V_k-1,y，V_k,y为k时刻的列车的y轴速度，当速度增量Δ_k,y小于速度增量阈值时，则零速状态有效；否则，零速状态无效，更新当前时间点的速度值V_k,y＝Δ_k,y+V_k-1,y，进入运动约束状态。

进一步地，所述滤波器为

a₁,a₂,a₃,a₄,b₁,b₂,b₃为设定的滤波参数。

进一步地，所述求解用于零速修正的姿态矩阵包括：

1)将滤波后的加速度正交化，得到

2)进行坐标转换；得到零速载体系下的加速度

式中，

为k时刻的零速载体系到载体系的转换矩阵，由

计算获得，而

为载体坐标系下的三轴角速度数据；

3)计算零速载体系下姿态角

姿态角

式中，

L为当地纬度，ω_ie为地球自转角速度，k为计算时刻；其中0为零时刻，b₀代表为零速载体系下数据，n为导航坐标系下数据，n₀为零速导航坐标系下数据；

4)计算零速下的姿态矩阵

所述姿态矩阵

式中，

为零速载体系到载体系的转换矩阵。

进一步地，所述进入运动约束状态，对惯性解算数据进行滤波修正，包括：

1)建立误差模型形式

X(t)为状态变量，A(t)为状态转移矩阵，w(t)为***噪声，y(t)为量测观测量，H(t)为量测矩阵，v(t)为量测噪声；

2)采用kalman器进行迭代滤波；

3)采用滤波后的估计参数对包括姿态矩阵、安装角和速度在内的惯性数据进行修正。

进一步地，所述

式中，φ_x,φ_y,φ_z是姿态误差，δV_x,δV_y,δV_z是速度误差，

是安装角误差；

所述量测观测量

进一步地，量测矩阵

状态转移矩阵

f^b为加速度输出

为载体坐标系到导航坐标系的转换矩阵，φ＝[φ_x,φ_y,φ_z]。

本发明有益效果如下：

本发明采用惯性导航设备增强了地铁列车自主导航能力，并通过零速修正技术，在地铁列车停站阶段消除历史累积误差，在列车运行过程中，采用列车运动约束技术，而保持列车在运行阶段有较高定位精度。

附图说明

附图仅用于示出具体实施例的目的，而并不认为是对本发明的限制，在整个附图中，相同的参考符号表示相同的部件。

图1为本发明实施例中的自主导航定位方法流程图；

图2为本发明实施例中实验一的前行速度示意图；

图3为本发明实施例中实验一的前行里程示意图；

图4为本发明实施例中实验二的前行速度示意图；

图5为本发明实施例中实验二的前行里程示意图。

具体实施方式

下面结合附图来具体描述本发明的优选实施例，其中，附图构成本申请一部分，并与本发明的实施例一起用于阐释本发明的原理。

本实施例公开了一种地铁列车的自主导航定位方法，如图1所示，包括以下步骤：

步骤S1、获取安装于列车上的惯性组件的初始姿态、速度及位置信息；

根据从外部获取的位置信息，获得惯性组件的初始姿态、速度及位置信息；

具体的，所述外部获取的位置信息可在地铁列车出库前，通过安装在列车上的双目摄像头对布置在地铁轨旁的设定位置坐标的二维码进行识别定位获得。

也可以通过其他能够准确测量地铁列车出库前的初始位置信息的方法来完成对安装于列车上的惯性组件的初始姿态、速度及位置信息的获得。

步骤S2、根据列车的惯性组件测量的角速度及加速度数据进行惯性解算；

所述惯性组件包括三轴陀螺仪和三轴加速度计，在获得初始姿态、速度及位置信息后，持续测量地铁列车运行过程中的三轴角速度及加速度信息，通过惯导机械编排，进行惯性解算，得到地铁列车运行过程中包括速度、位置、姿态数据在内的导航定位数据。

步骤S3、检测列车是否处于零速；是，则进入零速修正状态，对所述惯性组件的姿态进行修正；否，则进入运动约束状态，对惯性解算数据进行滤波修正。

在地铁列车运行过程中，有效检测列车的速度为零才能实施零速修正，以往零速检测利用惯性仪表的数据检测载体的运动与否，易造成误判，尤其在上下车等扰动条件下，采用位移计算方法，消除摇晃带来的仪表扰动。

具体的，本实施例的所述零速的判断条件为：列车的加速度变化率小于设定的加速度阈值，同时列车的位移变化率小于设定的位移阈值。

进一步地，所述列车的加速度变化率

式中，

为在设定时间范围(例如1S)内惯性组件测量的加速度f_i,j的滑动平均；

所述列车的位移变化率

式中，

为在设定时间范围(例如1S)内惯性解算的速度V_i,j的滑动平均；k为测量时刻，m、n为叠加次数；x,y,z为导航坐标系的三个轴向。

优选的，所述加速度阈值为0.05±0.02mg；所述位移阈值为5±1m。

在同时满足所述零速的两个判断条件，即Δ_f,k,j＜0.05±0.02mg；Δ_v,k,j＜5±1m时视为零速，否则视为非零速。

若是检测为零速情况，采用零速修正算法开展零速修正。

地铁列车停车时间较短，一般不超过1min，本实施例采用最小二乘方法，快速获得姿态修正误差。

具体的，零速修正算法包括以下步骤：

1)检测速度增量，判断零速状态是否有效；

为提高零速检测的可靠性，在根据上述条件判断零速后，进一步检测速度增量，由于地铁列车在轨道上行进的特点，以轨道方向为坐标系的y轴，则可认为列车只在y轴上有速度变化，在x轴和z轴上可视为0速。即速度增量Δ_k,y＝V_k,y-V_k-1,y，V_k,y为k时刻的列车的y轴速度，当速度增量Δ_k,y小于速度增量阈值(Δ_k,y＜0.2m/s)时，则零速状态有效；否则，零速状态无效，更新当前时间点的速度值V_k,y＝Δ_k,y+V_k-1,y，进入运动约束状态。

2)建立滤波器，对零速状态下测量的加速度进行滤波；

所述滤波器为

a₁,a₂,a₃,a₄,b₁,b₂,b₃为设定的滤波参数。

根据对滤波器参数拟合的结果，在a₁＝0.00000375683802，,a₂＝0.000011270514059,a₃＝0.000011270514059,a₄＝0.00000375683802，b₁＝-2.93717072844989,b₂＝2.87629972347933,b₃＝-0.939098940325283时，能够得到较优的滤波效果。

3)根据滤波后的加速度求解用于零速修正的姿态矩阵；

所述零速修正的姿态矩阵的求解包括：

a.将滤波后的加速度正交化，得到

b.进行坐标转换；将载体坐标系下的加速度

转换得到零速载体系下的加速度

式中，

为k时刻的零速载体系到载体系的转换矩阵，由

计算获得，而

为载体坐标系下的三轴角速度数据；

c.计算零速载体系下姿态角

姿态角

式中，

L为当地纬度，ω_ie为地球自转角速度，k为计算时刻；其中0为零时刻，b₀代表为零速载体系下数据，n为导航坐标系下数据，n₀为零速导航坐标系下数据。

d.计算零速下的姿态矩阵

所述姿态矩阵

式中，

导航坐标系到零速导航坐标系的转换矩阵

4)将速度置零，并将惯性解算的姿态矩阵

更新为所述姿态矩阵

若是检测为非零速情况，采用运动约束状态的滤波修正方法，处理地铁列车在运行过程中定位误差漂移问题，借助于列车运行特点，列车仅有前向速度，侧向与垂向速度为零的约束下，对自主导航算法进行滤波修正，提高动态定位精度。

具体的，所述运动约束状态的滤波修正方法包括：

1)建立误差模型形式

X(t)为状态变量，A(t)为状态转移矩阵，w(t)为***噪声，y(t)为量测观测量，H(t)为量测矩阵，v(t)为量测噪声；

具体的，所述

式中，φ_x,φ_y,φ_z是姿态误差，δV_x,δV_y,δV_z是速度误差，

是安装角误差；

所述量测观测量

所述量测矩阵

状态转移矩阵

f^b为加速度输出

为载体坐标系到导航坐标系的转换矩阵，φ＝[φ_x,φ_y,φ_z]。

***噪声

量测噪声

2)采用kalman器进行迭代滤波；

本实施例可采用公知的kalman器进行迭代滤波根据量测观测量对状态变量进行迭代滤波，任何可实现此过程的迭代滤波算法均不影响本发明的保护范围。

3)采用滤波后的估计参数对包括姿态矩阵、安装角和速度在内的惯性数据进行修正，以提高惯性解算输出的导航定位的位置速度数据的精度。

具体的，姿态矩阵修正：

安装角修正：

速度修正：V＝V+δV；

采用本实施例的地铁用自主导航定位方法，进行地铁环境实测根据修正后的速度信息递推计算位置信息，试验一结果如图2、3、表1所示，实验二结果图4、5和表2所示。

表1实验一里程误差测试结果

表2实验二里程误差测试结果

	上行(基准)	下行(基准)	上行(测试)	下行(测试)	上行(误差)	下行(误差)
							站间1	1491.252	1485.180	1490.23	1486.73	-1.022	1.55
站间2	1394.5	1394.472	1393.54	1395.43	-0.96	0.96
							站间3	1775.116	1777.149	1774.15	1778.17	-0.97	1.02

综上，相比于现有技术，本实施例用惯性导航设备增强了地铁列车自主导航能力，并通过零速修正技术，在地铁列车停站阶段消除历史累积误差，在列车运行过程中，采用列车运动约束技术，而保持列车在运行阶段有较高定位精度。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。

Claims (10)

1.一种地铁列车的自主导航定位方法，其特征在于，包括

获取安装于列车上的惯性组件的初始姿态、速度及位置信息；

根据所述惯性组件测量的角速度及加速度数据进行惯性解算；

检测列车是否处于零速；是，则进入零速修正状态，对所述惯性组件的姿态进行修正；否，则进入运动约束状态，对惯性解算数据进行滤波修正。

2.根据权利要求1所述的自主导航定位方法，其特征在于，所述零速的判断条件为：列车的加速度变化率小于设定的加速度阈值，同时列车的位移变化率小于设定的位移阈值。

3.根据权利要求2所述的自主导航定位方法，其特征在于，

所述列车的加速度变化率

式中，

为在设定时间范围内惯性组件测量的加速度f_i,j的滑动平均；

所述列车的位移变化率

式中，

为在设定时间范围内惯性解算的速度V_i,j的滑动平均；k为测量时刻，m、n为叠加次数；x,y,z为导航坐标系的三个轴向。

4.根据权利要求1所述的自主导航定位方法，其特征在于，进入零速修正状态，对所述惯性组件的姿态进行修正，具体包括：

1)检测速度增量，判断零速状态是否有效；

2)建立滤波器，对零速状态下测量的加速度进行滤波；

3)根据滤波后的加速度求解用于零速修正的姿态矩阵；

4)将速度置零，并将惯性解算的姿态矩阵更新为所述姿态矩阵。

5.根据权利要求4所述的自主导航定位方法，其特征在于，

所述速度增量Δ_k,y＝V_k,y-V_k-1,y，V_k,y为k时刻的列车的y轴速度，当速度增量Δ_k,y小于速度增量阈值时，则零速状态有效；否则，零速状态无效，更新当前时间点的速度值V_k,y＝Δ_k,y+V_k-1,y，进入运动约束状态。

6.根据权利要求4所述的自主导航定位方法，其特征在于，

所述滤波器为

a₁,a₂,a₃,a₄,b₁,b₂,b₃为设定的滤波参数。

7.根据权利要求1所述的自主导航定位方法，其特征在于，

所述求解用于零速修正的姿态矩阵包括：

1)将滤波后的加速度正交化，得到

2)进行坐标转换；得到零速载体系下的加速度

式中，

为k时刻的零速载体系到载体系的转换矩阵，由

计算获得，而

为载体坐标系下的三轴角速度数据；

3)计算零速载体系下姿态角

姿态角

式中，

L为当地纬度，ω_ie为地球自转角速度，k为计算时刻；其中0为零时刻，b₀代表为零速载体系下数据，n为导航坐标系下数据，n₀为零速导航坐标系下数据；

4)计算零速下的姿态矩阵

所述姿态矩阵

式中，

为零速载体系到载体系的转换矩阵。

8.根据权利要求1所述的自主导航定位方法，其特征在于，

所述进入运动约束状态，对惯性解算数据进行滤波修正，包括：

1)建立误差模型形式

X(t)为状态变量，A(t)为状态转移矩阵，w(t)为***噪声，y(t)为量测观测量，H(t)为量测矩阵，v(t)为量测噪声；

2)采用kalman器进行迭代滤波；

3)采用滤波后的估计参数对包括姿态矩阵、安装角和速度在内的惯性数据进行修正。

9.根据权利要求8所述的自主导航定位方法，其特征在于，所述

式中，φ_x,φ_y,φ_z是姿态误差，δV_x,δV_y,δV_z是速度误差，

是安装角误差；

所述量测观测量

10.根据权利要求8所述的自主导航定位方法，其特征在于，

量测矩阵

状态转移矩阵

f^b为加速度输出

为载体坐标系到导航坐标系的转换矩阵，φ＝[φ_x,φ_y,φ_z]。

Images