CN108844533A - 一种基于多传感器融合和姿态解算的自由姿态pdr定位方法 - Google Patents

Abstract

本发明公开了一种基于多传感器融合和姿态解算的自由姿态PDR定位方法，该方法包括两个部分的内容，一是通过分析行人行进过程中的运动特征，提出了一种自适应的更新状态判断阈值的MFSM步数检测方法，使之能够在手机相对人体位置稳定时的自由姿态下能够保证步数检测的精度；二是提出了一种简单准确的180°模糊判断方法，对由加速度进行PCA分析得到的运动轴方向进行判断从而得到行人前进方向。同时提出了模糊判断置信度的概念，以结合陀螺仪对方向估计结果进行校正。通过实际测量，该方法降低了行人利用便携电子设备进行PDR定位时的姿态限制，在设备放置位置相对人体稳定的自由姿态，如平端、口袋、背包等姿态下，均可获得准确的计步和方向估计结果。

Description

一种基于多传感器融合和姿态解算的自由姿态PDR定位方法

技术领域

本发明属于移动通信、定位导航领域，具体涉及一种基于多传感器融合和姿态解算的自由姿态PDR定位方法。

背景技术

随着移动通信技术的快速发展、数据业务和多媒体业务的快速增加，人们逐渐不满足于单纯的室外粗犷的位置确定与导航，对各种更复杂环境下的精确定位需求越来越高。尤其是在复杂的室内环境中，常常需要确定移动终端或设备在室内的精确位置，因此基于位置的服务(Location Based Service,LBS)越来越受到大家的重视。

基于惯性导航的定位技术需要已知初始点，并且定位误差会随着时间的延续而累积，但是基于多传感器信息的惯性导航因为部署成本低、抗干扰性强等优势日益受到人们的关注，尤其是在融合定位算法研究以及人工智能的研究发展中，具有十分重要的作用。基于惯性导航的PDR算法主要包含步态检测、步长估计、方向估计三个方面内容。由于手机等移动设备相对于汽车、飞机的独特性，在位置估计与行迹追踪的过程中，对步数和行进方向进行估计时，人们对于手机的不同放置方式会对定位结果造成极大的影响。所以有必要研究手机处于相对自由的姿态模式下进行PDR定位的方法。

将常见的姿态分为多种经典模式：平端模式、口袋模式、背包模式、通话模式等。上述模式共同点为手机相对于行人具有固定的相对位置，但是其放置姿态自由。对于平端模式下的PDR定位，步数检测一般采用峰值检测能够获取较准确的准确度，行进方向则一般直接采用利用重力和磁场计算得到的欧拉角中的航向角，此时需要手机顶端指向行进方向。然而其他模式下这一方法不能取得较好的结果，虽然有人提出通过计算手机顶端指向和航向角之间的偏差的方法来对估计结果进行补偿，然而基于各种姿态、多次放置之间的差异性，这类方法往往不能取得比较稳定的结果。本发明通过对上述常见模式下的PDR定位方法进行研究，提供了一种同样适用设备相对行人固定放置时的姿态无约束PDR定位方法，主要包括步数检测方法与行进方向估计的方法。

发明内容

本发明的目的是为了解决便携带电子设备如手机等在设备相对行人固定放置时的自由姿态下步数检测受到设备放置位置与姿态影响造成的精度下降以及手机方向传感器输出航向角对行人行进方向估计失效的问题，提供了一种基于多传感器融合和姿态解算的自由姿态PDR定位方法，该方法能够在设备相对行人固定放置时的自由姿态下有效的检测行进步数以及估计行进方向，从而克服利用便携电子设备如手机等进行PDR定位时设备需要平端，航向角方向指向行进方向的限制，能够保证手机在相对行人固定放置时的自由姿态下进行PDR定位的计步与方向准确性，具有良好的实用性和可扩展性且不会增加额外的成本。

本发明采用如下技术方案来实现的：

一种基于多传感器融合和姿态解算的自由姿态PDR定位方法，包括以下步骤：

1)通过预设置或利用基于GPS或WiFi定位方法提前获取设备的初始位置坐标，作为当前位置；

2)利用设备集成的惯性传感器和磁力计对加速度、重力、磁场强度、角速度进行同频率采样；

3)对采样数据进行预处理，利用重力采样值和磁场强度采样值将基于载体坐标系的加速度和角速度转换到导航坐标系(NED坐标系)下，并对转换后的数据进行截止频率大于2Hz低通滤波；

4)对低通滤波后的加速度取模值，考虑自由姿态下设备在行人行走过程中的运动特征，以加速度模值作为步数检测有效检测量，采用有限状态机的思想，进行适用于稳定自由姿态下状态判断阈值自适应更新的步态检测，得到待确认步态；

5)对步骤4)检测到的待确认步态进行步态确认，剔除误检步，最终确认是否行走了一步；

6)当检测到行走一步后，对当前步持续时间内的加速度进行PCA处理，处理对象为转换到导航坐标系水平面X、Y坐标轴的水平加速度，通过PCA处理的方法得到行进方向的运动轴方向；

7)采用基于加速度分布的统计分析方法对运动轴方向进行180°模糊判断，获得行进方向初步估计结果；

8)利用步骤2)滤波后的角速度信息和模糊判断置信度对行进方向初步估计结果进行校验修正；

9)通过对用户进行行走测试，估计行走步长；

10)在当前位置的基础上利用检测到的步数、方向、步长信息进行PDR定位，对位置进行迭代更新；

11)位置更新后继续步骤4)中的步态检测过程，直到行人停止行走。

本发明进一步的改进在于，步骤2)中，同频率采样的具体方法如下：

201)对加速度传感器、重力传感器、磁力计、陀螺仪以频率f进行基于软件的同频采样；

202)当传感器实际采样频率不同时，采用减采样的方法，选取采样频率最慢的传感器作为基准，选择与基准传感器采样时间戳最接近的其他传感器采样值作为此次采样的同序采样值，将非同序采样值删除以保证传感器采样频率相等。

本发明进一步的改进在于，步骤3)中，对采用数据进行预处理的具体方法如下：

301)利用重力和磁场计算载体坐标系和导航坐标系之间的旋转矩阵，已知A＝(A_x A_yA_z)^T为由三轴加速度组成的单位向量，E＝(E_x E_y E_z)^T为由三轴磁场强度组成的单位向量，令H＝E×A,M＝A×H，则由即可得到此刻的旋转矩阵利用将加速度和角速度转换到导航坐标系下，其中S_b为当前时刻传感器采样原始值，S_n为转换后的导航坐标系下的数值；

302)对导航坐标系下的加速度和角速度采用巴特沃兹滤波器进行低通滤波处理，其中采样频率f不小于50Hz，截止频率大于2Hz。

本发明进一步的改进在于，步骤4)中，步态检测的具体实现方法如下：

401)设定运动检测阈值Thr、波峰状态判断阈值Thr_pp、波谷状态判断阈值Thr_np、步态结束阈值Thr_neg，根据采样的加速度模值将人行走过程中采样点划分为静止状态S0、预备运动状态S1、进入峰值状态S2、离开峰值状态S3、进入谷值状态S4、离开谷值状态S6、噪声容忍状态S5、和步态结束状态S7；

402)波峰状态判断阈值、波谷状态判断阈值自适应更新公式为：

Thr_pp(i)＞＝Thr_np(i)+e (3)

其中，pp₀为波峰状态判断阈值初始值，Thr_pp(i)为第i步波峰状态判断阈值，max_i为第i步峰值状态中采样点的最大值，a＜1,b＜1；np₀为波谷状态判断阈值初始值，Thr_np(i)为第i步的波谷状态判断阈值，min_i为第i步谷值状态中采样点的最小值，c＞1,d＞1；e为大于等于0的常数，N₀为初始阈值作用步数，最小取4；

403)运动检测阈值Thr、步态结束阈值Thr_neg为常数，满足：

np₀＜Thr_neg＜g (4)

g＜Thr＜pp₀ (5)

其中g为重力常数；

404)初始状态从S0状态进入有限状态机，通过状态判断阈值来判断当前时刻状态，当进入步态结束状态S7时，判断为检测到一次待确认步；

405)在经过步骤5)进行步态确认后，若确为一步，则根据公式(1)～(5)自适应的进行下一步状态判断阈值的更新，并进行下一步的检测；若检测为不是真实一步，则不更新状态判断阈值，利用当前阈值继续进行步态检测。

本发明进一步的改进在于，步骤5)中，步态确认的具体实现方法如下：

501)在根据步骤4)中有限状态机进行步态检测过程中，对每一次加速度采样点的状态和采样时间进行标记，将第i步预备运动状态S1第一个点记为A_i，采样时间记为beginMoveTime(i)；将第i步步态结束状态S7点记为D_i，采样时间记为endStepTime(i)；将第i步进入峰值状态S2中的最大值点记为B_i，采样值记为max_i，采样时间记为maxValueTime(i)；将第i步进入谷值状态S4最小值点记为C_i，采样值记为min_i，采样时间记为minValueTime(i)；

502)通过步骤501)标记的时间和采样值，对检测到的待确认步进行步态确认，以剔除产生的多检步数，设置状态S1至S7持续时间最小值thr_time，和峰谷差最小值thr_value，当满足以下条件时：

endStepTime(i)-beginMoveTime(i)＞thr_time (6)

max_i-min_i＞thr_value (7)

则确定检测到一步，否则认为此待确认步态为误检步，不进行计步。

本发明进一步的改进在于，步骤6)中，估计运动轴方向的具体实现方法如下：

601)对第i步进行PCA分析的加速度采样区间为[beginTime(i),endStepTime(i)]，其中beginTime(i)＝max(endStepTime(i)-a×dt,beginMoveTime(i))，式中dt为一步平均持续时间；a为稍大于1的系数，根据步频结合实验测试估算a×dt取值；

602)对由步骤3)转换到导航坐标系下并进行低通滤波的水平面X、Y轴加速度信息进行PCA处理，得到最大特征值对应的特征向量 指示的方向就是行人行走的运动轴方向。

本发明进一步的改进在于，步骤7)中，180°模糊判断的具体实现方法如下：

701)根据步骤501)所标记的信息，以[beginMoveTime(i),maxValueTime(i)]为正向于行进方向的区间Rp，简称正向区间；以(maxValueTime(i),minValueTime(i)]为反向于行进方向区间Rn，简称反向区间；

702)将水平面由垂直于运动轴方向的过零点直线分为两个半平面，其中包含运动轴方向的半平面为Sp半平面，另一部分为Sn半平面；

703)对正向区间Rp和反向区间Rn内的水平加速度采样进行数量统计，若正向区间Rp内采样点分布在Sp半平面内，或反向区间Rn内采样点分布在Sn半平面内，标定运动方向为运动轴正向的系数p加1，否则标定运动方向为运动轴反向的系数n加1；

704)统计Rp、Rn两区间内的所有采样点，若p＞n，则此步的运动方向为运动轴方向，否则运动方向为运动轴反方向。

本发明进一步的改进在于，步骤8)中，方向校正过程的具体实现方法如下：

801)根据步骤7)中180°模糊判断时的运动轴正反向系数p、n得到模糊判断置信度，如下：

当时，则表示模糊判断结果完全随机不可信；当conf小于可信阈值thr_conf时，表示模糊判断结果可信度低；当conf越接近于1时，模糊判断可信度越高；

802)根据步骤3)中导航坐标系下滤波后的Z轴角速度计算第i-N步至第i步的方向相对变化量：

其中N为大于0的常数，当方向相对变化量ΔYaw(i-N,i)＜lowThr时，表示连续N步之内为直线行走，ΔYaw(i-N,i)＞upThr时，表示连续N步之内发生了转弯，其中gyro_z(j)表示第j次采样经过坐标系转换后的导航坐标系下Z轴的角速度，P为第i-N步到第i步的角速度采样区间，ΔT为两次连续采样的时间间隔，lowThr为直线行走方向相对变化判断阈值，upThr为转弯过程方向相对变化判断阈值；

803)一步模糊判断置信度conf小于可信阈值thr_conf时，表明模糊判断结果可信度较低，比较前N步中的模糊判断置信度，记录模糊判断置信度最大的步为第N_m步，以第N_m步的方向和方向变化量计算相对方向：

orien_R(i)＝orien(i-N_m)+ΔYaw(i-N_m,i) (10)

若运动轴方向在导航坐标系下的方向度数orien_M(i)与orien_R(i)之间方向夹角小于90°，则认为此时行进方向为运动轴方向，否则为运动轴反方向；

804)若连续两步的估计方向差比较小时，则对当前步和前一步原始方向求平均值作为当前步的估计方向；

805)当前状态为拐弯时，比较前N步中的模糊判断置信度，记录模糊判断置信度最大的步为第N_m步，以第N_m步的方向和方向变化量计算相对方向：

orien_R(i)＝orien(i-N_m)+ΔYaw(i-N_m,i) (10)

以orien_R(i)作为当前拐弯步的行进方向。

本发明进一步的改进在于，步骤9)中，步长估计的具体实现方法如下：

对行人进行训练，令行人沿直线行走一段距离，记录行走距离d和步数M，通过L＝d/M获得步长的估计结果。

本发明进一步的改进在于，步骤10)中，位置更新的具体实现方法如下：

已知初始点位置坐标(px₀,py₀)时，根据步数i、行进方向orien(i)、步长L，利用PDR位置更新公式，如下：

进行第i步PDR定位位置的计算更新。

本发明具有如下有益的技术效果：

本发明提供了一种基于多传感器融合和姿态解算的自由姿态PDR定位方法，该方法降低了利用智能手机进行室内定位时对手机放置姿态的限制，包括：利用步态分析中支撑相与摆动相的差异，设计了一种智能手机放置在以平端、裤袋、背包三种模式为代表的相对行人固定放置时的自由姿态下，利用有限状态机进行步态检测的状态判断阈值自适应更新方法，使得计步结果鲁棒性更高；利用有限状态机进行步态检测过程中进行状态标记，进而选择特定状态区间进行PCA处理以估计运动方向；利用对不同状态下水平加速度的分布特性进行统计分析，从而进行运动轴的180°模糊判断，判断过程简单，运算量小；定义了模糊判断置信度，并利用陀螺仪进行直行、转弯判别，结合模糊置信度对估计方向进行校验修正，提高了模糊判断与方向估计的准确程度。

附图说明

图1为本发明方法的定位实现过程框图；

图2为本发明方法MFSM步数检测状态转换关系图；

图3为本发明方法Sp、Sn平面划分图；

图4为本发明方法180°模糊判断流程图；

图5为本发明测试实验环境图；

图6为本发明方法在手机平端、裤袋、背包模式下PDR定位结果轨迹图。

具体实施方式

以下结合具体附图和实施例详细阐述本发明。

参见图1，本发明提供的一种基于多传感器融合和姿态解算的自由姿态PDR定位方法，包括以下步骤：

(1)通过预设置或利用基于GPS、WiFi等定位方法提前获取设备的初始位置坐标，作为当前位置；

(2)利用设备集成的惯性传感器和磁力计对加速度、重力、磁场强度、角速度进行同频率采样；同频率采样的具体方法如下：

(201)对加速度传感器、重力传感器、磁力计、陀螺仪以频率f进行基于软件的同频采样。

(202)当传感器实际采样频率不同时，采用减采样的方法，选取采样频率最慢的传感器作为基准，选择与基准传感器采样时间戳最接近的其他传感器采样值作为此次采样的同序采样值，将非同序采样值删除以保证传感器采样频率相等。

(3)对采样数据进行预处理，利用重力采样值和磁场强度采样值将基于载体坐标系的加速度和角速度转换到导航坐标系(NED坐标系)下，并对转换后的数据进行截止频率大于2Hz低通滤波；对采用数据进行预处理的具体方法如下：

(301)利用重力和磁场计算载体坐标系和导航坐标系之间的旋转矩阵，已知A＝(A_x A_y A_z)^T为由三轴加速度组成的单位向量，E＝(E_x E_y E_z)^T为由三轴磁场强度组成的单位向量，令H＝E×A,M＝A×H，则由即可得到此刻的旋转矩阵利用将加速度和角速度转换到导航坐标系下，其中S_b为当前时刻传感器采样原始值，S_n为转换后的导航坐标系下的数值；

(302)对导航坐标系下的加速度和角速度采用巴特沃兹滤波器进行低通滤波处理，其中采样频率f不小于50Hz，截止频率大于2Hz。

(4)对低通滤波后的加速度取模值，考虑自由姿态下设备在行人行走过程中的运动特征，以加速度模值作为步数检测有效检测量，采用有限状态机的思想，进行适用于稳定自由姿态下状态判断阈值自适应更新的步态检测，得到待确认步态；步态检测的具体实现方法如下：

(401)设定运动检测阈值Thr、波峰状态判断阈值Thr_pp、波谷状态判断阈值Thr_np、步态结束阈值Thr_neg，根据采样的加速度模值大小将人行走过程中采样点划分为静止状态(S0)、预备运动状态(S1)、进入峰值状态(S2)、离开峰值状态(S3)、进入谷值状态(S4)、离开谷值状态(S6)、噪声容忍状态(S5)和步态结束状态(S7)；

(402)波峰状态判断阈值、波谷状态判断阈值自适应更新公式为：

Thr_pp(i)＞＝Thr_np(i)+e (3)

其中，pp₀为波峰状态判断阈值初始值，Thr_pp(i)为第i步波峰状态判断阈值，max_i为第i步峰值状态中采样点的最大值，a＜1,b＜1；np₀为波谷状态判断阈值初始值，Thr_np(i)为第i步的波谷状态判断阈值，min_i为第i步谷值状态中采样点的最小值，c＞1,d＞1；e为大于等于0的常数，N₀为初始阈值作用步数，最小取4；

(403)运动检测阈值Thr、步态结束阈值Thr_neg为常数，满足：

np₀＜Thr_neg＜g (4)

g＜Thr＜pp₀ (5)

其中g为重力常数；

(404)初始状态从S0状态进入有限状态机，通过状态判断阈值根据图2所示状态转移关系来判断当前时刻状态，当进入步态结束状态(S7)时，判断为检测到一次待确认步；

(405)在经过步骤(5)进行步态确认后，若确为一步，则根据公式(1)～(5)自适应的进行下一步状态判断阈值的更新，并进行下一步的检测；若检测为不是真实一步，则不更新状态判断阈值，利用当前阈值继续进行步态检测。

(5)对步骤(4)检测到的待确认步态进行步态确认，剔除误检步，最终确认是否行走了一步；步态确认的具体实现方法如下：

(501)在根据步骤(4)中有限状态机进行步态检测过程中，对每一次加速度采样点的状态和采样时间进行标记，将第i步S1状态第一个点记为A_i，采样时间记为beginMoveTime(i)；将第i步S7状态点记为D_i，采样时间记为endStepTime(i)；将第i步S2状态中的最大值点记为B_i，采样值记为max_i，采样时间记为maxValueTime(i)；将第i步S4状态最小值点记为C_i，采样值记为min_i，采样时间记为minValueTime(i)；

(502)通过步骤(501)标记的时间和采样值，对检测到的待确认步进行步态确认，以剔除由于设备晃动等原因可能产生的多检步数，设置S1至S7持续时间最小值thr_time，和峰谷差最小值thr_value，当满足以下条件时：

endStepTime(i)-beginMoveTime(i)＞thr_time (6)

max_i-min_i＞thr_value (7)

则确定检测到一步，否则认为此待确认步态为误检步，不进行计步。

(6)当检测到行走一步后，对当前步持续时间内的加速度进行PCA处理，处理对象为转换到导航坐标系水平面X、Y坐标轴的水平加速度，通过PCA处理的方法得到行进方向的运动轴方向；估计运动轴方向的具体实现方法如下：

(601)对第i步进行PCA分析的加速度采样区间为[beginTime(i),endStepTime(i)]，其中beginTime(i)＝max(endStepTime(i)-a×dt,beginMoveTime(i))，式中dt为一步平均持续时间；a为稍大于1的系数，可以根据步频结合实验测试估算a×dt取值，选取a×dt＝600ms；

(602)对由步骤(3)转换到导航坐标系下并进行低通滤波的水平面X、Y轴加速度信息进行PCA处理，得到最大特征值对应的特征向量 指示的方向就是行人行走的运动轴方向。

(7)采用基于加速度分布的统计分析方法对运动轴方向进行180°模糊判断，获得行进方向初步估计结果；180°模糊判断的具体实现方法如下：

(701)根据步骤(501)所标记的信息，以[beginMoveTime(i),maxValueTime(i)]为正向于行进方向的区间Rp，简称正向区间；以(maxValueTime(i),minValueTime(i)]为反向于行进方向区间Rn，简称反向区间；

(702)如图3所示，将水平面由垂直于运动轴方向的过零点直线分为两个半平面，其中包含运动轴方向的半平面为Sp半平面，另一部分为Sn半平面；

(703)如图4所示流程图，对正向区间Rp和反向区间Rn内的水平加速度采样进行数量统计，若正向区间Rp内采样点分布在Sp半平面内，或反向区间Rn内采样点分布在Sn半平面内，标定运动方向为运动轴正向的系数p加1，否则标定运动方向为运动轴反向的系数n加1；

(704)统计Rp、Rn两区间内的所有采样点，若p＞n，则此步的运动方向为运动轴方向，否则运动方向为运动轴反方向。

(8)利用步骤(2)滤波后的角速度信息和模糊判断置信度对行进方向初步估计结果进行校验修正；方向校正过程的具体实现方法如下：

(801)根据步骤(7)中180°模糊判断时的运动轴正反向系数p、n得到模糊判断置信度，如下：

当时，则表示模糊判断结果完全随机不可信；当conf小于可信阈值thr_conf时，表示模糊判断结果可信度较低；当conf越接近于1时，模糊判断可信度越高；

(802)根据步骤(3)中导航坐标系下滤波后的Z轴角速度计算第i-N步至第i步的方向相对变化量：

其中N为大于0的常数，当方向相对变化量ΔYaw(i-N,i)＜lowThr时，表示连续N步之内为直线行走，ΔYaw(i-N,i)＞upThr时，表示连续N步之内发生了转弯，其中gyro_z(j)表示第j次采样经过坐标系转换后的导航坐标系下Z轴的角速度，P为第i-N步到第i步的角速度采样区间，ΔT为两次连续采样的时间间隔，lowThr为直线行走方向相对变化判断阈值，upThr为转弯过程方向相对变化判断阈值；

(803)一步模糊判断置信度conf小于可信阈值thr_conf时，表明模糊判断结果可信度较低，比较前N步中的模糊判断置信度，记录模糊判断置信度最大的步为第N_m步，以第N_m步的方向和方向变化量计算相对方向：

orien_R(i)＝orien(i-N_m)+ΔYaw(i-N_m,i) (10)

若运动轴方向在导航坐标系下的方向度数orien_M(i)与orien_R(i)之间方向夹角小于90°，则认为此时行进方向为运动轴方向，否则为运动轴反方向；

(804)若连续两步的估计方向差比较小时，则对当前步和前一步原始方向求平均值作为当前步的估计方向；

(805)当前状态为拐弯时，比较前N步中的模糊判断置信度，记录模糊判断置信度最大的步为第N_m步，以第N_m步的方向和方向变化量计算相对方向：

orien_R(i)＝orien(i-N_m)+ΔYaw(i-N_m,i) (10)

以orien_R(i)作为当前拐弯步的行进方向。

(9)通过对用户进行行走测试，估计行走步长；步长估计的具体实现方法如下：

对行人进行训练，令行人沿直线行走一段距离，记录行走距离d和步数M，通过L＝d/M获得步长的估计结果。

(10)在当前位置的基础上利用检测到的步数、方向、步长信息进行PDR定位，对位置进行迭代更新；位置更新的具体实现方法如下：

已知初始点位置坐标(px₀,py₀)时，根据步数i、行进方向orien(i)、步长L，利用PDR位置更新公式，如下：

进行第i步PDR定位位置的计算更新。

(11)位置更新后继续步骤(4)中的步态检测过程，直到行人停止行走。

实施例：

具体测试环境为图5所示，环境面积为41.3m×26.1m,信号采集终端为Android智能手机，红色轨迹为测试路线，箭头指向为行进方向；选择平端、裤袋、背包模式进行测试，定位轨迹如图6所示，其中电子罗盘方向轨迹表示平端模式利用手机电子罗盘方向作为行进方向时的轨迹，平端模式、裤袋模式、背包模式轨迹为本发明实施例结果，步数检测方法均采用本发明步态检测方法；

A、利用Android智能手机终端以50Hz频率对传感器(包括加速度传感器、重力传感器、磁场传感器、陀螺仪)进行同频率采样，获取加速度、重力、磁场、角速度信息及采样时间戳；

B、利用重力和磁场计算载体坐标系和导航坐标系之间的旋转矩阵。A＝(A_x A_y A_z)^T为由三轴加速度组成的单位向量，E＝(E_x E_y E_z)^T为由三轴磁场强度组成的单位向量，令H＝E×A,M＝A×H，则由即可得到时刻的旋转矩阵利用将加速度和角速度转换到导航坐标系下，其中S_b为当前时刻传感器采样原始值，S_n为转换后的导航坐标系下的数值；

C、对导航坐标系下的加速度和Z轴角速度进行低通滤波，截止频率选择2.5Hz，得到滤波后的三轴加速度a_x、a_y、a_z和z轴角速度gyro_z；

D、利用加速度模值进行MFSM步数检测，根据公式(1)～(5)进行状态判断阈值更新，选择参数a＝0.95，b＝0.9，c＝1.85，d＝1.25，e＝0.5，N＝6，pp₀＝10.4，np₀＝9.3，Thr＝10.3，Thr_neg＝9.4，利用状态判断阈值根据图2所示状态转移图进行状态判断，并标定好每次采样点的状态，当进入步态结束状态(S7)时，记为一次待确认步，同时根据标定状态找出A_i、B_i、C_i、D_i四个点的值和采样时间；

E、由A_i、B_i、C_i、D_i四个点的值和采样时间，根据公式(6)、(7)检测此待确认步是否需要剔除，若不满足剔除条件，则确认为计一步，然后确定180°模糊判断区间，以[beginMoveTime(i),maxValueTime(i)]为正向于行进方向区间Rp，以(maxValueTime(i),minValueTime(i)]为反向于行进方向区间Rn；

F、确定进行PCA分析的时间窗口，对第i步进行PCA分析的加速度采样时间区间为[beginTime(i),endStepTime(i)]，其中

beginTime(i)＝max(endStepTime(i)-600ms,beginMoveTime(i))；

G、对采样时间处于[beginTime(i),endStepTime(i)]内的水平加速度按照时间顺序排列为(x₁,y₁)、(x₂,y₂)……(x_K,y_K)，得到序列X＝(x₁,x₂,......x_K)、Y＝(y₁,y₂,......y_K)，然后利用公式计算得到X序列和Y序列的协方差，进而得到协方差矩阵然后计算协方差矩阵C的特征值λ₁、λ₂(λ₁≥λ₂)。特征值λ₁、λ₂对应的特征向量分别为其中指向即使运动轴方向；

H、对当前一步内属于正向区间Rp和反向区间Rn的所有采样点进行统计分析，以表示第i步内的第j次加速度采样转换得到的水平加速度向量，利用公式计算采样点方向与运动轴之间的夹角，若β＜90°，则认为采样点位于Sp中，否则认为采样点位于Sn中；在正向分布区间，若实际采样点位于Sp中，则正向系数p加1，否则反向系数n加1；在反向分布区间，若实际采样点位于Sn中，则正向系数p加1，否则反向系数n加1；最终，若p＞n，则运动方向为运动轴方向，否则为运动轴反方向；

I、由步骤H中的正向判断系数p和反向判断系数n代入公式(8)计算模糊判断置信度，当conf＜0.6时，表明模糊判断结果可信度较低；

J、利用角速度根据计算第i-N步至第i步的方向相对变化，当ΔYaw(i-4,i)＜15°时，表示连续四步之内为直线行走；当ΔYaw(i-4,i)＞60°时，表示连续四步之内发生转弯；当ΔYaw(i-1,i)＞45°时，表示当前步发生转弯。

K、当四步内为直线行走时，若其中一步与另外三步相差大于90°时，则以另外三步的平均值作为方向突变步的方向；若连续两步相差小于30°，则利用前一步初始方向值与当前步初始方向值得平均值作为当前步的方向；

L、当发生转弯时或者模糊判断置信度低时，在前三步中选择置信度最高的一步方向，并计算相对方向orien_R(i)＝orien(i-N_m)+ΔYaw(i-N_m,i)，当发生转弯时，则以orien_R(i)为行进方向；当模糊判断置信度低时，利用orien_R(i)对运动轴方向orien_M(i)进行模糊判断，若orien_M(i)与orien_R(i)之间方向夹角小于90°，此时行进方向为运动轴方向，否则为反方向；

M、进行步长的训练，采用固定步长，令行人沿直线行走一段距离，记录行走距离d和步数M，通过L＝d/M获得步长的估计结果，实验步长选择0.8m；

N、已知初始位置信息，利用步数、方向、步长信息，通过PDR位置更新公式

进行第i步定位位置的计算更新，并继续进行步骤D的步态检测过程，直到行人停止行走。

Claims (10)

1.一种基于多传感器融合和姿态解算的自由姿态PDR定位方法，其特征在于，包括以下步骤：

1)通过预设置或利用基于GPS或WiFi定位方法提前获取设备的初始位置坐标，作为当前位置；

2)利用设备集成的惯性传感器和磁力计对加速度、重力、磁场强度、角速度进行同频率采样；

3)对采样数据进行预处理，利用重力采样值和磁场强度采样值将基于载体坐标系的加速度和角速度转换到导航坐标系下，并对转换后的数据进行截止频率大于2Hz低通滤波；

4)对低通滤波后的加速度取模值，考虑自由姿态下设备在行人行走过程中的运动特征，以加速度模值作为步数检测有效检测量，采用有限状态机的思想，进行适用于稳定自由姿态下状态判断阈值自适应更新的步态检测，得到待确认步态；

5)对步骤4)检测到的待确认步态进行步态确认，剔除误检步，最终确认是否行走了一步；

6)当检测到行走一步后，对当前步持续时间内的加速度进行PCA处理，处理对象为转换到导航坐标系水平面X、Y坐标轴的水平加速度，通过PCA处理的方法得到行进方向的运动轴方向；

7)采用基于加速度分布的统计分析方法对运动轴方向进行180°模糊判断，获得行进方向初步估计结果；

8)利用步骤2)滤波后的角速度信息和模糊判断置信度对行进方向初步估计结果进行校验修正；

9)通过对用户进行行走测试，估计行走步长；

10)在当前位置的基础上利用检测到的步数、方向、步长信息进行PDR定位，对位置进行迭代更新；

11)位置更新后继续步骤4)中的步态检测过程，直到行人停止行走。

2.根据权利要求1所述的一种基于多传感器融合和姿态解算的自由姿态PDR定位方法，其特征在于，步骤2)中，同频率采样的具体方法如下：

201)对加速度传感器、重力传感器、磁力计、陀螺仪以频率f进行基于软件的同频采样；

202)当传感器实际采样频率不同时，采用减采样的方法，选取采样频率最慢的传感器作为基准，选择与基准传感器采样时间戳最接近的其他传感器采样值作为此次采样的同序采样值，将非同序采样值删除以保证传感器采样频率相等。

3.根据权利要求2所述的一种基于多传感器融合和姿态解算的自由姿态PDR定位方法，其特征在于，步骤3)中，对采用数据进行预处理的具体方法如下：

301)利用重力和磁场计算载体坐标系和导航坐标系之间的旋转矩阵，已知A＝(A_x A_yA_z)^T为由三轴加速度组成的单位向量，E＝(E_x E_y E_z)^T为由三轴磁场强度组成的单位向量，令H＝E×A,M＝A×H，则由即可得到此刻的旋转矩阵利用将加速度和角速度转换到导航坐标系下，其中S_b为当前时刻传感器采样原始值，S_n为转换后的导航坐标系下的数值；

302)对导航坐标系下的加速度和角速度采用巴特沃兹滤波器进行低通滤波处理，其中采样频率f不小于50Hz，截止频率大于2Hz。

4.根据权利要求3所述的一种基于多传感器融合和姿态解算的自由姿态PDR定位方法，其特征在于，步骤4)中，步态检测的具体实现方法如下：

401)设定运动检测阈值Thr、波峰状态判断阈值Thr_pp、波谷状态判断阈值Thr_np、步态结束阈值Thr_neg，根据采样的加速度模值将人行走过程中采样点划分为静止状态S0、预备运动状态S1、进入峰值状态S2、离开峰值状态S3、进入谷值状态S4、离开谷值状态S6、噪声容忍状态S5、和步态结束状态S7；

402)波峰状态判断阈值、波谷状态判断阈值自适应更新公式为：

Thr_pp(i)＞＝Thr_np(i)+e (3)

其中，pp₀为波峰状态判断阈值初始值，Thr_pp(i)为第i步波峰状态判断阈值，max_i为第i步峰值状态中采样点的最大值，a＜1,b＜1；np₀为波谷状态判断阈值初始值，Thr_np(i)为第i步的波谷状态判断阈值，min_i为第i步谷值状态中采样点的最小值，c＞1,d＞1；e为大于等于0的常数，N₀为初始阈值作用步数，最小取4；

403)运动检测阈值Thr、步态结束阈值Thr_neg为常数，满足：

np₀＜Thr_neg＜g (4)

g＜Thr＜pp₀ (5)

其中g为重力常数；

404)初始状态从S0状态进入有限状态机，通过状态判断阈值来判断当前时刻状态，当进入步态结束状态S7时，判断为检测到一次待确认步；

405)在经过步骤5)进行步态确认后，若确为一步，则根据公式(1)～(5)自适应的进行下一步状态判断阈值的更新，并进行下一步的检测；若检测为不是真实一步，则不更新状态判断阈值，利用当前阈值继续进行步态检测。

5.根据权利要求4所述的一种基于多传感器融合和姿态解算的自由姿态PDR定位方法，其特征在于，步骤5)中，步态确认的具体实现方法如下：

501)在根据步骤4)中有限状态机进行步态检测过程中，对每一次加速度采样点的状态和采样时间进行标记，将第i步预备运动状态S1第一个点记为A_i，采样时间记为beginMoveTime(i)；将第i步步态结束状态S7点记为D_i，采样时间记为endStepTime(i)；将第i步进入峰值状态S2中的最大值点记为B_i，采样值记为max_i，采样时间记为maxValueTime(i)；将第i步进入谷值状态S4最小值点记为C_i，采样值记为min_i，采样时间记为minValueTime(i)；

502)通过步骤501)标记的时间和采样值，对检测到的待确认步进行步态确认，以剔除产生的多检步数，设置状态S1至S7持续时间最小值thr_time，和峰谷差最小值thr_value，当满足以下条件时：

endStepTime(i)-beginMoveTime(i)＞thr_time (6)

max_i-min_i＞thr_value (7)

则确定检测到一步，否则认为此待确认步态为误检步，不进行计步。

6.根据权利要求5所述的一种基于多传感器融合和姿态解算的自由姿态PDR定位方法，其特征在于，步骤6)中，估计运动轴方向的具体实现方法如下：

601)对第i步进行PCA分析的加速度采样区间为[beginTime(i),endStepTime(i)]，其中beginTime(i)＝max(endStepTime(i)-a×dt,beginMoveTime(i))，式中dt为一步平均持续时间；a为稍大于1的系数，根据步频结合实验测试估算a×dt取值；

602)对由步骤3)转换到导航坐标系下并进行低通滤波的水平面X、Y轴加速度信息进行PCA处理，得到最大特征值对应的特征向量 指示的方向就是行人行走的运动轴方向。

7.根据权利要求6所述的一种基于多传感器融合和姿态解算的自由姿态PDR定位方法，其特征在于，步骤7)中，180°模糊判断的具体实现方法如下：

701)根据步骤501)所标记的信息，以[beginMoveTime(i),maxValueTime(i)]为正向于行进方向的区间Rp，简称正向区间；以(maxValueTime(i),minValueTime(i)]为反向于行进方向区间Rn，简称反向区间；

702)将水平面由垂直于运动轴方向的过零点直线分为两个半平面，其中包含运动轴方向的半平面为Sp半平面，另一部分为Sn半平面；

703)对正向区间Rp和反向区间Rn内的水平加速度采样进行数量统计，若正向区间Rp内采样点分布在Sp半平面内，或反向区间Rn内采样点分布在Sn半平面内，标定运动方向为运动轴正向的系数p加1，否则标定运动方向为运动轴反向的系数n加1；

704)统计Rp、Rn两区间内的所有采样点，若p＞n，则此步的运动方向为运动轴方向，否则运动方向为运动轴反方向。

8.根据权利要求7所述的一种基于多传感器融合和姿态解算的自由姿态PDR定位方法，其特征在于，步骤8)中，方向校正过程的具体实现方法如下：

801)根据步骤7)中180°模糊判断时的运动轴正反向系数p、n得到模糊判断置信度，如下：

当时，则表示模糊判断结果完全随机不可信；当conf小于可信阈值thr_conf时，表示模糊判断结果可信度低；当conf越接近于1时，模糊判断可信度越高；

802)根据步骤3)中导航坐标系下滤波后的Z轴角速度计算第i-N步至第i步的方向相对变化量：

其中N为大于0的常数，当方向相对变化量ΔYaw(i-N,i)＜lowThr时，表示连续N步之内为直线行走，ΔYaw(i-N,i)＞upThr时，表示连续N步之内发生了转弯，其中gyro_z(j)表示第j次采样经过坐标系转换后的导航坐标系下Z轴的角速度，P为第i-N步到第i步的角速度采样区间，ΔT为两次连续采样的时间间隔，lowThr为直线行走方向相对变化判断阈值，upThr为转弯过程方向相对变化判断阈值；

803)一步模糊判断置信度conf小于可信阈值thr_conf时，表明模糊判断结果可信度较低，比较前N步中的模糊判断置信度，记录模糊判断置信度最大的步为第N_m步，以第N_m步的方向和方向变化量计算相对方向：

orien_R(i)＝orien(i-N_m)+ΔYaw(i-N_m,i) (10)

若运动轴方向在导航坐标系下的方向度数orien_M(i)与orien_R(i)之间方向夹角小于90°，则认为此时行进方向为运动轴方向，否则为运动轴反方向；

804)若连续两步的估计方向差比较小时，则对当前步和前一步原始方向求平均值作为当前步的估计方向；

805)当前状态为拐弯时，比较前N步中的模糊判断置信度，记录模糊判断置信度最大的步为第N_m步，以第N_m步的方向和方向变化量计算相对方向：

orien_R(i)＝orien(i-N_m)+ΔYaw(i-N_m,i) (10)

以orien_R(i)作为当前拐弯步的行进方向。

9.根据权利要求8所述的一种基于多传感器融合和姿态解算的自由姿态PDR定位方法，其特征在于，步骤9)中，步长估计的具体实现方法如下：

对行人进行训练，令行人沿直线行走一段距离，记录行走距离d和步数M，通过L＝d/M获得步长的估计结果。

10.根据权利要求9所述的一种基于多传感器融合和姿态解算的自由姿态PDR定位方法，其特征在于，步骤10)中，位置更新的具体实现方法如下：

已知初始点位置坐标(px₀,py₀)时，根据步数i、行进方向orien(i)、步长L，利用PDR位置更新公式，如下：

进行第i步PDR定位位置的计算更新。