CN108444473A - 一种行人室内轨迹定位方法 - Google Patents

Abstract

本发明公开一种行人室内轨迹定位方法，包括以下步骤，利用传感器采集载体坐标系下的三轴加速度、三轴角速度和三轴磁感应强度作为数据源，所述三轴为X轴、Y轴和Z轴；进行计步和步态检测；计算航向角，基于主方向进行所述航向角修正：走直线时，利用启发式随机漂移消除法进行航向角修正；走曲线时，利用扩展卡尔曼滤波器进行偏移补偿，修正航向角；根据三轴加速度，建立非线性步长估计模型，计算行进过程中的步长；根据步态、行进步数、航向角以及步长，计算行人行走轨迹和位置。本发明有效降低了步态检测的误差，提高了室内定位的精度，解决了由于陀螺漂移而产生的航向角发散问题，提高了用户在步长多样化时的估计精度。

Description

一种行人室内轨迹定位方法

技术领域

本发明属于室内定位技术领域，尤其涉及行人室内轨迹定位方法。

背景技术

随着GPS全球定位系统的发展以及智能便携终端(智能手机、平板电脑、智能硬件等)的普及，与室内相关的位置服务(LBS)越来越受到人们的关注。然而，在室内环境中，由于楼宇等建筑物的遮挡，导致GPS信号非常微弱，定位精度无法达到人们要求。因此，人们对室内定位技术需求十分迫切。

现有的室内定位技术主要分为以下几种：基于短距离的无线通信定位、基于Wifi信号的无线定位方法和基于惯性传感器的定位算法。以上室内定位技术普遍存在的缺点是成本较高或者精度较低。另外步行者航位推算方法(PDR)用于定位的误差来源主要是步数步长精度不高带来的累积误差及航向角的偏移。

发明内容

为了解决现有技术中存在的上述问题，本发明提出一种行人室内轨迹定位方法，采用基于惯性传感器的定位方式，利用基于MEMS技术的惯性传感器和运动学相关的理论，从步态检测、航向修正、步长检测、实时定位四个阶段出发，对步数、航向角、步长的推算方面分别进行优化，可以有效提高室内定位的精度。

一种行人室内轨迹定位方法，包括以下步骤，利用传感器采集载体坐标系下的三轴加速度、三轴角速度和三轴磁感应强度作为数据源，所述三轴为X轴、Y轴和Z轴；进行计步和步态检测；计算航向角，基于主方向进行所述航向角修正：走直线时，利用启发式随机漂移消除法进行航向角修正；走曲线时，利用扩展卡尔曼滤波器进行偏移补偿，修正航向角；根据所述三轴加速度，建立非线性步长估计模型，计算行进过程中的步长；根据步态、行进步数、航向角以及步长，计算行人行走轨迹和位置。

计步和步态检测通过一种改进的波峰检测法来实现，可精准计步、识别原地踏步及手动晃动非正常行走状态。

本发明去除了身体晃动等非正常情况的噪声，精准的检测出行走的步数和手动晃动的情况，并区分了原地踏步，有效降低了步态检测的误差，提高了室内定位的精度；采用基于主方向的航向角修正方法，解决了由于陀螺漂移而产生的航向角发散问题；以Weinberg非线性步长估计模型作为研究基础，通过增加了权重参数和步频信息，提高了用户在步长多样化时的估计精度。

附图说明

图1为本发明实施例1行人室内轨迹定位方法流程图；

图2为本发明实施例2中计步和步态检测流程图；

图3为实施例2中原地踏步检测流程图；

图4为实施本发明任一实施例后行人实际在室内走动的轨迹效果图，图中实线为办公室实际轨迹，虚线为行人轨迹。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

实施例1

一种行人室内轨迹定位方法，如图1所示，包括以下步骤，

利用传感器采集载体坐标系下的三轴加速度、三轴角速度和三轴磁感应强度作为数据源，所述三轴为X轴、Y轴和Z轴；

进行计步和步态检测；

计算航向角，基于主方向进行所述航向角修正：走直线时，利用启发式随机漂移消除法进行航向角修正；走曲线时，利用扩展卡尔曼滤波器进行偏移补偿，修正航向角；

根据三轴加速度，建立非线性步长估计模型，计算行进过程中的步长；

根据步态、行进步数、航向角以及步长，计算行人行走轨迹和位置。

计算航向角和计算行进过程中的步长无顺序要求，可同时执行也可顺序执行。

通常情况下，行人使用九轴蓝牙传感器和智能终端搭载的APP配合实施本实施例。九轴蓝牙传感器包括采集加速度的加速度计、采集角速度的陀螺仪和采集磁感应强度的磁力计，所述九轴蓝牙传感器位于行人的脚上，以脚踝效果最佳。智能终端与传感器通过蓝牙进行通信连接，传感器采集的数据上传至智能终端，可通过APP展示相关数据，并呈现行人的轨迹。传感器的工作频率以50Hz为最佳。

根据传感器绑在脚踝上的方向，定义载体坐标系为b系，参考坐标系为n系，n系为地理坐标系。在b系中，X轴为左右方向，Y轴为上下方向，Z轴为前后方向(由于传感器绑在脚踝上，行走方向为Y轴在水平面上的投影)。

实施例2

在实施例1的基础上，本实施例中计步和步态检测采用如下方式：

S11、采集传感器内置的加速度计、陀螺仪和磁力计的X轴、Y轴、Z轴三个方向上的数据并保存；

S12、计算整体加速度，通过加速度计的三轴加速度计算整体加速度，避免因为载体坐标系与地理坐标系不同而引起的人体垂直方向上加速度不规律、误差大等因素，降低了设备姿态带来的影响，计算公式为

其中a_x、a_y、a_z为加速度计三个方向x、y、z的实时采集值；

S13、对整体加速度进行均值滤波；设置一个滑动窗口，将每个窗口内的数据分为一组，对其求和后取平均值，将平均值保存作为新的整体加速度的值，滤去噪声，使数据更加平滑，更易于波峰检测；

S14、利用滑动窗口的取值进行波峰检测，过程如下：

(14-1)判断潜在峰值是否在设定范围内变化，如是则进入步骤(14-2)，否则更新数据，重新检测潜在峰值，从而避免身体抖动和传感器自身的误差造成数据波动；

(14-2)判断当前点的波动趋势，若当前的点为下降趋势，之前的点为上升趋势，则进入步骤(14-3)，否则更新数据，返回(14-1)；

(14-3)判断潜在波峰的出现时间与前一峰值的时间差是否在预设的第一阈值范围内，如是则进入步骤(14-4)，否则更新数据，返回(14-1)，从而排除一些突然坐下及转动身体等带来的加速度值激增；

(14-4)判断Z轴的角速度在滑动窗口的方差是否小于设定的第二阈值，如是则进入步骤(14-5)，否则更新数据，返回(14-1)，从而排除手势晃动的情况；

(14-5)取潜在峰值及其前10个采样点和后10个采样点进行比较，若潜在峰值最大，则为真正的波峰，步数加1，否则更新数据，返回(14-1)；

S15、检测原地踏步；具体过程如下：

(15-1)对X轴的角速度gyroX进行低通滤波，去除噪声；

(15-2)在满足上述计步条件的基础上，取出每一步中X轴角速度的最大值maxGyroX和最小值minGyroX；

(15-3)设定一个第三阈值，若maxGyroX与minGyroX的差值小于第三阈值，则此次步态为原地踏步，将此次行走的步长设为0，否则为行进步态，步数加1。

实施例3

在实施例1的基础上，本实施例基于主方向进行所述航向角修正的过程是：

S21、将载体坐标系b系下三个方向的加速度a^b(a_x，a_y，a_z)，通过四元数坐标变换矩阵变换到导航坐标系n系a_n(a_东,a_北,a_天)；

S22、每次行走的方向由姿态角决定，姿态角包括航向角ψ，俯仰角θ，横滚角γ，通过式(2)～(4)组成的方程组得出：

其中，为四元数坐标变换矩阵；

四元数坐标变换矩阵根据陀螺仪输出的角速度，利用如下四元数微分方程计算出来；

其中，Q(q₀，q₁，q₂，q₃)为四元数，ω为输出的角速度，为四元数乘法，从而计算得到四元数坐标变换矩阵

计算航向角ψ：

S23、在启发式随机漂移消除算法的基础上，设定“米”字形的8个主方向，采用3个相邻步间的航向变化来检测直线行走和转弯，公式如下：

其中，m表示行走路线的状态，1代表直线，0代表转弯；Ψ_i表示当前步的航向角，Ψ_i-1和Ψ_i-2分别表示前一步和前两步的航向角，Ψ_th表示设定的偏差阈值，偏差阈值为10±2°左右；

S24、当判断行人走直线，即m＝1时，在启发式随机漂移消除算法反馈系统中，前一时刻航向角相对于当前行人主方向的偏移角度经MOD函数求取，MOD函数定义如下：

其中，Δ为当前航向角和上一步航向角之间的差值，ΔINT(k)为取整函数，结果为不大于k值的最大整数；将Ψ_i-1、Δ代入公式(8)：

所得结果处于[0，Δ]内，当处于区间与内分别代表不同的偏移方向，将与作比较，即E_i的符号代表不同的偏移方向；

根据E_i的符号，通过二进制控制器进行偏移修正：当E_i为正值时，表示航向角的方向偏向主方向的左侧，则反馈变量I_i减去修正系数i_c；当E_i为负值时，表示航向角的方向偏向主方向的右侧，则反馈变量I_i加上修正系数i_c，具体的二进制控制器反馈修正过程如公式(10)所示：

将得到的I_i对当前航向角进行修正，使得航向角方向逐渐回归主方向，即：

Ψ_i-HDE＝Ψ_raw，i+I_i (11)

其中Ψ_i-HDE为修正后的航向角，Ψ_raw,i为原航向角，当判断行人走曲线，即m＝0时，启发式随机漂移消除算法(HDE)停止了陀螺仪的偏移补偿，此时通过扩展卡尔曼滤波(EKF)进行偏移补偿。

实施例4

在实施例2的基础上，本实施例采用一种新的非线性步长估计模型，并结合了步频的信息，利用如下公式，对步长进行估计。

其中，a_max、a_min分别代表行走一步时的最大加速度值和最小加速度值；k代表使估计步长与真实步长之间误差的最优值，它依赖于用户自身的真实步长估计得到，针对不同的用户取不同的值，衡量了加速度值对步长估计的影响，使结果更加准确；l为权重参数，它控制a_max与a_min之和在整个算法中的影响程度；β代表步频控制系数，由训练数据经最小二乘法拟合得到；F为步频，由式(13)计算得到。

其中T_Peak和T_Valley分别为上述计步和步态检测过程中每一步的波峰与波谷的出现时间。

最后，计算行人行走轨迹和位置的过程如下：

设定一个初始坐标，根据计算得到的步数，航向角，步长，利用式(14)计算下一个点的坐标，从而推算出该行人的行走轨迹，

其中，X_k和Y_k分别表示行人第k步在X轴方向(东)和Y轴方向(北)上的位移，S_k表示行人在第k步上的步长，θ_k表示行人在第k步上的航向角。

本发明方案所公开的技术手段不仅限于上述实施方式所公开的技术手段，还包括由以上技术特征任意组合所组成的技术方案。

Claims (8)

1.一种行人室内轨迹定位方法，其特征在于，包括以下步骤，

利用传感器采集载体坐标系下的三轴加速度、三轴角速度和三轴磁感应强度作为数据源，所述三轴为X轴、Y轴和Z轴；

进行计步和步态检测；

计算航向角，基于主方向进行所述航向角修正：走直线时，利用启发式随机漂移消除法进行航向角修正；走曲线时，利用扩展卡尔曼滤波器进行偏移补偿，修正航向角；

根据所述三轴加速度，建立非线性步长估计模型，计算行进过程中的步长；

根据步态、行进步数、航向角以及步长，计算行人行走轨迹和位置。

2.根据权利要求1所述的行人室内轨迹定位方法，其特征在于，所述计算航向角的步骤和计算行进过程中的步长的步骤同时执行或顺序执行。

3.根据权利要求1所述的行人室内轨迹定位方法，其特征在于，所述传感器为九轴蓝牙传感器，包括采集加速度的加速度计、采集角速度的陀螺仪和采集磁感应强度的磁力计，所述九轴蓝牙传感器位于行人的脚上。

4.根据权利要求3所述的行人室内轨迹定位方法，其特征在于，所述九轴蓝牙传感器的工作频率为50Hz。

5.根据权利要求1～4任一项所述的行人室内轨迹定位方法，其特征在于，所述计步和步态检测的过程如下，

S11、采集传感器内置的加速度计、陀螺仪和磁力计的X轴、Y轴、Z轴三个方向上的数据并保存；

S12、计算整体加速度，计算公式为

其中a_x、a_y、a_z为加速度计三个方向x、y、z的实时采集值；

S13、对所述整体加速度进行均值滤波；设置一个滑动窗口，将每个窗口内的数据分为一组，对其求和后取平均值，将所述平均值保存作为新的整体加速度的值，滤去噪声；

S14、利用滑动窗口的取值进行波峰检测，过程如下：

(14-1)判断潜在峰值是否在设定范围内变化，如是则进入步骤(14-2)，否则更新数据，重新检测潜在峰值；

(14-2)判断当前点的波动趋势，若当前的点为下降趋势，之前的点为上升趋势，则进入步骤(14-3)，否则更新数据，返回(14-1)；

(14-3)判断潜在波峰的出现时间与前一峰值的时间差是否在预设的第一阈值范围内，如是则进入步骤(14-4)，否则更新数据，返回(14-1)；

(14-4)判断Z轴的角速度在所述滑动窗口的方差是否小于设定的第二阈值，如是则进入步骤(14-5)，否则更新数据，返回(14-1)；

(14-5)取潜在峰值及其前10个采样点和后10个采样点进行比较，若潜在峰值最大，则为真正的波峰，步数加1，否则更新数据，返回(14-1)；

S15、检测原地踏步；具体过程如下：

(15-1)对X轴的角速度gyroX进行低通滤波，去除噪声；

(15-2)在满足计步条件的基础上，取出每一步中X轴角速度的最大值maxGyroX和最小值minGyroX；

(15-3)设定一个第三阈值，若maxGyroX与minGyroX的差值小于第三阈值，则此次步态为原地踏步，将此次行走的步长设为0，否则为行进步态，步数加1。

6.根据权利要求1～4任一项所述的行人室内轨迹定位方法，其特征在于，基于主方向进行所述航向角修正的过程是：

S21、将载体坐标系b系下三个方向的加速度a^b(a_x，a_y，a_z)，通过四元数坐标变换矩阵变换到导航坐标系n系a_n(a_东,a_北,a_天)；

S22、每次行走的方向由姿态角决定，姿态角包括航向角ψ，俯仰角θ，横滚角γ，所述航向角通过如下方程组得出：

其中，为四元数坐标变换矩阵；

计算航向角ψ：

S23、在启发式随机漂移消除算法的基础上，设定“米”字形的8个主方向，采用3个相邻步间的航向变化来检测直线行走和转弯，公式如下：

其中，m表示行走路线的状态，1代表直线，0代表转弯；Ψ_i表示当前步的航向角，Ψ_i-1和Ψ_i-2分别表示前一步和前两步的航向角，Ψ_th表示设定的偏差阈值，偏差阈值为10±2°

左右；

S24、当判断行人走直线，即m＝1时，在启发式随机漂移消除算法反馈系统中，前一时刻航向角相对于当前行人主方向的偏移角度经MOD函数求取，MOD函数定义如下：

其中，Δ为当前航向角和上一步航向角之间的差值，ΔINT(k)为取整函数，结果为不大于k值的最大整数；将Ψ_i-1、Δ代入公式(8)：

所得结果处于[0，Δ]内，当处于区间与内分别代表不同的偏移方向，将与作比较，即E_i的符号代表不同的偏移方向；

根据E_i的符号，通过二进制控制器进行偏移修正：当E_i为正值时，表示航向角的方向偏向主方向的左侧，则反馈变量I_i减去设定的修正系数i_c；当E_i为负值时，表示航向角的方向偏向主方向的右侧，则反馈变量I_i加上修正系数i_c，具体的二进制控制器反馈修正过程如公式(10)所示：

将得到的I_i对当前航向角进行修正，使得航向角方向逐渐回归主方向，即：

Ψ_i-HDE＝Ψ_raw，i+I_i (11)

其中，Ψ_i-HDE为修正后的航向角，Ψ_raw,i为原航向角，当判断行人走曲线，即m＝0时，通过扩展卡尔曼滤波进行偏移补偿。

7.根据权利要求5所述的行人室内轨迹定位方法，其特征在于，所述行进过程中的步长的计算过程是：

其中，a_max、a_min分别表示行走一步时的最大加速度值和最小加速度值，k表示使估计步长与真实步长之间误差的最优值，它依赖于用户自身的真实步长估计得到，l为权重参数，β表示步频控制系数，由训练数据经最小二乘法拟合得到，F为步频，由如下公式(9)计算得到。

其中T_Peak和T_Valley分别为步态检测阶段中每一步的波峰与波谷的出现时间。

8.根据权利要求7所述的行人室内轨迹定位方法，其特征在于，所述计算行人行走轨迹和位置的过程如下：

设定一个初始坐标，根据计算得到的步数，航向角，步长，利用式(14)计算下一个点的坐标，从而推算出该行人的行走轨迹，

其中，X_k和Y_k分别表示行人第k步在X轴方向(东)和Y轴方向(北)上的位移，S_k表示行人在第k步上的步长，θ_k表示行人在第k步上的航向角。