专利名称：一种面向消防和救援的危险作业人员无盲区定位方法
技术领域：
本发明涉及一种人员跟踪定位技术，尤其涉及一种面向消防、救援等危险作业人员的无盲区三维定位和状态识别技术。
背景技术：
虽然随着社会和科技的进步，各种危险作业移动机器人的研究如火如荼日趋深入，应用前景广阔美好，但不可否认，无法预期如何久远的未来，消防、地震、矿难等突发事故的处置还是依赖作业人员深入事故现场进行处置和救援。在此过程中，保证作业人员的生命安全、保持战斗力是首要任务。因此在作业人员深入事故现场，全身心、全力以赴进行事故处置的过程，对其所处位置、作业状态进行远程监控和指挥无疑是非常必要、亟待展开，而且也是存在诸多技术问题需要攻克。关于定位系统，世界上发展较为成熟与精确的就是全球卫星定位系统(GPS，Global Positioning System), GPS利用多颗GPS卫星联合对持有接收器的节点协作定位，现在GPS定位已经可以达到很高的精度了。但是在某些特定的场景下，如在有遮挡的场合如建筑物内，茂密的森林等情况，GPS无法使用，这就需要利用盲区自定位的方法进行节点的自定位。目前国内危险作业人员还没有佩戴此类的定位和监控系统，人员的生命安全无法得到有效的保证。本发明是基于MEMS、GPS、3G、Virtual Reality以及智能控制等技术实现消防、救援人员在广域危险室外环境和复杂非结构化室内环境的全地域无盲区定位和状态监测(正常、跌倒、受困等)，从事实现对救援消防作业人员的远程监控、调度和安全保障。
发明内容
本发明的目的是，提供一种面向消防、救援等危险作业人员的无盲区定位方法。本发明的技术方案如下:一种面向消防和救援的危险作业人员无盲区定位方法，该方法在GPS信号有效时，利用GPS模块直接得到经纬度和高度，当GPS信号无效时，以GPS无效点为原点，进行盲区相对位置的定位，定位方法如下:第一步:将人的运动方式大体分为静止，跑，走，上楼，下楼五个动作，对加速度传感器采集的加速度信号进行软件滤波，然后采用滑动时间窗的方法进行加速度信号的特征提取，相邻的窗之间重叠半个窗长，先对窗口进行标准差处理，得到标准差#^;根据标准差#^的大小和表征每一步动作的波峰和波谷之间的采样点大小，进行动作识别；第二步:根据动作分类和加速度信号，再加上电子罗盘采集的方位角信息，进行相对位移的计算；第三步:将方位角信息和相对位移转化为相应的经纬度输出。作为优选实施方式，其中的第一步采用如下的方法进行动作识别:(I)设采样频率为l OOhz，将一个窗口长度为80个样本点的矩形窗滑过原始的加速度号，得到标准差>/ D(X)；(2)对于每一步的动作，应该有I个波谷和I个波峰，当#^ <250mg时，若每个波谷和波峰之间有15到30个采样点，且波峰和波谷的差值T大于200mg，则判断为“走”的动作；(3) ^VD(X) >250mg，每个波谷和波峰之间有8到20个采样点，且波峰和波谷的差值T大于500mg，则判断为“跑”的动作；(4)若^/5^ >250mg，每个波谷和波峰之间有20到40个采样点，且波峰和波谷的差值T大于400mg，据此判断为上下楼动作；(5)若判断为上楼或下楼动作时，结合气压传感器的采集数据区分下楼动作和上楼动作；(6)当一秒内没有任何动作符合上述的动作判定时，则判定为静止。本发明避免对加速度观测值积分，而基于行人步行的运动生理学特性，利用行走时加速度波形的周期性特征和统计值与行走速度相关的特点，直接估计行走步长，使其可以使低成本传感器也获得高精度的位置信息，并加入了对电子罗盘的修正方法使其输出方向更为准确，本发明更是采用gps和盲区定位相结合的方式，使其可以长时间准确的输出定位信息。
图1gps定位流 程图。图2盲区定位流程图。图3DR算法原理图。 图4动作分类流程图。图5盲区推估功能框图。
具体实施例方式下面结合附图和实施例对本发明进行说明。实现本发明的硬件包括:一个GPS模块，I块stm32芯片，一块51单片机，I个3轴数字加速度计ADXL345，I个3轴数字罗盘HMC5883，I个3轴数字陀螺仪L3G4200D，一个BMP085气压传感器。stm32芯片为主要功能元件，GPS模块，加速度计，气压传感器和51单片机都直接与其相连，它实时检测GPS信号。本发明在GPS信号有效时，利用GPS模块直接得到经纬度和高度，当GPS信号无效时，以GPS无效点为原点，进行盲区相对位置的定位。我们从中提取经纬度和高度信息(B，L, H) B为纬度，L为经度，H为高度，第6字段为定位质量指示，0为无效，I为有效，当有效时直接输出经纬度和高度，当数据无效时，盲区定位功能启动。功能框图如图1。软件系统采用uc/os-1I实时操作系统，定位方面主要有两个任务，任务一负责GPS数据的读取，当数据有效时直接输出，当数据无效时，任务一挂起，低于它优先级的任务2开始执行，任务2即盲区推估功能，盲区推估功能实现方法
I盲区推估定位当GPS数据无效时，盲区推估功能启动，获取相对位移，方位角，然后通过大地主题正解算法转化为相应的经纬度(B，L，H)输出，每走五步，进行一次转换，输出相应的经纬度(B，L，H)，具体流程图如图2所示。2，求相对位移盲区推估主要是运用传感器去估计其位移，本发明采取的是动作识别的方法进行盲区推估的，设GPS无效点为原点，采取行走航迹推算算法roR，即从一个已知坐标开始，通过传感器获取人在该位置的方向，速度等数据来推算行人在下一时刻出现未知的过程。如图3所示E、N分别表示东向、北向位置分量，(E0, N0)是行人在h时刻的初始位置，S。和0 Q分别表示行人从t0时刻至IJ 时刻从位置(E0, N0)行走到位置(E1, N1)的移动距离和绝对航向，则行人在h时刻的位置可表示为:E1=Ec^S0Sin 0 0N1=Nc^S0COS 0 0同理，行人在t2时刻的位置(E2，N2)可表示为:
「00351 \ E2 = E1+ sIsin0I = E0+ Si1=O Si Sinei (N2 = N1 + S1CosG1 = N0 + Ei=o Si cos Bi 根据上述原理，可获得行人在tk时刻位置的航位推算公式为:
「00371 f Ek = Ek-1 + Sk sin 0k = E0 + Efc Si sin Bi I Nk = Nk_i + Skcos0k = N0 + Eto1 Si cos 0;然而由于步行行为的多样性与多变性，使得DR在应用于行人导航系统时，其具体的实现形式略有变化。根据步行特点以及行人导航系统低成本、易佩戴的要求，行人的行走距离很难直接通过传感器直接测量，也不容易根据加速度积分的方式进行计算。因此，行人导航系统中主要采用步长获得相对位移量，首先将人的运动方式大体分为静止，跑，走，上楼，下楼五个动作，根据其每个动作的行走距离不同赋予不同的值再根据其行走的方向求出位移。3动作分类为了区分这五种动作，我们利用了加速度和气压2种传感器，加速度计原始输出虽然具有一定的周期性，但受噪音影响变化复杂，不易于直接进行计步计算，需对信号进行滤波，尽可能消除噪音影响。采用平均法进行滤波，属于软件滤波平均滤波法的公式为
/3—1
-——7’-’这样得出的加速度曲线较为平滑，五种特征的提取都是釆
Yj
戶用滑动时间窗的方法，将一个窗口长度为80个样本点的矩形窗滑过原始的加速
度信号，相邻的窗之间重叠半个窗长。因为加速度采样频率为lOOhz，所以一个窗长为0.8秒，对于加速度的判定，采用矢量和的形式a = jai +畤+ a|，这消除由于仪器佩戴不垂直所带来的误差，加速度传感器是以mg为单位的,静止时为IOOOmg就是Ig,
I)本发明采用基于时域的方法进行检测的，走和上下楼，跑最主要的区别是比较平稳，所以首先对窗口进行标准差处理，D(X) = Eli1fe^1E(X)为窗口平均数即为标准差，小于阈值的进入行走判定，否则归于另一类。2)对于一步行走应该有I个波谷一个波峰，将波峰和波谷的差值T作为阈值，每个波谷和波峰之间有15到30个采样点，大于阈值并且在采样点范围内则属于一步行走。3)当标准差判定时归于上下楼和跑步那一类时，每一步也有I个波谷一个波峰，将波峰和波谷的差值作T为阈值，但是跑步每个波谷和波峰之间有8到20个采样点，而上下楼每个波谷和波峰之间有20到40个采样点4)当第三步被判断为上下楼是，再用气压传感器进行判定，想将气压根据公式
(l Xalt为高度，p为当前大气压，P0为标准大气压，单位为厘米，
alt= 4433000 x VJ
当高度下降10厘米时判定为下楼，否则为上楼。5)当一秒内没有任何动作符合时，判定为静止。具体算法流程如图4所示。图中，Su为上楼步长，Hu上楼高度，Sd为下楼步长，Hd下楼高度，Sz为行走步长，Sp为跑步步长。4电子罗盘的校准51单片机主要进行电子罗盘校正于在电子罗盘方面采用HMC5883L三轴电子罗盘，这种电子罗盘有着一个和一般电子罗盘相同的缺点，那就是在校准之后，不能有任何倾斜，否则将会造成较大数值偏差，为克服这一缺点，采用卡尔曼滤波的方法进行修正，参见图5,具体措施如下:I构建方位角的状态方程；使用3轴数字陀螺仪L3G4200D，获得航向角方向的角加速度ax，那么状态方程为X(k)=X(k_l)+axkt+W(k) ;X(k)为k时刻的航向角，X (k_l)为k_l时刻的航向角，axk为k时刻的角加速度，t为陀螺仪每次读取数据的时间，W(k)和下面所说的V(k)分别为过程和测量的噪声，我们假定其恒定不变都为4度。2方位角的测量值由电子罗盘进行观测，Z (k)系统的观测量，V(k)为测量噪声，3首先预计k时刻的航向角X (k I k_l) =X (k_l | k_l) +axkt, X (k | k_l)是利用上一状态预测的结果，X(k-l|k-l)是上一状态最优的结果，匕为卡尔曼增益，X(k-llk-l)的协方差为 P (k-11 k-1)，X (k I k-1)的协方差为 P (k I k-1)，P (k I k-1) = (P (k-11 k-1) 2+42)1/2,Kg (K) = (P (k I k-1)2/ (P (k I k-1) 2+42))1/2，X (k I k) =X (k I k-1) +Kg (k) (Z (k) -X (k I k-1))，P (k I k)= ((l-Kg(k))*P(k|k-l)2)1/2.
这样就可以修正因倾斜引起的偏差，但是在人体行走过程中，也有航向角方面的晃动，比如，迈右腿身体自然就会往左边倾斜，而实际上还是属于直线行走，这样我们采取如下方法进行校正，下图所有参数初始值都为零。关于大地主题正解算法是已知一点经纬度，距离，方位角，求另一点的经纬度的方法，详见大地测量学中有关大地主题正反算中的正算。
权利要求
1.一种面向消防和救援的危险作业人员无盲区定位方法，该方法在GPS信号有效时，利用GPS模块直接得到经纬度和高度，当GPS信号无效时，以GPS无效点为原点，进行盲区相对位置的定位，定位方法如下: 第一步:将人的运动方式大体分为静止，跑，走，上楼，下楼五个动作，对加速度传感器采集的加速度信号进行软件滤波，然后采用滑动时间窗的方法进行加速度信号的特征提取，相邻的窗之间重叠半个窗长，先对窗口进行标准差处理，得到标准差#^:根据标准差#^的大小和表征每一步动作的波峰和波谷之间的采样点大小，进行动作识别； 第二步:根据动作分类和加速度信号，再加上电子罗盘采集的方位角信息，进行相对位移的计算； 第三步:将方位角信息和相对位移转化为相应的经纬度输出。
2.根据权利要求1所述的无盲区定位方法，其特征在于，其中的第一步采用如下的方法进行动作识别: (1)设采样频率为lOOhz，将一个窗口长度为80个样本点的矩形窗滑过原始的加速度Ih号，得到标准差D PO ； (2)对于每一步的动作，应该有I个波谷和I个波峰，当#^<25(tog时，若每个波谷和波峰之间有15到30个采样点，且波峰和波谷的差值T大于200mg，则判断为“走”的动作； (3)^VD(X) >250mg，每个波谷和波峰之间有8到20个采样点，且波峰和波谷的差值T大于500mg，则判断为“跑”的动作； (4)若#^>250mg，每个波谷和波峰之间有20到40个采样点，且波峰和波谷的差值T大于400mg，据此判断为上下 楼动作； (5)若判断为上楼或下楼动作时，结合气压传感器的采集数据区分下楼动作和上楼动作； (6)当一秒内没有任何动作符合上述的动作判定时，则判定为静止。
3.根据权利要求1所述的无盲区定位方法，其特征在于，第三步中，通过大地主题正解算法将方位角信息和相对位移转化为相应的经纬度输出。
全文摘要
本发明属于跟踪定位技术领域，涉及一种面向消防和救援的危险作业人员无盲区定位方法，该方法在GPS信号无效时，以GPS无效点为原点，进行盲区相对位置的定位，方法如下将人的运动方式大体分为静止，跑，走，上楼，下楼五个动作，对加速度传感器采集的加速度信号进行软件滤波，然后采用滑动时间窗的方法进行加速度信号的特征提取，相邻的窗之间重叠半个窗长，先对窗口进行标准差处理，得到标准差；根据标准差的大小和表征每一步动作的波峰和波谷之间的采样点大小，进行动作识别；再加上电子罗盘采集的方位角信息，进行相对位移的计算；将方位角信息和相对位移转化为相应的经纬度输出。本发明可以长时间准确的输出定位信息。
文档编号G01C21/10GK103234540SQ20131008964
公开日2013年8月7日 申请日期2013年3月20日 优先权日2013年3月20日
发明者丁承君, 李 根, 段萍, 申敏 申请人:河北工业大学