CN105698795A

CN105698795A - 一种室内定位步长计算方法

Info

Publication number: CN105698795A
Application number: CN201510887154.1A
Authority: CN
Inventors: 刘昱; 李圣龙
Original assignee: Tianjin University
Current assignee: Tianjin University
Priority date: 2015-12-04
Filing date: 2015-12-04
Publication date: 2016-06-22

Abstract

本发明公开了一种室内定位步长计算方法，将惯性测量单元置于人员的腰部，人员在室内行走过程中，获得人员惯性传感器数据，包括加速度和四元数；用四元数生成坐标转换矩阵，把基于IMU载体坐标系的加速度转换到地理坐标系；根据地理坐标系下的三轴加速度，得到步态检测的三轴合加速度；处理合加速度数据，得到零速率修正点，完成步态检测，将人员连续的运动划分为每一个单步；利用倒立摆模型，计算人员每一个运动单步的距离，即单步步长，单步步长累加得到人员运动的总距离，得到运动步长。本发明有效地避免二次积分的误差累积，在不影响行人行走的情况下，有效地提高了室内定位步长计算的精度。

Description

一种室内定位步长计算方法

技术领域

本发明涉及室内定位技术领域，特别是涉及一种室内定位步长计算的方法。

背景技术

定位技术，简单的说就是获取物体的位置信息。伴随着社会的发展和科技的进步，人们在生活和工作中对定位和导航的需求不断增长。全球卫星定位系统很好地完成了室外空旷环境下的定位；而对于室内和卫星信号衰弱的环境，运用惯性导航技术，可以实现人员的定位和跟踪。

近年来，随着惯性技术和电子加工技术的发展，惯性测量单元(InertialMeasurementUnit，IMU)的性价比不断提高，使得基于惯性导航的室内定位方案快速发展。运用惯性导航进行室内定位的方案，主要包括步长计算、方向估计等技术。早期的步长计算技术都是将行人的每一个单步步长设置为常数，而行人行走的随机性和不确定性导致这种技术不适用在普通的步长计算中；随着技术发展，研究者根据步长与步频(步频与身高、体重、健康情况等存在很大的关系)的关系，提出基于步频的步长计算方法；根据牛顿定律：对行人运动的加速度进行二次积分，获得运动的距离，而由于IMU本身偏差、电磁干扰等因素，导致误差呈现平方增长，并且不断累积，在一分钟内就可以达到误差100米以上。由于对加速度直接积分误差很大，很多研究者提出利用现有的基础设施(如：WiFi、RFID、监视摄像头等)来修正积分误差，但由于依赖于定位环境的基础设施，因而很难在特殊环境(如：火灾现场、矿难现场等)使用。

发明内容

为了克服上述现有技术存在的问题，本发明提出了一种室内定位步长计算方法，采用基于倒立摆模型来计算人员在室内的运动步长的方法，有效降低误差的累积，提高步长计算的精度。

本发明提出了一种室内定位步长计算方法，该方法包括以下步骤：

步骤1、将惯性测量单元置于人员的腰部，人员在室内行走过程中，获得人员惯性传感器数据，包括加速度和四元数q₀、q₁、q₂、q₃，其中：

q₀＝cos(α/2)

q₁＝sin(α/2)cos(β_x)

q₂＝sin(α/2)cos(β_y)

q₃＝sin(α/2)cos(β_z)

α是绕旋转轴旋转的角度，cos(β_x)、cos(β_y)、cos(β_z)为旋转轴在方向的分量；

步骤2、利用四元数生成坐标转换矩阵，把基于IMU载体坐标系的加速度转换到地理坐标系：

R = [\begin{matrix} q_{0}^{2} + q_{1}^{2} - q_{2}^{2} - q_{3}^{2} & 2 (q_{1} q_{2} + q_{0} q_{3}) & 2 (q_{1} q_{3} - q_{0} q_{2}) \\ 2 (q_{1} q_{2} - q_{0} q_{3}) & q_{0}^{2} - q_{1}^{2} + q_{2}^{2} - q_{3}^{2} & 2 (q_{2} q_{3} + q_{0} q_{1}) \\ 2 (q_{1} q_{3} + q_{0} q_{2}) & 2 (q_{2} q_{3} - q_{0} q_{1}) & q_{0}^{2} - q_{1}^{2} - q_{2}^{2} + q_{3}^{2} \end{matrix}]

其中，R为坐标转换矩阵，q₀、q₁、q₂、q₃表示四元数；

步骤3、根据地理坐标系下的三轴加速度，得到步态检测的三轴合加速度：

A = \sqrt{a_{N}^{2} + a_{E}^{2} + a_{D}^{2}}

其中，A为三轴合加速度，a_N、a_E、a_D表示地理坐标系下三轴加速度；

步骤4、处理合加速度数据，选取合加速度中的极大值点，并且极大值点满足下列关系，即：相邻极大值点的时间间隔大于设定的阈值和极大值大于设定的阈值。阈值是根据经验设定的。从而得到零速率修正点：

\{\begin{matrix} Δ T > T_{T h} \\ A > A_{T h} \end{matrix}

其中，ΔT表示两个零速率修正点的时间间隔，T_Th和A_Th表示根据经验设定的临界值；

完成步态检测，将人员连续的运动划分为每一个单步；

步骤5、利用倒立摆模型，计算人员每一个运动单步的距离，即单步步长：

S_one＝S_sup+S_bip

其中，S_one表示单步步长，S_sup表示支撑脚时的位移值，S_bip表示双脚为支撑脚时的位移值；

S_{s u p} = 2 \sqrt{2 {ΔY}_{1} * L_{1} - {ΔY}_{1} * {ΔY}_{1}}

S_{b i p} = 2 \sqrt{2 {ΔY}_{2} * L_{2} - {ΔY}_{2} * {ΔY}_{2}}

ΔY₁＝L₁*(1-cosδ)

ΔY₂＝L₂*(1-cosδ)

其中，L₁表示单脚支撑时倒置单摆的摆长，取人体垂直站立时IMU在人体的携带点与左脚脚底的距离；L₂表示双脚为支撑脚时行进过程的单摆摆长；ΔY₁表示单脚为支撑脚时垂直方向上的最大位移值；ΔY₂表示双脚为支撑脚时垂直方向上的最小位移值；δ表示行进过程中单脚支撑与双脚支撑的分割临界点时刻支撑腿与垂直地面方向的夹角，是行进过程中腿向前迈出的最大角度值；

进而，单步步长累加得到人员运动的总距离，得到运动步长：

L_{s u m} = S_{0} + Σ_{n = 1}^{N} S_{x_n}

其中：L_sum为运动步长，S₀为初始距离，n为单步步数，S_{x_n}为第n个单步步长。

与现有技术中相比，本发明有效地避免二次积分的误差累积，在不影响行人行走的情况下，有效地提高了室内定位步长计算的精度。

附图说明

图1是本发明的室内定位步长计算方法的算法模型示意图；

图2是IMU固定位置模拟图；

图3是人员行走时腰部的运动轨迹模拟图。

具体实施方式

以下结合附图及具体实施方式，进一步详述本发明的技术方案。

本发明选用的惯性测量单元(IMU)为PNI公司生产的SPACEPOINTSCOUT，内置三轴加速度计、三轴陀螺仪以及三轴磁力计。将IMU放置在行人的腰部位置，如图2所示。鉴于腰部位于人体重心附近，在行进过程中其运动相较身体其他部位平稳，无较大幅度的左右摇摆或上下翻转等复杂动作，将IMU固定于腰部，安装简单且对人员的行进过程影响较小。

步骤1、行人自由行走时，可以采集行人运动的三轴加速度以及四元数。

步骤2、利用采集的四元数生成坐标转换矩阵，如下：

R = [\begin{matrix} q_{0}^{2} + q_{1}^{2} - q_{2}^{2} - q_{3}^{2} & 2 (q_{1} q_{2} + q_{0} q_{3}) & 2 (q_{1} q_{3} - q_{0} q_{2}) \\ 2 (q_{1} q_{2} - q_{0} q_{3}) & q_{0}^{2} - q_{1}^{2} + q_{2}^{2} - q_{3}^{2} & 2 (q_{2} q_{3} + q_{0} q_{1}) \\ 2 (q_{1} q_{3} + q_{0} q_{2}) & 2 (q_{2} q_{3} - q_{0} q_{1}) & q_{0}^{2} - q_{1}^{2} - q_{2}^{2} + q_{3}^{2} \end{matrix}]

运用坐标转换矩阵，可以将基于IMU载体坐标系的三轴加速度a_x、a_y、a_z转换到地理坐标系，得到地理坐标系的三轴加速度a_N、a_E、a_D，方法如下：

[\begin{matrix} a_{N} \\ a_{E} \\ a_{D} \end{matrix}] = R * [\begin{matrix} a_{x} \\ a_{y} \\ a_{z} \end{matrix}]

步骤3、运用地理坐标系下的三轴加速度a_N、a_E、a_D，计算用于步态检测的合加速度A：

A = \sqrt{a_{N}^{2} + a_{E}^{2} + a_{D}^{2}}

步骤4、运用合加速度A进行步态检测，得到零速率修正点，将行人连续的运动划分为每一个单步。

\{\begin{matrix} Δ T > T_{T h} \\ A > A_{T h} \end{matrix}

其中：ΔT表示两个零速率修正点的时间间隔，T_Th和A_Th表示根据经验设定的临界值。

步骤5、对每一个运动单步，使用倒立摆模型，计算出每一个单步的步长。

人员行走时腰部的运动轨迹，如图3所示。波浪形的实线为步行过程中，腰部的运动轨迹。S_sup表示行进过程中，单脚为支撑脚时人体前进的位移；S_bip表示双脚为支撑脚时人体前进的位移。则每一个单步的步长为：

S_one＝S_sup+S_bip

对于S_sup和S_bip，依据以下计算获得：

根据几何知识：

ΔY₁＝L₁*(1-cosδ)

ΔY₂＝L₂*(1-cosδ)

其中，L₁表示单脚支撑时倒置单摆的摆长，取人体垂直站立时IMU在人体的携带点与左脚脚底的距离(单位：米)；L₂表示双脚为支撑脚时行进过程的单摆摆长；ΔY₁表示单脚为支撑脚时垂直方向上的最大位移值；ΔY₂表示双脚为支撑脚时垂直方向上的最小位移值；δ表示行进过程中单脚支撑与双脚支撑的分割临界点时刻支撑腿与垂直地面方向的夹角，是行进过程中腿向前迈出的最大角度值；

结合上式：

S_{s u p} = 2 \sqrt{2 {ΔY}_{1} * L_{1} - {ΔY}_{1} * {ΔY}_{1}}

S_{b i p} = 2 \sqrt{2 {ΔY}_{2} * L_{2} - {ΔY}_{2} * {ΔY}_{2}}

将每一个单步的步长进行累加，进而计算出整个运动的步长：

L_{s u m} = S_{0} + Σ_{n = 1}^{N} S_{x_n}

有关本发明的一种室内定位步长计算的方法测试，描述如下：

(1)将IMU固定在腰部位置，参加实际环境里的行进测试。测试者熟悉了测试条件和方法之后，选择自己最舒服和最习惯的速度步行通过长度为47米的直线型光滑走廊。需首先测量每一名测试者脚底到腰部位置之间的距离(L₁值)以及单脚支撑与双脚支撑的分割临界点时刻支撑腿与垂直地面方向的夹角(δ值)。对于不同性别的测试者，参数L₂的取值不同。男性身体的重心位置到髋骨的垂直距离比女性要长一点，所以实验中对男性测试者取L₂为0.15m，女性测试者则取L₂为0.10m；

(2)如下表所示，每名测试者所进行的8组行进实验得到的，虽然不同的测试者在行走实验时，各自行走习惯的差异导致行进轨迹不一定完全符合倒立摆模型，存在着不同程度的左右摆动，但总位移基本处于42-52米之间。在实际行进长度为47米的情况下，所得结果符合实际情况。

实验者A

实验者B

实验者C

实验者D

实验者E

实验者F

第一次

39.5米

42.8米

43米

48.6米

49.4米

50.5米

第二次

42米

42.5米

46.7米

52.9米

50.2米

50.4米

第三次

44米

51.4米

47.2米

53.1米

50.9米

46.3米

第四次

46米

44.9米

52.5米

47.7米

53.6米

44.9米

第五次

43.8米

45米

51.5米

43.5米

52.8米

42.7米

第六次

47.2米

43.4米

53.4米

42.8米

47.3米

47.2米

第七次

46.4米

47.2米

52.3米

46.5米

50.4米

44.2米

第八次

49.2米

40米

48.8米

48.9米

46.8米

44.1米

Claims

1.一种室内定位步长计算方法，其特征在于，所述该方法包括以下步骤

q₀＝cos(α/2)

q₁＝sin(α/2)cos(β_x)

q₂＝sin(α/2)cos(β_y)

q₃＝sin(α/2)cos(β_z)

R = [\begin{matrix} q_{0}^{2} + q_{1}^{2} - q_{2}^{2} - q_{3}^{2} & 2 (q_{1} q_{2} + q_{0} q_{3}) & 2 (q_{1} q_{3} - q_{0} q_{2}) \\ 2 (q_{1} q_{2} - q_{0} q_{3}) & q_{0}^{2} - q_{1}^{2} + q_{2}^{2} - q_{3}^{2} & 2 (q_{2} q_{3} + q_{0} q_{1}) \\ 2 (q_{1} q_{3} + q_{0} q_{2}) & 2 (q_{2} q_{3} - q_{0} q_{1}) & q_{0}^{2} - q_{1}^{2} - q_{2}^{2} + q_{3}^{2} \end{matrix}]

其中，R为坐标转换矩阵，q₀、q₁、q₂、q₃表示四元数；

A = \sqrt{a_{N}^{2} + a_{E}^{2} + a_{D}^{2}}

步骤4、处理合加速度数据，选取合加速度中的极大值点，并且极大值点满足下列关系，即：相邻极大值点的时间间隔大于设定的阈值和极大值大于设定的阈值。阈值是根据经验设定的。从而得到零速率修正点。

\{\begin{matrix} Δ T > T_{T h} \\ A > A_{T h} \end{matrix}

完成步态检测，将人员连续的运动划分为每一个单步；

S_one＝S_sup+S_bip

S_{s u p} = 2 \sqrt{2 {ΔY}_{1} * L_{1} - {ΔY}_{1} * {ΔY}_{1}}

S_{b i p} = 2 \sqrt{2 {ΔY}_{2} * L_{2} - {ΔY}_{2} * {ΔY}_{2}}

ΔY₁＝L₁*(1-cosδ)

ΔY₂＝L₂*(1-cosδ)

L_{s u m} = S_{0} + Σ_{n = 1}^{N} S_{x_n}