一种基于惯性传感器的地铁定位方法与系统

摘要

本发明公开了一种基于惯性传感器的地铁定位方法，包括：(1)利用移动终端的加速度传感器采集列车的启动加速度；(2)根据重力加速度和所述启动加速度确定所述移动终端相对列车的初始姿态，并根据所述初始姿态对列车的加速度进行分解；(3)计算列车加速度传感器的零点漂移向量以及陀螺仪传感器的零点漂移向量；(4)修正所述沿列车轨道方向的加速度分量，并根据修正后的沿列车轨道方向的加速度分量计算列车的位移。本发明仅使用移动终端自带的陀螺仪及加速度计，利用离线地图和基站小区的id，实现地铁内部高精度定位。本发明方法具有低成本、高精度的特点，能够解决现有移动终端地图基站定位模式误差大，切换慢的问题，具有很好应用前景。

权利要求

1.一种基于惯性传感器的地铁定位方法，其特征在于，包括：

(1)当判断到用户进入地铁中，利用移动终端的加速度传感器采集列车的启动加速度；

(2)根据重力加速度和所述启动加速度确定所述移动终端相对列车的初始姿态，并根据所述初始姿态对列车的加速度进行分解，至少得到沿列车轨道方向的加速度分量；

(3)计算列车加速度传感器的零点漂移向量以及陀螺仪传感器的零点漂移向量；

(4)根据所述加速度传感器的零点漂移向量以及陀螺仪传感器的零点漂移向量修正所述沿列车轨道方向的加速度分量，并根据修正后的沿列车轨道方向的加速度分量计算列车的位移，得到列车的定位信息。

2.如权利要求1所述的基于惯性传感器的地铁定位方法，其特征在于，所述步骤(2)中根据重力加速度和所述启动加速度确定所述移动终端相对列车的初始姿态，具体为：(2.1)利用重力加速度通过梯度下降法求解移动终端的相对重力姿态；

(2.2)利用列车前进加速度纠正步骤(2.1)中的相对重力姿态得到最终相对姿态。

3.如权利要求2所述的方法，其特征在于，所述步骤(2.1)具体包括：(2.1.1)移动终端在静止状态时加速度传感器测量移动终端的加速度，此时移动终端的加速度为重力加速度；

(2.1.2)根据已知的重力加速度大小和方向，利用梯度下降法以迭代方式测量得到重力加速度旋转到地球坐标系下的旋转轴与旋转角，实现初步定位。

4.如权利要求2或3所述的方法，其特征在于，所述步骤(2.2)具体为：利用列车启动时加速度方向与列车坐标系Y轴方向一致的特点，将列车启动时加速度传感器测得加速度围绕步骤(2.1)所得坐标系Z轴旋转，得到最终列车与移动终端相对姿态关系。

5.如权利要求1或2所述的方法，其特征在于，所述步骤(2)中根据所述初始姿态对列车的加速度进行分解，至少得到沿列车轨道方向的加速度分量，具体包括：(2.3)利用陀螺仪测量初始相对姿态确定之后移动终端相对列车实时姿态旋转角速度，以此更新坐标系旋转矩阵；

(2.4)利用实时更新得到的坐标系旋转矩阵，对加速度传感器测量得到加速度向量旋转至列车坐标系内，旋转后向量Y轴分量即为沿列车轨道方向的加速度分量。

6.如权利要求1或2所述的方法，其特征在于，所述步骤(3)中计算列车加速度的零点漂移向量具体包括：(3.1)利用步骤(2)中所得到的加速度沿列车横向振动分量计算列车振动短时能量，以此判断列车是否停站；

(3.2)利用列车停站时加速度计测量沿列车前进方向加速度即为零点漂移这一特点结合加速度零点漂移线性增加特性，在前两站停站时测得加速度零点漂移斜率实时估算加速度零点漂移向量。

7.如权利要求1或2所述的方法，其特征在于，所述步骤(3)中计算陀螺仪的零点漂移向量具体包括：计算加速度计测量加速度向量与已知重力加速度之间叉积，得到陀螺仪零点漂移向量。

8.如权利要求1或2所述的方法，其特征在于，所述步骤(4)中根据所述加速度的零点漂移向量以及陀螺仪的零点漂移向量修正所述沿列车轨道方向的加速度分量具体包括：(4.1)利用步骤(3)计算得到的加速度传感器的零点漂移向量，以向量和的形式修正步骤(2)中沿列车轨道方向的加速度分量；

(4.2)利用步骤(3)计算得到的陀螺仪零点漂移向量，以向量和的形式修正陀螺仪实时测量的角速度。

9.如权利要求1或2所述的方法，其特征在于，所述步骤(4)中根据修正后的沿列车轨道方向的加速度分量计算列车的位移，得到列车的定位信息具体包括：(4.3)利用修正后沿列车轨道方向加速度分量二次积分得到列车位移；

(4.4)根据轨道交通沿线经纬度数据计算当前位移对应地图经纬度，得到列车实时经纬度定位信息。

10.一种基于惯性传感器的地铁定位系统，其特征在于，包括启动加速度获取模块、初始姿态获取与加速度分解模块、零点漂移计算模块以及列车定位模块，其中：所述启动加速度获取模块，用于当判断到用户进入地铁中，利用移动终端的加速度传感器采集列车的启动加速度；

所述初始姿态获取与加速度分解模块，用于根据重力加速度和所述启动加速度确定所述移动终端相对列车的初始姿态，并根据所述初始姿态对列车的加速度进行分解，至少得到沿列车轨道方向的加速度分量；

所述零点漂移计算模块，用于计算列车加速度传感器的零点漂移向量以及陀螺仪传感器的零点漂移向量；

所述列车定位模块，用于根据所述加速度传感器的零点漂移向量以及陀螺仪传感器的零点漂移向量修正所述沿列车轨道方向的加速度分量，并根据修正后的沿列车轨道方向的加速度分量计算列车的位移，得到列车的定位信息。

说明书

一种基于惯性传感器的地铁定位方法与系统

技术领域

[0001] 本发明属于地理定位技术领域，更具体地，涉及一种基于惯性传感器的地铁定位方法及系统。

背景技术

[0002] 近年来，随着经济的不断发展，各种移动终端(例如智能手机、平板电脑等，本发明中后续为了描述方便均以智能手机例)在我国不断普及，人们在使用智能手机的同时通过市场上多种多样的地图软件方便地调用手机的GPS以及基站定位系统可以简单轻松地为自己导航，这类导航方式已广泛的应用于我们生活的方方面面，比如驾车、出行、公交等等。GPS和基站两种定位方式涵盖了市面上绝大部分智能手机地图定位软件。智能手机地图定位软件的存在极大方便了我们的出行，让我们不再迷失自己的位置。然而它们并不能覆盖到我们生活中某些常见情况，例如地铁，由于卫星无线电信号无法穿透地下土壤，GPS在地铁中完全无法使用；若手机地图采用基站定位方式，又由于基站信号在隧道内反射叠加等原因，造成基站定位误差过大(约400m)，输出定位数据存在无固定发生周期、发生时间间隔大等问题无法满足用户对手机地图的高精度定位需求。

[0003] 现有技术在手机上广泛依赖于GPS信号和基站定位信号。专利号为2011103039353，提出利用GPS和GPRS信号的公交定位方案，该方案利用GPS信号定位，将定位结果通过GPRS上传至服务器供乘客查看。专利号为2015110143538，提出一种无GPS信号下定位方法，该方法在无GPS信号时利用WIFI信号获得终端位置情况，并上传服务器。综上所述，目前手机定位主要依赖于GPS一旦GPS信号丢失，只能选择定位精度低的基站或WIFI进行定位，不适于地铁这类高速交通工具定位。

发明内容

[0004] 在上述应用背景下，为解决目前地铁定位精度低、延迟大的问题。利用手机惯性传感器实现高精度地铁定位具有极大应用前景。本发明在智能手机基础上利用其惯性传感器实现高精度地铁定位。本发明采用技术方案如下：

[0005] 首先，当乘客入站时利用乘客进入地铁站后手机需切换基站的原理，本发明提出基于基站CellID的乘客入站判断策略。当手机基站发生切换事件时，手机获取当前基站小区识别码CellID将其跟手机云端数据进行比对，如果连接到的基站CellID与地铁站基站CellID一致，则判定乘客进入地铁站，此时，手机关闭GPS以节约功耗；同时，开启加速度传感器和陀螺仪为地铁定位提供原始数据。

[0006] 然后，当乘客上车后，要计算加速度在列车前进方向分量需已知手机相对列车之间的位置关系。本发明利用重力加速度和列车启动加速度实现手机初始姿态确定。

[0007] 最后，针对惯性传感器测量误差的问题，本发明提出基于参考点检测的误差修正方案。本发明通过计算列车横向加速度短时能量判断列车运行状态(行驶、停止)当检测到列车停止时，计算列车加速度零点漂移，在后续行驶过程中对其进行修正。同时，利用加速度计修正陀螺仪长期测量出现的零点漂移提高陀螺仪定位精度，并最终输出列车定位结果。

[0008] 为了实现上述目的，本发明提供了一种基于惯性传感器的地铁定位方法，包括：

[0009] (1)当判断到用户进入地铁中，利用移动终端的加速度传感器采集列车的启动加速度；

[0010] (2)根据重力加速度和所述启动加速度确定所述移动终端相对列车的初始姿态，并根据所述初始姿态对列车的加速度进行分解，至少得到沿列车轨道方向的加速度分量；

[0011] (3)计算列车加速度传感器的零点漂移向量以及陀螺仪传感器的零点漂移向量；

[0012] (4)根据所述加速度传感器的零点漂移向量以及陀螺仪传感器的零点漂移向量修正所述沿列车轨道方向的加速度分量，并根据修正后的沿列车轨道方向的加速度分量计算列车的位移，得到列车的定位信息。

[0013] 本发明的一个实施例中，所述步骤(2)中根据重力加速度和所述启动加速度确定所述移动终端相对列车的初始姿态，具体为：

[0014] (2.1)利用重力加速度通过梯度下降法求解移动终端的相对重力姿态；

[0015] (2.2)利用列车前进加速度纠正步骤(2.1)中的相对重力姿态得到最终相对姿态。

[0016] 本发明的一个实施例中，所述步骤(2.1)具体包括：

[0017] (2.1.1)移动终端在静止状态时加速度传感器测量移动终端的加速度，此时移动终端的加速度为重力加速度；

[0018] (2.1.2)根据已知的重力加速度大小和方向，利用梯度下降法以迭代方式测量得到重力加速度旋转到地球坐标系下的旋转轴与旋转角，实现初步定位。

[0019] 本发明的一个实施例中，所述步骤(2.2)具体为：

[0020] 利用列车启动时加速度方向与列车坐标系Y轴方向一致的特点，将列车启动时加速度传感器测得加速度围绕步骤(2.1)所得坐标系Z轴旋转，得到最终列车手机相对姿态关系。

[0021] 本发明的一个实施例中，所述步骤(2)中根据所述初始姿态对列车的加速度进行分解，至少得到沿列车轨道方向的加速度分量，具体包括：

[0022] (2.3)利用陀螺仪测量初始相对姿态确定之后手机相对列车实时姿态旋转角速度，以此更新坐标系旋转矩阵；

[0023] (2.4)利用实时更新得到的坐标系旋转矩阵，对加速度传感器测量得到加速度向量旋转至列车坐标系内，旋转后向量Y轴分量即为沿列车轨道方向的加速度分量。

[0024] 本发明的一个实施例中，所述步骤(3)中计算列车加速度的零点漂移向量具体包括：

[0025] (3.1)利用步骤(2)中所得到的加速度沿列车横向振动分量计算列车振动短时能量，以此判断列车是否停站；

[0026] (3.2)利用列车停站时加速度计测量沿列车前进方向加速度即为零点漂移这一特点结合加速度零点漂移线性增加特性，在前两站停站时测得加速度零点漂移斜率实时估算加速度零点漂移向量。

[0027] 本发明的一个实施例中，所述步骤(3)中计算陀螺仪的零点漂移向量具体包括：

[0028] 计算加速度计测量加速度向量与已知重力加速度之间叉积，得到陀螺仪零点漂移向量。

[0029] 本发明的一个实施例中，所述步骤(4)中根据所述加速度的零点漂移向量以及陀螺仪的零点漂移向量修正所述沿列车轨道方向的加速度分量具体包括：

[0030] (4.1)利用步骤(3)计算得到的加速度传感器的零点漂移向量，以向量和的形式修正步骤(2)中沿列车轨道方向的加速度分量；

[0031] (4.2)利用步骤(3)计算得到的陀螺仪零点漂移向量，以向量和的形式修正陀螺仪实时测量的角速度。

[0032] 本发明的一个实施例中，所述步骤(4)中根据修正后的沿列车轨道方向的加速度分量计算列车的位移，得到列车的定位信息具体包括：

[0033] (4.3)利用修正后沿列车轨道方向加速度分量二次积分得到列车位移；

[0034] (4.4)根据轨道交通沿线经纬度数据计算当前位移对应地图经纬度，得到列车实时经纬度定位信息。

[0035] 本发明的一个实施例中，在所述步骤(1)中，当移动终端发生基站切换时，移动终端获取切换后所连接的基站小区识别码CellID并判断所述CellID是否是地铁站基站CellID，如题是则判定乘客进入地铁站。

[0036] 按照本发明的另一方面，还提供了一种基于惯性传感器的地铁定位系统，包括启动加速度获取模块、初始姿态获取与加速度分解模块、零点漂移计算模块以及列车定位模块，其中：

[0037] 所述启动加速度获取模块，用于当判断到用户进入地铁中，利用移动终端的加速度传感器采集列车的启动加速度；

[0038] 所述初始姿态获取与加速度分解模块，用于根据重力加速度和所述启动加速度确定所述移动终端相对列车的初始姿态，并根据所述初始姿态对列车的加速度进行分解，至少得到沿列车轨道方向的加速度分量；

[0039] 所述零点漂移计算模块，用于计算列车加速度传感器的零点漂移向量以及陀螺仪传感器的零点漂移向量；

[0040] 所述列车定位模块，用于根据所述加速度传感器的零点漂移向量以及陀螺仪传感器的零点漂移向量修正所述沿列车轨道方向的加速度分量，并根据修正后的沿列车轨道方向的加速度分量计算列车的位移，得到列车的定位信息。

[0041] 与现有的技术对比，本方案所提供的技术方案具有以下优点：

[0042] 1、不依赖GPS、WIFI信号实现高精度定位，所需传感器在智能手机中广泛应用，无需硬件改造，易于市场推广。

[0043] 2、本发明可与目前主流手机地图融合，实现地铁高精度导航。目前手机地图地上地下定位算法相同，导致地上定位精度高、地下定位精度低的现状。本发明提出基于CellID的乘客入站判断策略，可以自动实现地上地下定位算法切换。

[0044] 3、目前基于手机定位主要依赖GPS实现高精度定位，本文利用惯性传感器实现地铁环境中高精度定位，在GPS信号不佳的地下仍能正常工作。

[0045] 4、本发明提出惯性原件误差修正方案，有效降低误差在站间积累，提高系统整体定位精度。

附图说明

[0046] 图1是本发明实施例中基于惯性传感器的地铁定位方法的流程示意图；

[0047] 图2是本发明实施例中手机与列车之间相对姿态关系示意图；

[0048] 图3是本发明实施例中初步对齐后的手机坐标系与列车坐标系之间相对姿态关系；

[0049] 图4是本发明实施例中列车的速度-位移曲线图；

[0050] 图5是本发明实施例中列车的连续停站速度曲线图；

[0051] 图6是本发明实施例中连续定位实验结果图。

具体实施方式

[0052] 为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

[0053] 针对智能手机地图定位软件在地铁内部定位精度优化问题，主要可以从两方面着手处理：一方面，利用地铁公司拥有地铁全部线路调度信息的优势，尝试开放其调度信息给地图软件公司，以提高地铁定位精度；另一方面，使用智能手机自带传感器及通讯装置优化地铁内部定位结果。方法一简单有效，但地铁公司出于安全性等多方面考虑，目前并没有开放接口的先例。因而，要提高手机地图在地铁内部定位精度需要从手机内部传感器以及通讯装置入手。

[0054] 鉴于惯性传感器对物体运动形式及运动地点和环境没有特殊限制，可以弥补GPS和基站定位依赖外部信号的不足。而且，目前智能手机中加速度传感器和陀螺仪等惯性传感元件已得到广泛普及。综上所述，利用智能手机内部惯性传感器替代传统GPS和基站定位算法，实现地铁环境下高精度定位具有很大研究价值和应用前景。利用惯性传感器实现地铁定位的主要难点在于手机相对列车初步姿态确定以及惯性传感器误差消除。

[0055] 本文提出算法整体结构如图1所示，智能手机地铁定位算法结构分为四阶段：传感器触发与数据采集、手机姿态确定及加速度分解、误差修正和位置估计。

[0056] 其中乘客入站判断策略主要根据入站后手机切换基站CellID判断。当乘客携带手机进入地铁站内时，随GPS信号一并衰减的还有手机与地面通信基站之间的信号强度。当手机与基站之间信号强度降低至不满足通讯条件时(不同手机存在不同阈值)，根据手机系统自身基站切换算法会连接到地铁站内基站。当手机基站发生切换事件时，手机获取当前基站小区识别码CellID将其跟手机云端数据进行比对，如果连接到的基站CellID与地铁站基站CellID一致，则判定乘客进入地铁站，此时，手机关闭GPS以节约功耗；同时，开启加速度传感器和陀螺仪为地铁定位提供原始数据。

[0057] 如图2，针对乘客上车之后手机与列车之间相对姿态关系解算。本发明提出基于梯度下降法的手机与列车相对姿态解算方案。该解算方案通过两步实现手机与列车之间初始姿态解算：

[0058] 1.利用重力加速度通过梯度下降法求解手机相对重力姿态；

[0059] 2.利用列车前进加速度纠正第1步对齐结果得到最终相对姿态。

[0060] 根据梯度下降法概念，结合手机坐标系与重力加速度之间存在夹角的应用场景，构建最优解方程。设地球坐标系转换到手机坐标系估算初始位置为 重力场由于已知大小方向，我们将其记为E`d，传感器运动后测得的场强为S`sensor。已知的地球重力场在地球坐标系下表示为E`d经过坐标轴旋转 以后，对齐到手机坐标系，其输出值与当前传感器测得的重力场向量相比，相差最小的解，即为误差最小的估算姿态。

[0061]

[0062] 根据上述等式，将该最优解问题带入梯度下降法公式可得如下公式，其中， 是第k次估算旋转向量，μ为最速下降步长。 的梯度与范数的商为自变量为 时，在平面中下降最快的方向(即解平面的负梯度方向)，乘以步长μ即可得到下降最快步长。值得注意的是μ距离系数在每次迭代过程中可变。设初始旋转向量为 经过k+1次迭代后，得到k+1次迭代修正后估算旋转向量结果 此时 带入等式(3-7)值最小即为最优解。

[0063]

[0064] 又因为重力加速度方向大小已知，本文将重力加速度带入梯度下降法方程对齐进行简化。其简化后参数如下所示：

[0065]

[0066] E`d＝[0 0 0 1]

[0067] S`Sensor＝[0 sx sy sz]

[0068] 将上述简化参数带入梯度下降法中，得到对应等式和雅克比矩阵。进而可以求得旋转矩阵。

[0069]

[0070]

[0071] 初步对齐后，手机坐标系与列车坐标系之间相对姿态关系如图3所示。要将手机坐标系和列车坐标系完全对齐，需在列车加速过程中，对初次对齐后坐标系围绕列车坐标系Z轴旋转，当旋转中坐标系Y轴加速度最大即可确定手机与列车之间相对姿态关系。加速度向量 初次转换后坐标系表示记为(0,ax,ay,az)，其中az≈g。理论上，坐标系完全对齐之后ax＝0，ay表征加速度在水平方向分量。设axy为加速度a在XY平面内的分量，ay加速度a在初次校正后坐标系内Y轴上投影，坐标轴X`Y`围绕Z轴顺时针旋转β与XY坐标轴对齐，则转角β满足如下公式。

[0072]

[0073] 将求得角度β带入四元数公式，可对向量二次旋转，其公式如下[0074]

[0075] acor＝qaq-1

[0076] 如公式所示，将β带入带入四元数旋转公式后，设定旋转轴为Z轴，即可得到最终对齐后向量输出acor。对加速度acor的Y轴分量进行积分即可得到列车位移信息。但是，由于惯性传感元件误差原因，系统输出存在零点漂移，为提高系统定位精度，本发明提出基于参考点检测的误差修正算法。

[0077] 如图4，列车在停车时真实速度为0，此时由于停车后Z轴不受车体振动等原因影响，其加速度输出应稳定为0(重力加速度g已修正)。设图4(a)中光滑曲线为系统输出列车沿轨道切线方向行驶速度V(Ta)，设列车真实速度为RPVa，速度误差ΔV(Ta)计算公式如下。

[0078] ΔV(Ta)＝V(Ta)-RPVa

[0079] 根据加速度测量误差随时间变化缓慢的特征，我们可以认为在短时间内，加速度误差是稳定不变的。于是，在时间a至时间b中的加速度误差变化率可以通过如下所示公式求得

[0080]

[0081] 因此，加速度误差补偿值已知，由于加速度误差变化缓慢且近似为线性，如图5，则可得到修正之后任意时刻速度值。

[0082] Vcal(t)＝V(t)-ΔV(Ta)-ΔA×(t-Ta)

[0083] 下面只需要根据修正后速度积分即可获得行驶距离。

[0084] 针对上述误差修正算法，进行实验得到结果，如图6所示。图6(a)为引入误差修正算法之前积分所得速度，从图中可以看出列车每次停站由于加速度零点漂移的存在导致速度在站点间不断累加，至第4次停站时速度误差ΔV≈10m/s，引入武汉地铁2号线最高速度80km/h(即22.2m/s)，速度误差约为45％。图6(b)为零点误差修正后所得速度，从图中可以看出随加速度零点漂移修正，每次停站后速度近似为0，且误差不会在站点间积累导致长距离定位发生偏差。图6(c)描述了修正后列车定位结果，如图所示，最终定位结果为4000m相对岳家嘴至洪山广场线路全长4.279km线路误差为6.5％，在可接受范围内。综上所述，智能手机地铁定位系统通过引入误差修正算法，有效降低速度零点漂移，使整体定位误差在有效范围内。

[0085] 进一步地，本发明还提供了一种基于惯性传感器的地铁定位系统，包括启动加速度获取模块、初始姿态获取与加速度分解模块、零点漂移计算模块以及列车定位模块，其中：

[0086] 所述启动加速度获取模块，用于当判断到用户进入地铁中，利用移动终端的加速度传感器采集列车的启动加速度；

[0087] 所述初始姿态获取与加速度分解模块，用于根据重力加速度和所述启动加速度确定所述移动终端相对列车的初始姿态，并根据所述初始姿态对列车的加速度进行分解，至少得到沿列车轨道方向的加速度分量；

[0088] 所述零点漂移计算模块，用于计算列车加速度传感器的零点漂移向量以及陀螺仪传感器的零点漂移向量；

[0089] 所述列车定位模块，用于根据所述加速度传感器的零点漂移向量以及陀螺仪传感器的零点漂移向量修正所述沿列车轨道方向的加速度分量，并根据修正后的沿列车轨道方向的加速度分量计算列车的位移，得到列车的定位信息。

[0090] 本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。