CN101907467B - 基于运动量测信息的个人定位方法及装置 - Google Patents

Abstract

本发明公开了一种基于运动量测信息的个人定位方法及装置，本发明通过对行动人员运动状态的量测，通过运动分类和运动模型来推算运动距离和方向，并最终确定行动人员的当前位置；同时它将运动模型和GPS结合起来，利用GPS来修正定位误差和模型参数误差。本发明克服了传统导航定位器在GPS信号缺失时无法定位的缺点，又具有较高的定位精度。本发明的装置具有轻巧便携，抗干扰性强，可靠性和精度较高的技术特点。

Description

基于运动量测信息的个人定位方法及装置

技术领域

本发明涉及导航与定位领域，尤其涉及一种基于运动量测信息的个人定位方法及装置。

背景技术

导航与定位技术是现代社会的重要支撑性基础技术之一。目前主要的定位技术按照原理分为如下几种：GPS定位；惯性导航定位(INS)、GPS/INS融合定位以及射频识别(RFID)定位等。它们各有优缺点。GPS定位是目前使用最广泛的定位方式。它的特点是定位精度较好，使用范围广，定位可靠。但缺点是存在着信号遮挡问题，在城市建筑内，林荫道等很多场合无法提供定位信息。惯性导航定位(INS)是根据惯性测量单元的数据，并采用时间积分的方法来获得轨迹，具有不受外界信号干扰，环境适应性强的优点。但缺点是具有误差累积效应。因此实用中往往需要将惯性导航定位和GPS定位融合在一起使用，以定期修正惯导的累积误差。然而精度较高的惯性测量传感器十分昂贵，低成本的惯性测量单元又误差较大，都无法直接在个人导航系统中应用。射频识别(RFID)技术主要适合室内环境，而且也需要预先布置好大量的射频定位设备才能工作，成本较高，目前尚处于实验阶段。

除了以上提及的定位技术之外，与本发明有一定相关性的是步数计数器。它通过检测人体垂直轴上的加速度变化脉冲来确定走步，并进行计数。但它只能计数，没有任何定位功能。

在个人定位的情况下，具有使用环境多变，运动方式多变的特点。前面提及的技术都无法完全满足要求。

发明内容

为了克服现有技术方法精度不高，可靠性和环境适应性差的问题，本发明提供了一种基于运动量测信息的个人导航定位方法和装置，本发明能有效实现使用者在各种环境下的可靠定位；具有携带便捷、使用方便、环境适应性好的特点。

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

一种基于运动量测信息的个人定位方法，该方法包括以下步骤：

(1)标定运动模型参数；

(2)接收来自传感器的数据；

(3)确定当前朝向以及实际运动方向和朝向的差角；

(4)确定当前的实际运动方向和步频；

(5)由步频根据运动模型确定对应的步长；

(6)由实际运动方向和步长推算当前位置。

(7)在能获得GPS信号时，利用GPS定位信息修正定位结果和对应运动模型参数。

一种用于基于运动量测信息的个人定位装置，它包括：运动传感装置、人工及GPS定位信息输入装置、信息处理和数据存储装置、定位导航结果输出装置。其中，所述运动传感装置、人工及GPS定位信息输入装置、定位导航结果输出装置分别与信息处理和数据存储装置相连。

本发明的有益效果是：本发明通过对行动人员运动状态的量测，通过运动分类和运动模型来推算运动距离与方向，并最终确定行动人员的当前位置。同时它将运动模型和GPS结合起来，利用GPS来修正定位误差和模型参数误差。克服了传统导航定位器在GPS信号缺失时无法定位的缺点，又具有较高的定位精度。该发明的装置具有轻巧便携，抗干扰性强，可靠性和精度较高的技术特点。

附图说明

图1是本发明的构成原理图；

图2是本发明的装置结构示意图；

图3是本发明的方法总体流程图。

具体实施方式

一、本发明的原理

如图1所示，本发明基于运动量测信息，结合人体运动模型的方法来计算行进距离，判断方向，确认地点高度，最后输出导航解决方案。本方法的原理包括以下部分：①传感信息输入部分：输出运动的三轴线加速度值与三轴角速度值，高度值变化值，当前朝向值，GPS定位值等；②运动分类及定位部分：接收传感信息输入部分输入的信息后，根据设定的步频阈值，判别运动类型，获得运动运动步长和运动方向值，并进行位置计算；③数据误差修正部分：为运动分类及定位部分提供模型和定位数据修正；④标定处理部分：为运动分类部分提供各种运动模型的模型参数和分类判别阈值。

1.传感器数据输入部分：该部分一般由如下传感器构成：三轴加速度和角速度计、三轴电子罗盘、高度计等。以佩戴于人体腰部的导航装置中心为原点，垂直向上为Z轴，水平向右和向前分别为X轴和Y轴建立人体坐标系。该部分输出数据为人体坐标系三个轴上的线性加速度和角速度，以及当前人体坐标系相对导航坐标系(正北)的朝向和当前高度。其中导航坐标系以某一已知位置为原点，正东和正北分别为X和Y轴，高度方向为Z轴建立，分别用X^nav、Y^nav和Z^nav表示。当有GPS信号时，也可以将该信号作为定位信息输出。当有现场的电子地图时，也可以在电子地图中人工输入当前位置作为导航起始点或定位点。

2.运动分类及定位部分：该部分包含两个部分：运动类别判定算法和位置推算算法。根据实际情况，将人体运动分为走路和跑步两个大类。人体生理学研究表明，人体运动时步频和运动速度存在着一定的关系。运动类别判定算法是采用了如下的一个人体运动模型：

$f=C_0{v}^{b_0}---\left(1\right)$

公式(1)中，f为步频，v为运动速度，C₀和b₀为两个待定的系数。设l为每步的步长，由步频f、速度v和步长l之间的相互关系有：

v＝l*f    (2)

结合公式(1)和公式(2)，可得：

$l={C_0}^{-1/b_0}{f}^{(1-b_0)/b_0}={\mathrm{Cf}}^b---\left(3\right)$

其中：

b＝(1-b₀)/b₀。

公式(3)就是所采用的运动模型，它代表了步长和步频的关系。当模型参数C和b通过标定确定后，由垂直轴测得的线性加速度脉冲周期可以获得步频f，从而由公式(3)可以得到步长l。

对于行走和跑步两种不同的运动类型，通过标定可以分别确定它们对应的模型参数，分别为(C_w，b_w)和(C_r，b_r)。如何区分两者是靠一个步频阈值f_t来确定的：

$f_t=\frac{{C_w}^{-\frac{1}{b_w+1}}{1.9444}^{\frac{1}{b_w+1}}+{C_r}^{-\frac{1}{b_r+1}}{1.9444}^{\frac{1}{b_r+1}}}{2}---\left(4\right)$

公式(4)中，1.9444m/s的速度是根据一般行走和跑步之间的速度来确定的，1.9444m/s相当于7km/h的速度。因此当检测到使用者当前的步频大于f_t时，就判断为跑步，而小于f_t时则判为走路。每种状态分别对应不同的运动模型参数。

当前的朝向ψ一般由电子罗盘传感器给出。陀螺仪给出的角速度经过时间积分后也能获得当前的朝向改变，然而该方法受角速度漂移的影响较大。在系统中我们将从陀螺仪获得的朝向改变只用于一至二个行走周期的短时间，用于和罗盘输出的角度改变进行比较。当两者的差别超过一个阈值时，判定为电子罗盘受周围铁器磁场的影响出现了方向偏移，此时以陀螺仪积分获得的角度变化量代替电子罗盘获得的角度变化量。

当人体向正前方运动时，运动方向就是当前朝向。而当人体后退、侧移时，则运动方向与朝向成一定角度。后退时，运动方向与当前朝向成180°差角；侧移时，运动方向与朝向成90°或-90°差角。在实际中，这个差角是通过检测两个水平轴上线性加速度峰值之比来获得的。设在本周期中人体坐标系中X和Y轴上的加速度值峰值分别为a_x和a_y，那么在X-Y平面内人体朝向与实际运动方向的差角θ为：

θ＝arctan(a_x/a_y)    (5)

由于加速度计误差等原因，所获得的差角θ在-180°～180°之间。可以确定如下四种情况：(a)若θ∈[-45°，45°]，则认为是向前行进，θ＝0；(b)若θ∈(45°～135°]，则认为是向右侧方行走，θ＝90°；(c)若θ∈(-45°～-135°]，则认为是向左侧方行走，θ＝-90°；(d)若θ∈(-135°，-180°)或者θ∈(135°，180°]，则认为是向后行进，θ＝180°。因此实际的运动方向由当前朝向ψ和差角θ的和得到。

当爬坡或者爬楼时，在X-Y平面上的实际有效步长会缩短，但同时会在Z方向上有位移。设坡度角为φ，则X-Y方向上和Z方向上的实际步长为：

l_x-y＝lcosφ，l_z＝lsinφ    (6)

爬坡状态较难依靠传感器信息确定，需要依赖电子地图或者人工确定。楼梯和上下坡的角度φ可以事先人工输入，也可以由电子地图获知。某一特定实现方案中，楼梯的坡度可以根据多数情况预先确定一个初始值。在没有任何坡度信息的情况下，可以直接以气压高度计的输出作为当前高度。

在运动分类与定位模块中，位置推算算法接收来自于运动类别判定算法输出的运动步长和运动方向信息，并进行累积推算。设当前为第K步，航向角(与正北方向夹角)为ψ，有：

x_k＝x_k-1+l_x-ysinψ

y_k＝y_k-1+l_x-ycosψ    (7)

z_k＝z_k-1+l_z

3.数据误差修正部分：该部分接收来自GPS定位或者人工定位的信息，为运动分类和定位部分提供可靠的导航结果和运动模型参数修正。该部分有两个功能：①定位误差修正；②运动模型参数修正。

当有GPS信息时，它无疑比基于纯运动模型的定位结果更加可靠，因此因该充分利用。当前GPS的定位结果和位置推算结果的差值可以作为定位误差修正值，直接修正当前的位置推算结果。

设由GPS获得的当前时刻定位信息与之前某时刻GPS的定位信息之差为ΔS_GPS，对应该时间段内来自于运动分类和定位模块获得的定位结果之差为ΔS_m，对应该段时间内总共行走了N步，可以求出运动类型判定算法中的步长修正值Δl。Δl可由公式(8)求出：

$\mathrm{\Δl}=\frac{\mathrm{\Δ}{S}_{\mathrm{GPS}}-\mathrm{\Δ}{S}_m}{N}---\left(8\right)$

因此修正后的步长为模型运动步长l和步长修正值Δl的和。只要有有效GPS信号就可以随时修正定位误差。而模型参数的修正只有在如下条件都满足时才进行：①当前存在有效GPS信号或者人工路标等外部定位辅助；②用于计算ΔS_GPS的这段时间中，步频变化很小，而且运动类别没有发生变化；③用于计算ΔS_GPS的这段时间中，使用者的运动轨迹是近似直线，即运动方向变化在一个很小范围之内；④用于计算ΔS_GPS的这段时间中，运动步数N大于某一个阈值N_t。否则模型参数修正值都设定为0。

4.标定处理模块部分：在运动模型中，每个不同使用者的参数(C_w，b_w)和(C_r，b_r)是不同的，在使用前需要通过标定过程来获得。在一段长度为L的路程中，使用者分别用慢走和快走两种方式走完全程，总步数和用时为(N₁，T₁)和(N₂，T₂)，由公式(3)可得：

$b_w=\frac{\lg (N_2/N_1)}{\lg (N_1{T}_2/N_2{T}_1)}$ (9)

$C_w=\frac{L/N_1}{{(N_1/T_1)}^{b_w}}=\frac{L/N_2}{{(N_2/T_2)}^{b_w}}$

同理，分别用慢跑和快跑两种方式完成全程L，总步数和用时分别为(N₁′，T₁′)和(N₂′，T₂′)，由公式(3)可得：

$b_r=\frac{\lg ({N_2}^{\′}/{N_1}^{\′})}{\lg ({N_1}^{\′}{T_2}^{\′}/{N_2}^{\′}{T_1}^{\′})}$ (10)

$C_r=\frac{L/{N_1}^{\′}}{{({N_1}^{\′}/{T_1}^{\′})}^{b_r}}=\frac{L/{N_2}^{\′}}{{({N_2}^{\′}/{T_2}^{\′})}^{b_r}}$

总长度L可以事先测得，也可以由在运动中直接由GPS等外部定位辅助获得。并由如下公式计算运动分类频率阈值：

$f_t=\frac{{C_w}^{-\frac{1}{b_w+1}}{1.9444}^{\frac{1}{b_w+1}}+{C_r}^{-\frac{1}{b_r+1}}{1.9444}^{\frac{1}{b_r+1}}}{2}---\left(11\right)$

经过标定后的运动模型参数(C_w，b_w)、(C_r，b_r)和频率阈值f_t都存放在装置的系统存储器中，作为该使用者的参数。也可以每个使用者在使用前根据自己的不同ID号调用属于自己的运动模型参数，以确保参数正确。

如图3所示，本发明基于运动量测信息的个人定位方法，包括以下步骤：

1、标定运动模型参数

在开始定位之前，先采用本发明所提出的运动参数标定方法进行运动模型参数标定：每个不同使用者的参数(C_w，b_w)和(C_r，b_r)是不同的。对步行模型参数标定得方法如下：在一段长度为L的路程中，使用者分别用慢走和快走两种方式走完全程，总步数和用时为(N₁，T₁)和(N₂，T₂)，由公式(9)可得出走路模型参数。同理，分别用慢跑和快跑两种方式完成全程L，总步数和用时分别为(N₁′，T₁′)和(N₂′，T₂′)，由公式(10)得出跑步模型参数。由公式(11)可得出运动分类频率阈值。经过标定后的运动模型参数(C_w，b_w)、(C_r，b_r)以及计算得到的运动分类步频阈值f_t都存放在装置的系统非易失性存储器中，作为该使用者的运动模型参数。也可以每个使用者在使用前根据自己的不同ID号调用属于自己的运动模型参数，以确保参数正确。

2、接收来自传感器的数据

传感器数据一般来自如下传感器：三轴加速度和角速度计，三轴电子罗盘，高度计等。以佩戴于人体腰部的导航装置中心为原点，垂直向上为Z轴，水平向右和向前分别为X轴和Y轴建立人体坐标系。该模块的输出数据为人体坐标系三个轴上的线性加速度和角速度，以及当前人体坐标系相对导航坐标系(正北)的朝向和当前高度。其中导航坐标系以某一已知位置为原点，正东和正北分别为X和Y轴，高度方向为Z轴建立，分别用X^nav、Y^nav和Z^nav表示。当有GPS信号时，也可以将该定位信号作为导航起始点。当有现场的电子地图时，也可以在电子地图中人工输入当前位置作为导航起始点或定位点。

3、确定当前朝向以及实际运动方向和朝向的差角

当前的朝向ψ由电子罗盘传感器给出。从陀螺仪获得的朝向改变只用于一至二个行走周期的短时间，用于和罗盘输出的角度改变进行比较。当两者的差别超过一个阈值时，判定为电子罗盘受周围铁器磁场的影响出现了方向偏移，此时以陀螺仪积分获得的角度变化量代替电子罗盘获得的角度变化量。

当人体向正前方运动时，运动方向就是当前朝向。而当人体后退、侧移时，则运动方向与朝向成一定角度。后退时，运动方向与当前朝向成180°差角；侧移时，运动方向与朝向成90°或-90°差角。在实际中，这个差角是通过检测两个水平轴上线性加速度峰值之比来获得的。设人体坐标系中X和Y轴上的加速度值峰值分别为a_x和a_y，那么在X-Y平面内人体朝向与实际运动方向的差角θ为：

θ＝arctan(a_x/a_y)

由于加速度计误差等原因，所获得的差角θ在-180°～180°之间。可以确定如下四种情况：(a)若θ∈[-45°，45°]，则认为是向前行进，θ＝0；(b)若θ∈(45°～135°]，则认为是向右侧方行走，θ＝90°；(c)若θ∈(-45°～-135°]，则认为是向左侧方行走，θ＝-90°；(d)若θ∈(-135°，-180°)或者θ∈(135°，180°]，则认为是向后行进，θ＝180°。

4、确定当前的实际运动方向和步频

实际的运动方向由当前朝向ψ和差角θ的和得到。由垂直轴测得的线性加速度脉冲周期可以获得步频f。

5、由步频根据运动模型确定对应的步长

对于行走和跑步两种不同的运动类型，它们对应的模型参数分别为(C_w，b_w)和(C_r，b_r)。如何区分两者是靠一个步频阈值f_t来确定的。因此当检测到使用者当前的步频大于f_t时，就判断为跑步，而小于或等于f_t时则判为走路。每种状态分别对应不同的运动模型参数。并由公式(3)确定对应的步长。

6、由实际运动方向和步长推算当前位置

当爬坡或者爬楼时，在X-Y平面上的实际有效步长会缩短，但同时会在Z方向上有位移。设坡度角为φ，则X-Y方向上和Z方向上的实际步长为：

l_x-y＝lcosφ，l_z＝lsinφ

爬坡状态较难依靠传感器信息本身确定，需要依赖电子地图或者人工确定。楼梯和上下坡的角度φ可以事先人工输入，也可以由电子地图获知。某一特定实现方案中，楼梯的坡度可以根据多数情况预先确定一个初始值。

位置推算算法接收来自于运动类别判定算法输出的运动步长和运动方向信息，并进行累积推算。设当前为第K步，航向角(与正北方向夹角)为ψ，则在导航坐标系下的坐标为：

$\left\{\begin{array}{c}{x}_k=x_{k-1}+l_{x-y}\sin \mathrm{\ψ}\\ y_k=y_{k-1}+l_{x-y}\cos \mathrm{\ψ}\\ z_k=z_{k-1}+l_z\end{array}\right.$

当走完下一步后，又重新执行步骤2)至6)。如此往复，由(x₀，y₀.z₀)开始，可以不断获得(x₁，y₁，z₁)，(x₂，y₂.z₂)，(x₃，y₃，z₃)…(x_n，y_n，z_n)，并最终得到当前定位和全部运动轨迹。

7、当有GPS信息或者人工定位信息输入时，执行定位修正以及运动模型参数修正量计算，否则，将所有校正量设为0，随后通过定位及运动模型参数修正步骤将修正量加到对应参量中去。

当前GPS的定位结果和位置推算结果的差值可以直接作为定位误差修正值。步长误差修正量则可由下面的方法计算。设由GPS获得的当前定位信息与之前某一次的定位信息之差为ΔS_GPS，对应该时间段内来自于运动分类和定位模块获得的定位结果之差为ΔS_m，对应该段时间内总共行走了N步，可以求出运动类型判定算法中的步长修正值Δl。Δl可由公式(8)求出：

$\mathrm{\Δl}=\frac{\mathrm{\Δ}{S}_{\mathrm{GPS}}-\mathrm{\Δ}{S}_m}{N}$

只要有有效GPS信号就可以随时修正定位误差。而模型参数的修正只有在如下条件都满足时才进行：①当前存在有效GPS信号或者人工路标等外部定位辅助；②用于计算ΔS_GPS的这段时间中，步频变化很小，而且运动类别没有发生变化；③用于计算ΔS_GPS的这段时间中，使用者的运动轨迹是近似直线，即运动方向变化在一个很小范围之内；④用于计算ΔS_GPS的这段时间中，运动步数N大于某一个阈值N_t。否则模型参数修正值都设定为0。

如图2所示，本发明用于基于运动量测信息的个人定位装置，它包括：运动传感装置、人工及GPS定位信息输入装置、信息处理和数据存储装置、定位导航结果输出装置。其中，所述运动传感装置、人工及GPS定位信息输入装置、定位导航结果输出装置分别与信息处理和数据存储装置相连。具体如下：

1、运动传感装置

运动传感装置包括：三轴加速度、三轴陀螺仪、三轴电子罗盘、气压高度计。其中，三轴加速度计提供运动加速度；三轴陀螺仪提供运动角速度；三轴电子罗盘提供航向和姿态角；气压高度计提供当前高度信息。运动传感器装置采集使用者的各种运动状态和姿态信息，提供给信息处理和数据存储装置。

2、人工及GPS定位信息输入装置

GPS信息可以是普通民用GPS信息，也可以是经过差分站获得的较高精度的DGPS定位信息。人工及GPS定位信息输入装置可以是键盘或者是触摸屏或者屏幕上显示的软键盘等。所有这些信息也输入到信息处理和数据存储装置中。

3、信息处理和数据存储装置

信息处理装置包括由嵌入式微处理器或者微控制器等构成的中央处理单元，它可以是ARM、MIPS或者DSP等嵌入式微处理器，也可以是低端的MCS-51单片机等微控制器。数据存储装置包括有内存或非易失性存储器件等构成的程序和数据存储空间。信息处理装置负责接收运动传感装置和人工及GPS定位信息输入装置的信息，处理得到导航定位结果以及当前运动状态和参数等信息。数据存储装置存放程序和数据，以及模型运动参数等。处理结果输出到定位导航结果输出装置中。

4、定位导航结果输出装置

该装置提供了处理过后的个人定位数据输出和显示的功能。它具有有多种方式输出：一种方式是通过标准接口如RS-232/RS-422/RS-485，或者USB等将个人的定位信息记录输出到相应的主设备如计算机；也可以有LCD屏幕输出，它可以将人行走的距离，方向，高度，实时反映在显示有电子地图的屏幕上，使使用者清晰了解自己所处位置与环境；也可以具有无线发射模块，提供将定位数据通过无线信号发射出去的功能，使指挥部实时了解所有当前应急响应人员的位置。并不是所有这些输出装置都是必须的，可以只具有其中的某一种或者某两种方式。

下面详细描述本发明的具体实施例，本发明的目的和效果将变得更加明显。

实施例1

一种基于运动量测信息的个人定位方法，包括以下步骤：

1)标定运动模型参数

在本例中，L＝100m，N₁＝196，T₁＝158.05s，N₂＝138，T₂＝60.98s，因此得：

$b_w=\frac{\lg (N_2/N_1)}{\lg (N_1{T}_2/N_2{T}_1)}=\frac{\lg (138/196)}{\lg [(196\×60.98)/(138\×158.05)]}=0.5833$

$C_w=\frac{L/N_1}{{(N_1/T_1)}^{b_w}}=\frac{100/196}{{(196/158.05)}^{0.5833}}=0.4500$

同理，分别用慢跑和快跑两种方式完成全程L，(N₁′，T₁′)和(N₂′，T₂′)分别为(100，39.28)和(75，23.45)，由公式(10)可得(C_r，b_r)。在本例中，计算可得：

$b_r=\frac{\lg ({N_2}^{\′}/{N_1}^{\′})}{\lg ({N_1}^{\′}{T_2}^{\′}/{N_2}^{\′}{T_1}^{\′})}=\frac{\lg (75/100)}{\lg [(100\×23.45)/(75\×39.28)]}=1.2609$

$C_r=\frac{L/{N_1}^{\′}}{{({N_1}^{\′}/{T_1}^{\′})}^{b_r}}=\frac{100/100}{{(100/39.28)}^{1.2609}}=0.3078$

接下来根据公式(4)计算运动分类步频阈值f_t。在本例中，

$f_t=\frac{{C_w}^{-\frac{1}{b_w+1}}{1.9444}^{\frac{1}{b_w+1}}+{C_r}^{-\frac{1}{b_r+1}}{1.9444}^{\frac{1}{b_r+1}}}{2}=2.3899$

经过标定后的运动模型参数(C_w，b_w)、(C_r，b_r)以及计算得到的运动分类步频阈值f_t都存放在装置的系统非易失性存储器中，作为该使用者的运动模型参数。也可以每个使用者在使用前根据自己的不同ID号调用属于自己的运动模型参数，以确保参数正确。

2)接收来自传感器的数据

在本例中，设起始点位置

为(100，33，0.6)，单位为米。

3)确定当前朝向以及实际运动方向和朝向的差角

在本例中，a_x＝0.06g，a_y＝0.15g，其中g为重力加速度。由公式(5)可以算得θ＝arctan(a_x/a_y)＝21.8°，显然θ∈[-45°，45°]，因此最后确定θ＝0。表明当时使用者在向前走。

4)确定当前的实际运动方向和步频

在本例中，当前朝向ψ＝25°，θ＝0，表明使用者在向北偏东25°方向运动。由垂直轴测得的线性加速度脉冲周期可以获得步频f，本例子中f＝2.149Hz。

5)由步频根据运动模型确定对应的步长

在本例中，f＝2.149Hz＜f_t＝2.3899Hz，因此判断为走路状态。而后根据公式(3)算出对应步长。本例中

米。

6)由实际运动方向和步长推算当前位置

在本例中，坡度角φ＝0，步长l_x-y＝l＝0.7031，l_z＝0，航向角ψ＝25°，因此有：

x₁＝x₀+l_x-ysinψ＝100+0.7031×sin25°＝100.2971

y₁＝y₀+l_x-ycosψ＝33+0.7031×cos25°＝33.6372

z₁＝z₀+l_z＝0.6+0＝0.6

当走完下一步后，又重新执行步骤2)至6)，并获得(x₂，y₂.z₂)。如此往复，可以不断获得(x₃，y₃，z₃)，(x₄，y₄，z₄)…(x_n，y_n，z_n)，并最终得到当前定位和全部运动轨迹。

7)当有GPS信息或者人工定位信息输入时，执行定位修正以及运动模型参数修正量计算步骤，否则，将所有校正量设为0。随后通过定位及运动模型参数修正步骤将修正量加到对应参量中去。

在本例中，如上条件都满足，N＝266＞N_t＝100，因此可以进行校正。在这段时刻中，设ΔS_GPS＝120.35m，ΔS_m＝115.78，因此有：

$\mathrm{\Δl}=\frac{\mathrm{\Δ}{S}_{\mathrm{GPS}}-\mathrm{\Δ}{S}_m}{N}=\frac{120.35-115.78}{266}=0.0172$

因此，校正后的步长为l＝l+Δl＝0.7031+0.0172＝0.7203米。

实施例2

一种用于基于运动量测信息的个人定位装置，它包括：

1)运动传感装置，包括：三轴加速度计提供运动加速度；三轴陀螺仪提供运动角速度；三轴电子罗盘提供航向和姿态角；气压高度计提供当前高度信息。并不是所有这些传感器都是必须的，在某些实现案例中，也可能没有陀螺仪，或者采用单轴的电子罗盘和陀螺仪等，也可能没有高度计。运动传感器装置采集使用者的各种运动状态和姿态信息，提供给息处理和数据存储装置。

2)人工及GPS定位信息输入装置，GPS定位信息输入装置可以是固定设计在系统处理板上，也可以由通讯接口通过线缆和系统相连接。GPS信息可以是普通民用GPS信息，也可以是经过差分站获得得较高精度的DGPS定位信息。人工信息输入装置可以是键盘或者是触摸屏或者屏幕上显示的软键盘等。所有这些信息也输入到息处理和数据存储装置中。

3)信息处理和数据存储装置，信息处理装置包括由嵌入式微处理器或者微控制器等构成的中央处理单元，它可以是ARM、MIPS或者DSP等嵌入式微处理器，也可以是低端的MCS-51单片机等微控制器。数据存储装置包括有内存或非易失性存储器件等构成的程序和数据存储空间。信息处理装置负责接收运动传感装置和人工及GPS定位信息输入装置的信息，运行由本发明所提出的方法设计的程序，得到导航定位结果以及当前运动状态和参数等信息。数据存储装置存放程序和数据，以及模型运动参数等。处理结果输出到定位导航结果输出装置中。

4)定位导航结果输出装置，输出电路与核心处理器相连，提供了处理过后的个人定位数据输出和显示的功能。它具有有多种方式输出：一种方式是通过标准接口如RS-232/RS-422/RS-485，或者USB等将个人的定位信息记录输出到相应的主设备如计算机；也可以有LCD屏幕输出，它可以将人行走的距离，方向，高度，实时反映在显示有电子地图的屏幕上，使使用者清晰了解自己所处位置与环境；也可以具有无线发射模块，提供将定位数据通过无线信号发射出去的功能，使指挥部实时了解所有当前应急响应人员的位置。并不是所有这些输出装置都是必须的，可以只具有其中的某一种或者某两种方式。

Claims (7)

1.一种基于运动量测信息的个人定位方法，其特征在于，该方法包括以下步骤：

(1)标定运动模型参数；

(2)接收来自传感器的数据；

(3)确定当前朝向以及实际运动方向和朝向的差角；

(4)确定当前的实际运动方向和步频；

(5)由步频根据运动模型确定对应的步长；

(6)由实际运动方向和步长推算当前位置；

(7)当有GPS信息或者人工定位信息输入时，执行定位修正，否则定位修正量为0；在同时满足一定特定条件时，进行运动模型参数修正量计算，否则模型参数修正值设定为0；通过定位及运动模型参数修正步骤将修正量加到对应系统参量中去；

其中，所述步骤(1)具体为：每个不同使用者的行走运动模型参数(C_w，b_w)和跑步运动模型参数(C_r，b_r)是不同的；对步行模型参数标定的方法如下：在一段长度为L的路程中，使用者分别用慢走和快走两种方式走完全程，总步数和用时为(N₁，T₁)和(N₂，T₂)，由下式可得出模型参数：

$b_w=\frac{\lg \left(N_2{N}_1\right)}{\lg (N_1{T}_2/N_2{T}_1)}$

$C_w=\frac{L/N_1}{{(N_1/T_1)}^{b_w}}=\frac{L/N_2}{{(N_2/T_2)}^{b_w}};$

对跑步运动模型参数标定的方法如下：分别用慢跑和快跑两种方式完成全程L，总步数和用时分别为(N₁′，T₁′)和(N₂′，T₂′)，由下式得出模型参数

$b_r=\frac{\lg ({N_2}^{\′}/{N_1}^{\′})}{\lg ({N_1}^{\′}{T_2}^{\′}/{N_2}^{\′}{T_1}^{\′})}$

$C_r=\frac{L/{N_1}^{\′}}{{({N_1}^{\′}/{T_1}^{\′})}^{b_r}}=\frac{L/{N_2}^{\′}}{{({N_2}^{\′}/{T_2}^{\′})}^{b_r}};$

由下式可得出运动分类频率阈值：

$f_t=\frac{{C_w}^{\frac{1}{b_w+1}}{1.9444}^{\frac{1}{b_w+1}}+{C_r}^{\frac{1}{b_r+1}}{1.9444}^{\frac{1}{b_r+1}}}{2}.$

2.根据权利要求1所述基于运动量测信息的个人定位方法，其特征在于，所述步骤(2)具体为：传感器数据来自如下传感器：三轴加速度、三轴陀螺仪、三轴电子罗盘、气压高度计；以佩戴于人体腰部的导航装置中心为原点，垂直向上为Z轴，水平向右和向前分别为X轴和Y轴建立人体坐标系；传感器数据为人体坐标系三个轴上的线性加速度和角速度，以及当前人体坐标系相对导航坐标系的朝向和当前高度；其中导航坐标系以某一已知位置为原点，正东和正北分别为X和Y轴，高度方向为Z轴建立，分别用X^nav、Y^nav和Z^nav表示；当有GPS信号时，将该定位信号作为导航起始点；当有现场的电子地图时，在电子地图中人工输入当前位置作为导航起始点或定位点。

3.根据权利要求1所述基于运动量测信息的个人定位方法，其特征在于，所述步骤(3)具体为：当前的朝向ψ由电子罗盘传感器给出；从陀螺仪获得的朝向改变只用于一至二个行走周期的短时间，用于和罗盘输出的角度改变进行比较；当两者的差别超过一个阈值时，判定为电子罗盘受周围铁器磁场的影响出现了方向偏移，此时以陀螺仪积分获得的角度变化量代替电子罗盘获得的角度变化量；当人体向正前方运动时，运动方向就是当前朝向；而当人体后退、侧移时，则运动方向与朝向成一定角度；后退时，运动方向与当前朝向成180°差角；侧移时，运动方向与朝向成90°或-90°差角；在实际中，这个差角是通过检测两个水平轴上线性加速度峰值之比来获得的；设人体坐标系中X和Y轴上的加速度值峰值分别为a_x和a_y，那么在X-Y平面内人体朝向与实际运动方向的差角θ为：

θ＝arctan(a_x/a_y)

差角θ在-180°～180°之间；确定如下四种情况：(a)若θ∈[-45°，45°]，则认为是向前行进，θ＝0；(b)若θ∈(45°～135°]，则认为是向右侧方行走，θ＝90°；(c)若θ∈(-45°～-135°]，则认为是向左侧方行走，θ＝-90°；(d)若θ∈(-135°，-180°)或者θ∈(135°，180°]，则认为是向后行进，θ＝180°。

4.根据权利要求1所述基于运动量测信息的个人定位方法，其特征在于，所述步骤(4)具体为：实际的运动方向由当前朝向ψ和差角θ的和得到，由垂直轴测得的线性加速度脉冲周期获得步频f。

5.根据权利要求1所述基于运动量测信息的个人定位方法，其特征在于，所述步骤(5)具体为：当检测到使用者当前的步频f大于步频阈值f_t时，就判断为跑步，由公式

确定步长；而使用者当前的步频f小于或等于步频阈值f_t时，则判为走路，由公式

确定步长。

6.根据权利要求1所述基于运动量测信息的个人定位方法，其特征在于，所述步骤(6)具体为：当爬坡或者爬楼时，在X-Y平面上的实际有效步长会缩短，但同时会在Z方向上有位移；设坡度角为φ，则X-Y方向上和Z方向上的实际步长为：

l_x-y＝lcosφ，l_z＝lsinφ

爬坡状态较难依靠传感器信息本身确定，需要依赖电子地图或者人工确定；楼梯和上下坡的角度φ由事先人工输入或由电子地图获知；某一特定实现方案中，楼梯的坡度根据多数情况预先确定一个初始值；

位置推算算法接收来自于运动类别判定算法输出的运动步长和运动方向信息，并进行累积推算；设当前为第K步，航向角为ψ，所述航向角为运动方向与正北方向夹角；则在导航坐标系下的坐标为：

x_k＝x_k-1+l_x-ysinψ

y_k＝y_k-1+l_x-y cosψ；

z_k＝z_k-1+l_z

当走完下一步后，又重新执行步骤2)至7)，并获得(x₂，y₂，z₂)；如此往复，不断获得(x₃，y₃，z₃)，(x₄，y₄，z₄)…(x_n，y_n，z_n)，并最终得到当前定位和全部运动轨迹。

7.根据权利要求1所述基于运动量测信息的个人定位方法，其特征在于，所述步骤(7)具体为：当前GPS的定位结果和位置推算结果的差值直接作为定位误差修正值；步长误差修正量则可由下面的方法计算：设由GPS获得的当前定位信息与之前某一次的定位信息之差为ΔS_GPS，对应该时间段内来自于运动分类和定位方法获得的定位结果之差为ΔS_m，对应该段时间内总共行走了N步，求出运动类型判定算法中的步长修正值Δl；Δl可由下式求出：

$\mathrm{\Δl}=\frac{{\mathrm{\ΔS}}_{\mathrm{GPS}}-{\mathrm{\ΔS}}_m}{N};$

只要有有效GPS信号就随时修正定位误差；而模型参数的修正只有在如下条件都满足时才进行：①当前存在有效GPS信号或者人工路标外部定位辅助；②用于计算ΔS_GPS的这段时间中，步频变化很小，而且运动类别没有发生变化；③用于计算ΔS_GPS的这段时间中，使用者的运动轨迹是近似直线，即运动方向变化在一个很小范围之内；④用于计算ΔS_GPS的这段时间中，运动步数N大于某一个阈值N_t；否则模型参数修正值都设定为0。

Images