专利名称：基于运动量测信息的个人定位方法及装置的制作方法
技术领域：
本发明涉及导航与定位领域，尤其涉及一种基于运动量测信息的个人定位方法及
直O
背景技术：
导航与定位技术是现代社会的重要支撑性基础技术之一。目前主要的定位技术 按照原理分为如下几种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轴 建立，分别用Xnav、Ynav和Znav表示。当有GPS信号时，也可以将该信号作为定位信息输出。当 有现场的电子地图时，也可以在电子地图中人工输入当前位置作为导航起始点或定位点。2.运动分类及定位部分该部分包含两个部分运动类别判定算法和位置推算算 法。根据实际情况，将人体运动分为走路和跑步两个大类。人体生理学研究表明，人体运动 时步频和运动速度存在着一定的关系。运动类别判定算法是采用了如下的一个人体运动模 型f = C0Vbo( 1 )公式(1)中，f为步频，ν为运动速度，C0和IDci为两个待定的系数。设1为每步的 步长，由步频f、速度ν和步长1之间的相互关系有ν = l*f (2)结合公式⑴和公式⑵，可得I = C;m°fWbo=Cfb(3)
6
其中: 公式(3)就是所采用的运动模型，它代表了步长和步频的关系。当模型参数C和b 通过标定确定后，由垂直轴测得的线性加速度脉冲周期可以获得步频f，从而由公式(3)可 以得到步长1。对于行走和跑步两种不同的运动类型，通过标定可以分别确定它们对应的模型参 数，分别为(cw，bw)和(Cpb》。如何区分两者是靠一个步频阈值&来确定的
( 4 )公式(4)中，1.9444m/s的速度是根据一般行走和跑步之间的速度来确定的， 1. 9444m/s相当于7km/h的速度。因此当检测到使用者当前的步频大于ft时，就判断为跑 步，而小于ft时则判为走路。每种状态分别对应不同的运动模型参数。当前的朝向Ψ —般由电子罗盘传感器给出。陀螺仪给出的角速度经过时间积分 后也能获得当前的朝向改变，然而该方法受角速度漂移的影响较大。在系统中我们将从陀 螺仪获得的朝向改变只用于一至二个行走周期的短时间，用于和罗盘输出的角度改变进行 比较。当两者的差别超过一个阈值时，判定为电子罗盘受周围铁器磁场的影响出现了方向 偏移，此时以陀螺仪积分获得的角度变化量代替电子罗盘获得的角度变化量。当人体向正前方运动时，运动方向就是当前朝向。而当人体后退、侧移时，则运动 方向与朝向成一定角度。后退时，运动方向与当前朝向成180°差角；侧移时，运动方向与 朝向成90°或-90°差角。在实际中，这个差角是通过检测两个水平轴上线性加速度峰值 之比来获得的。设在本周期中人体坐标系中X和Y轴上的加速度值峰值分别为ax和ay，那 么在X-Y平面内人体朝向与实际运动方向的差角θ为:θ = arctan (ax/ay) (5)由于加速度计误差等原因，所获得的差角θ在-180° 180°之间。可以确定如 下四种情况(a)若θ e [-45°，45° ]，则认为是向前行进，θ =0;(b)若θ e (45° 135° ]，则认为是向右侧方行走，θ =90° ；(c)若θ e (-45° -135° ]，则认为是向 左侧方行走，θ =-90° ； (d)若 θ e (-135°，-180° )或者 θ G (135°，180° ]，则认 为是向后行进，θ =180°。因此实际的运动方向由当前朝向Ψ和差角θ的和得到。当爬坡或者爬楼时，在X-Y平面上的实际有效步长会缩短，但同时会在Z方向上有 位移。设坡度角为Φ，则X-Y方向上和Z方向上的实际步长为lx_y = Icos Φ , Iz = Isin Φ (6)爬坡状态较难依靠传感器信息确定，需要依赖电子地图或者人工确定。楼梯和上 下坡的角度Φ可以事先人工输入，也可以由电子地图获知。某一特定实现方案中，楼梯的 坡度可以根据多数情况预先确定一个初始值。在没有任何坡度信息的情况下，可以直接以 气压高度计的输出作为当前高度。 在运动分类与定位�？橹校恢猛扑闼惴ń邮绽醋杂谠硕啾鹋卸ㄋ惴ㄊ涑龅脑� 动步长和运动方向信息，并进行累积推算。设当前为第K步，航向角(与正北方向夹角)为 ψ，有 Xk = Xk-AlrySinv
yk = y.^+l^cos Ψ (7)zk = Ζη+1ζ3.数据误差修正部分该部分接收来自GPS定位或者人工定位的信息，为运动分 类和定位部分提供可靠的导航结果和运动模型参数修正。该部分有两个功能①定位误差 修正；②运动模型参数修正。当有GPS信息时，它无疑比基于纯运动模型的定位结果更加可靠，因此因该充分 利用。当前GPS的定位结果和位置推算结果的差值可以作为定位误差修正值，直接修正当 前的位置推算结果。设由GPS获得的当前时刻定位信息与之前某时刻GPS的定位信息之差为Δ Sgps,对 应该时间段内来自于运动分类和定位�？榛竦玫亩ㄎ唤峁钗� Sm，对应该段时间内总 共行走了 N步，可以求出运动类型判定算法中的步长修正值ΔΙ。Δ 1可由公式(8)求出& = -A(8)
N因此修正后的步长为模型运动步长1和步长修正值Δ 1的和。只要有有效GPS信 号就可以随时修正定位误差。而模型参数的修正只有在如下条件都满足时才进行①当前 存在有效GPS信号或者人工路标等外部定位辅助；②用于计算ASepsW这段时间中，步频变 化很小，而且运动类别没有发生变化；③用于计算ASeps的这段时间中，使用者的运动轨迹 是近似直线，即运动方向变化在一个很小范围之内；④用于计算ASeps的这段时间中，运动 步数N大于某一个阈值Nt。否则模型参数修正值都设定为0。4.标定处理�？椴糠衷谠硕Ｐ椭校扛霾煌褂谜叩牟问�(Cw，bw)和((；，br) 是不同的，在使用前需要通过标定过程来获得。在一段长度为L的路程中，使用者分别用慢 走和快走两种方式走完全程，总步数和用时为(N1, T1)和(N2, T2)，由公式(3)可得
, Ig(JV2ZiV1)K= 二二、(9)
lg(i^r2 /N2T1) Cw =
—LIN1 _ LIN2 ~ (NlZTl)b^ = (N2ZT2)b^同理，分别用慢跑和快跑两种方式完成全程L，总步数和用时分别为(N/，T1 和(N2'，T2')，由公式(3)可得IlgwT2V^T1,) (10) Cr —.…ιιηη ,、 ~ (-/.τ t/T ,、br
ZZiV1' 一 LIN2' {Νλ Τλ')κ ~ (N2 T21Y 总长度L可以事先测得，也可以由在运动中直接由GPS等外部定位辅助获得。并 由如下公式计算运动分类频率阈值, _ Cw ft-+11.9444^+1 +Cr ^+11.9444^+1
Λ= 2 (11；经过标定后的运动模型参数(Cw，bw)、(Cr, br)和频率阈值ft都存放在装置的系统 存储器中，作为该使用者的参数。也可以每个使用者在使用前根据自己的不同ID号调用属 于自己的运动模型参数，以确保参数正确。
8
如图3所示，本发明基于运动量测信息的个人定位方法，包括以下步骤1、标定运动模型参数在开始定位之前，先采用本发明所提出的运动参数标定方法进行运动模型参数标 定每个不同使用者的参数(Cw，bw)和((；，br)是不同的。对步行模型参数标定得方法如 下在一段长度为L的路程中，使用者分别用慢走和快走两种方式走完全程，总步数和用时 为(N1J1)和(N2，T2)，由公式(9)可得出走路模型参数。同理，分别用慢跑和快跑两种方式 完成全程L，总步数和用时分别为(N/，T1')和(N2'，T2')，由公式(10)得出跑步模型 参数。由公式(11)可得出运动分类频率阈值。经过标定后的运动模型参数(Cw，bw)、(Cr, br)以及计算得到的运动分类步频阈值ft都存放在装置的系统非易失性存储器中，作为该 使用者的运动模型参数。也可以每个使用者在使用前根据自己的不同ID号调用属于自己 的运动模型参数，以确保参数正确。2、接收来自传感器的数据传感器数据一般来自如下传感器三轴加速度和角速度计，三轴电子罗盘，高度计 等。以佩戴于人体腰部的导航装置中心为原点，垂直向上为Z轴，水平向右和向前分别为X 轴和Y轴建立人体坐标系。该模块的输出数据为人体坐标系三个轴上的线性加速度和角速 度，以及当前人体坐标系相对导航坐标系(正北)的朝向和当前高度。其中导航坐标系以 某一已知位置为原点，正东和正北分别为X和Y轴，高度方向为Z轴建立，分别用Xnav、Ynav 和Znav表示。当有GPS信号时，也可以将该定位信号作为导航起始点。当有现场的电子地 图时，也可以在电子地图中人工输入当前位置作为导航起始点或定位点。3、确定当前朝向以及实际运动方向和朝向的差角当前的朝向Ψ由电子罗盘传感器给出。从陀螺仪获得的朝向改变只用于一至二 个行走周期的短时间，用于和罗盘输出的角度改变进行比较。当两者的差别超过一个阈值 时，判定为电子罗盘受周围铁器磁场的影响出现了方向偏移，此时以陀螺仪积分获得的角 度变化量代替电子罗盘获得的角度变化量。当人体向正前方运动时，运动方向就是当前朝向。而当人体后退、侧移时，则运动 方向与朝向成一定角度。后退时，运动方向与当前朝向成180°差角；侧移时，运动方向与 朝向成90°或-90°差角。在实际中，这个差角是通过检测两个水平轴上线性加速度峰值 之比来获得的。设人体坐标系中X和Y轴上的加速度值峰值分别为ax和ay，那么在X-Y平 面内人体朝向与实际运动方向的差角θ为θ = arctan (ax/ay)由于加速度计误差等原因，所获得的差角θ在-180° 180°之间。可以确定如 下四种情况(a)若θ e [-45°，45° ]，则认为是向前行进，θ =0;(b)若θ e (45° 135° ]，则认为是向右侧方行走，θ =90° ；(c)若θ e (-45° -135° ]，则认为是向 左侧方行走，θ =-90° ； (d)若 θ e (-135°，-180° )或者 θ G (135°，180° ]，则认 为是向后行进，θ = 180°。4、确定当前的实际运动方向和步频实际的运动方向由当前朝向Ψ和差角θ的和得到。由垂直轴测得的线性加速度 脉冲周期可以获得步频f。5、由步频根据运动模型确定对应的步长
对于行走和跑步两种不同的运动类型，它们对应的模型参数分别为(Cw，bw)和 (Cr, br) 0如何区分两者是靠一个步频阈值ft来确定的。因此当检测到使用者当前的步频 大于ft时，就判断为跑步，而小于或等于ft时则判为走路。每种状态分别对应不同的运动 模型参数。并由公式(3)确定对应的步长。6、由实际运动方向和步长推算当前位置当爬坡或者爬楼时，在X-Y平面上的实际有效步长会缩短，但同时会在Z方向上有 位移。设坡度角为Φ，则X-Y方向上和Z方向上的实际步长为lx_y = Icos Φ , Iz = Isin Φ爬坡状态较难依靠传感器信息本身确定，需要依赖电子地图或者人工确定。楼梯 和上下坡的角度Φ可以事先人工输入，也可以由电子地图获知。某一特定实现方案中，楼 梯的坡度可以根据多数情况预先确定一个初始值。位置推算算法接收来自于运动类别判定算法输出的运动步长和运动方向信息，并 进行累积推算。设当前为第K步，航向角(与正北方向夹角)为Ψ，则在导航坐标系下的坐 标为
‘Xk= Xk-i+lx-y\yk=y+LyCOSl//
. W1H当走完下一步后，又重新执行步骤2)至6)。如此往复，由(Χ(1，％. Z0)开始，可以不 断获得(X1, I1, Z1)，(x2, y2. Z2)，(x3, y3，z3)…(xn，yn，zn)，并最终得到当前定位和全部运动 轨迹。7、当有GPS信息或者人工定位信息输入时，执行定位修正以及运动模型参数修正 量计算，否则，将所有校正量设为0，随后通过定位及运动模型参数修正步骤将修正量加到 对应参量中去。当前GPS的定位结果和位置推算结果的差值可以直接作为定位误差修正值。步长 误差修正量则可由下面的方法计算。设由GPS获得的当前定位信息与之前某一次的定位信 息之差为ASeps，对应该时间段内来自于运动分类和定位�？榛竦玫亩ㄎ唤峁钗� Sm， 对应该段时间内总共行走了 N步，可以求出运动类型判定算法中的步长修正值Δ 1。Δ1可 由公式⑶求出Al=dsOPS-^m
N只要有有效GPS信号就可以随时修正定位误差。而模型参数的修正只有在如下条 件都满足时才进行①当前存在有效GPS信号或者人工路标等外部定位辅助；②用于计算 ASgps的这段时间中，步频变化很�。以硕啾鹈挥蟹⑸浠虎塾糜诩扑鉇Seps的这 段时间中，使用者的运动轨迹是近似直线，即运动方向变化在一个很小范围之内；④用于计 算ASeps的这段时间中，运动步数N大于某一个阈值Nt。否则模型参数修正值都设定为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, N1 = 196，T1 = 158. 05s, N2 = 138，T2 = 60. 98s,因此得 b _ ^(N2ZN1) _lg(138/196)Q5g33
w IgCiV1T2/TV2^) lg[(l 96 X 60.98) /(138 χ 158.05)]. LfNl ^ 100/196 (N1ZTl)b^ ~ (196/158.05)°
同理，分别用慢跑和快跑两种方式完成全程L，(N/，T1')和(N2'，T2')分别 Cw =-1T- =-r-j^rr = 0.4500
Lw 」 w …λ* ζ οα/1 η 、0.5833
为(100,39. 28)和(75，23. 45)，由公式(10)可得(Cr，br)。在本例中，计算可得 br= WW) =_MW_= 1.2609
IgCiV1T2ViV2T1') lg[(l 00 X 23.45)/(75 X 39.28)]
LZN1' _ 100/100 (N1'/T1')"- ~ (100/39.28)1 Cr =….二、=________…細=0.3078
11
接下来根据公式(4)计算运动分类步频阈值ft。在本例中，f _.. Cw ^+11.9444ft-+1 +Cr ^L9444^1 _ . ^99
2 一 .经过标定后的运动模型参数(Cw，bw)、(Cr, br)以及计算得到的运动分类步频阈值 ft都存放在装置的系统非易失性存储器中，作为该使用者的运动模型参数。也可以每个使 用者在使用前根据自己的不同ID号调用属于自己的运动模型参数，以确保参数正确。2)接收来自传感器的数据在本例中，设起始点位置μΤν， Γν，ΖΓ")为(100，33，0. 6)，单位为米。3)确定当前朝向以及实际运动方向和朝向的差角在本例中，ax = 0. 06g, ay = 0. 15g，其中g为重力加速度。由公式(5)可以算得 θ = arctan(ax/ay) =21.8°，显然 θ G [-45°，45° ]，因此最后确定 θ =0。表明当 时使用者在向前走。4)确定当前的实际运动方向和步频在本例中，当前朝向ψ =25°，θ = 0，表明使用者在向北偏东25°方向运动。 由垂直轴测得的线性加速度脉冲周期可以获得步频f，本例子中f = 2. 149Hz。5)由步频根据运动模型确定对应的步长在本例中，f = 2. 149Hz < ft = 2. 3899Hz,因此判断为走路状态。而后根据公式 ⑶算出对应步长。本例中/ = Cw/" =0.45χ2.149。·5833 = 0.7031米。6)由实际运动方向和步长推算当前位置在本例中，坡度角φ = 0，步长lx_y = 1 = 0. 7031，Iz = 0，航向角Ψ = 25°，因 此有X1 = x0+lx_ysin￥ = 100+0. 7031 X sin25° = 100.2971yi = y0+lx_ycos￥ = 33+0. 7031Xcos25° = 33.6372Z1 = z0+lz = 0. 6+0 = 0. 6当走完下一步后，又重新执行步骤2)至6)，并获得(x2，y2.z2)。如此往复，可以不 断获得(X3，y3，Z3)，(x4, y4, Z4)…(xn，yn, zn)，并最终得到当前定位和全部运动轨迹。7)当有GPS信息或者人工定位信息输入时，执行定位修正以及运动模型参数修正 量计算步骤，否则，将所有校正量设为0。随后通过定位及运动模型参数修正步骤将修正量 加到对应参量中去。在本例中，如上条件都满足，N = 266 > Nt = 100，因此可以进行校正。在这段时 刻中，设 ASgps = 120. 35m，Δ Sm = 115. 78，因此有
「 A7 ASops-ASm 120.35-115.78Al = ~—-- =-= 0.0172
N266因此，校正后的步长为1 = 1+Δ 1 = 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屏幕输出，它可以将人行走的距离，方向，高度，实时反映在显示有电子地 图的屏幕上，使使用者清晰了解自己所处位置与环境；也可以具有无线发射模块，提供将定 位数据通过无线信号发射出去的功能，使指挥部实时了解所有当前应急响应人员的位置。 并不是所有这些输出装置都是必须的，可以只具有其中的某一种或者某两种方式。
1权利要求
一种基于运动量测信息的个人定位方法，其特征在于，该方法包括以下步骤(1)标定运动模型参数。(2)接收来自传感器的数据。(3)确定当前朝向以及实际运动方向和朝向的差角。(4)确定当前的实际运动方向和步频。(5)由步频根据运动模型确定对应的步长。(6)由实际运动方向和步长推算当前位置。(7)当有GPS信息或者人工定位信息输入时，可以执行定位修正，否则定位修正量为0。在同时满足一定特定条件时，还可以进行运动模型参数修正量计算，否则模型参数修正值设定为0。通过定位及运动模型参数修正步骤将修正量加到对应系统参量中去。
2.根据权利要求1所述基于运动量测信息的个人定位方法，其特征在于，所述步骤(1) 具体为每个不同使用者的行走运动模型参数(Cw，bw)和跑步运动模型参数(C,，b,)是不同 的。对步行模型参数标定的方法如下在一段长度为L的路程中，使用者分别用慢走和快走 两种方式走完全程，总步数和用时为(N1, T1)和(N2，T2)，由下式可得出模型参数 对跑步运动模型参数标定的方法如下分别用慢跑和快跑两种方式完成全程L，总步 数和用时分别为(N/，T1')和(N2'，T2')，由下式得出模型参数 由下式可得出运动分类频率阈值
3.根据权利要求1所述基于运动量测信息的个人定位方法，其特征在于，所述步骤(2) 具体为传感器数据来自如下传感器三轴加速度、三轴陀螺仪、三轴电子罗盘、气压高度 计。以佩戴于人体腰部的导航装置中心为原点，垂直向上为Z轴，水平向右和向前分别为X 轴和Y轴建立人体坐标系。传感器数据为人体坐标系三个轴上的线性加速度和角速度，以 及当前人体坐标系相对导航坐标系的朝向和当前高度。其中导航坐标系以某一已知位置为 原点，正东和正北分别为X和Y轴，高度方向为Z轴建立，分别用Xnav、Ynav和Znav表示。当有 GPS信号时，也可以将该定位信号作为导航起始点。当有现场的电子地图时，也可以在电子 地图中人工输入当前位置作为导航起始点或定位点。
4.根据权利要求1所述基于运动量测信息的个人定位方法，其特征在于，所述步骤(3) 具体为当前的朝向Ψ由电子罗盘传感器给出。从陀螺仪获得的朝向改变只用于一至二个 行走周期的短时间，用于和罗盘输出的角度改变进行比较。当两者的差别超过一个阈值时，判定为电子罗盘受周围铁器磁场的影响出现了方向偏移，此时以陀螺仪积分获得的角度变 化量代替电子罗盘获得的角度变化量。当人体向正前方运动时，运动方向就是当前朝向。而 当人体后退、侧移时，则运动方向与朝向成一定角度。后退时，运动方向与当前朝向成180° 差角；侧移时，运动方向与朝向成90°或-90°差角。在实际中，这个差角是通过检测两个 水平轴上线性加速度峰值之比来获得的。设人体坐标系中X和Y轴上的加速度值峰值分别 为\和 ，那么在X-Y平面内人体朝向与实际运动方向的差角θ为:θ = arctan (ax/ay)差角θ在-180° 180°之间。可以确定如下四种情况(a)若θ e [-45°， 45° ]，则认为是向前行进，θ =0;(b)若θ e (45° 135° ]，则认为是向右侧方行走， θ =90° ；(c)若θ e (-45° -135° ]，则认为是向左侧方行走，θ =-90° ； (d)若 θ e (-135°，-180° )或者 θ G (135°，180° ]，则认为是向后行进，θ = 180°。
5.根据权利要求1所述基于运动量测信息的个人定位方法，其特征在于，所述步骤(4) 具体为实际的运动方向由当前朝向Ψ和差角θ的和得到，由垂直轴测得的线性加速度脉 冲周期获得步频f。
6.根据权利要求1所述基于运动量测信息的个人定位方法，其特征在于，所述步骤(5) 具体为当检测到使用者当前的步频f大于步频阈值ft时，就判断为跑步，由公式/ = CJh' 确定步长。而使用者当前的步频f小于或等于步频阈值ft时，则判为走路，由公式/ = CJk 确定步长。
7.根据权利要求1所述基于运动量测信息的个人定位方法，其特征在于，所述步骤(6) 具体为当爬坡或者爬楼时，在X-Y平面上的实际有效步长会缩短，但同时会在Z方向上有 位移。设坡度角为Φ，则X-Y方向上和Z方向上的实际步长为lx_y = Icos Φ , Iz = Isin Φ爬坡状态较难依靠传感器信息本身确定，需要依赖电子地图或者人工确定。楼梯和上 下坡的角度Φ可以事先人工输入，也可以由电子地图获知。某一特定实现方案中，楼梯的 坡度可以根据多数情况预先确定一个初始值。位置推算算法接收来自于运动类别判定算法输出的运动步长和运动方向信息，并进行 累积推算。设当前为第K步，航向角(与正北方向夹角)为Ψ，则在导航坐标系下的坐标 为Xk = Xk-i+lx-ysin￥Yk = Yk-1 + lx-yCOS ΨZk — Zk-l + lz当走完下一步后，又重新执行步骤2)至7)，并获得(x2，y2.z2)。如此往复，可以不断获 得(x3，y3，z3)，(x4, y4，z4)…(xn，yn，zn)，并最终得到当前定位和全部运动轨迹。
8.根据权利要求1所述基于运动量测信息的个人定位方法，其特征在于，所述步骤(7) 具体为当前GPS的定位结果和位置推算结果的差值可以直接作为定位误差修正值。步长 误差修正量则可由下面的方法计算设由GPS获得的当前定位信息与之前某一次的定位信 息之差为ASeps，对应该时间段内来自于运动分类和定位方法获得的定位结果之差为Δ Sm， 对应该段时间内总共行走了 N步，可以求出运动类型判定算法中的步长修正值Δ 1。Δ1可 由下式求出AI ^ASgps-ASm N只要有有效GPS信号就可以随时修正定位误差。而模型参数的修正只有在如下条件都 满足时才进行①当前存在有效GPS信号或者人工路标等外部定位辅助；②用于计算ASeps 的这段时间中，步频变化很�。以硕啾鹈挥蟹⑸浠虎塾糜诩扑鉇Seps的这段时 间中，使用者的运动轨迹是近似直线，即运动方向变化在一个很小范围之内；④用于计算 ASgps的这段时间中，运动步数N大于某一个阈值Nt。否则模型参数修正值都设定为0。
9. 一种用于基于运动量测信息的个人定位装置，其特征在于，它包括运动传感装置、 人工及GPS定位信息输入装置、信息处理和数据存储装置、定位导航结果输出装置。其中， 所述运动传感装置、人工及GPS定位信息输入装置、定位导航结果输出装置分别与信息处 理和数据存储装置相连。
全文摘要
本发明公开了一种基于运动量测信息的个人定位方法及装置，本发明通过对行动人员运动状态的量测，通过运动分类和运动模型来推算运动距离和方向，并最终确定行动人员的当前位置；同时它将运动模型和GPS结合起来，利用GPS来修正定位误差和模型参数误差。本发明克服了传统导航定位器在GPS信号缺失时无法定位的缺点，又具有较高的定位精度。本发明的装置具有轻巧便携，抗干扰性强，可靠性和精度较高的技术特点。
文档编号G01C21/04GK101907467SQ201010248929
公开日2010年12月8日 申请日期2010年8月6日 优先权日2010年8月6日
发明者刘济林, 王嘉锋, 项志宇 申请人:浙江大学
专业数控铣床产品供应商
20年生产经验，实力工厂，精工品质，质量有保障
