一种基于新型多传感器融合技术的行人导航装置和方法

一种基于新型多传感器融合技术的行人导航装置和方法
【专利摘要】本发明公开了一种基于新型多传感器融合技术的行人导航装置和方法，装置包括手持智能设备平台、观测量处理单元和融合滤波器；方法包括如下步骤：(1)手持智能设备利用自身硬件获取IMU、磁力计、压力计、WiFi、BLE和GNSS等的原始数据；(2)观测量处理单元处理手持智能设备提供的原始数据以提供位置或速度等观测量给融合滤波器；(3)融合滤波器利用运动学模型作为系统模型，观测量处理单元的结果建立观测模型，经过融合滤波器的处理最终得到行人导航结果。本发明克服了在没有其他辅助系统的情况下，导航误差会迅速累积的缺点；IMU处理模块考虑了手持智能设备的多种模式，突破了传统多传感器融合IMU需和载体固定的限制；提高了行人导航的精确度。
【专利说明】
一种基于新型多传感器融合技术的行人导航装置和方法
技术领域
[0001] 本发明涉及多传感器融合和行人导航领域，尤其是一种基于新型多传感器融合技 术的行人导航装置和方法。
【背景技术】
[0002] 随着移动互联网的发展，室内外的行人导航应用蓬勃发展，例如大型商场室内导 航、医院病人追踪、超市人流分析等。国内外多个市场分析报告一致认为行人导航将是一个 有巨大市场的研究方向。与此同时，便携式智能设备，例如：智能手机、平板电脑和智能手表 等在过去十年中一直超高速发展，已成为人们生活不可缺少的一部分。这些便携式设备大 多数具备强大的处理器、无线收发器、摄像头、全球导航卫星系统GNSS接收机和众多传感 器。因此，这些便携式智能设备已成为多传感器融合和行人导航相关应用的理想平台。
[0003] 目前单一行人导航技术都存在不同程度的缺陷。基于WiFi和蓝牙等无线系统的行 人导航技术通常具有无线信号强度在恶劣的环境下波动较大，无法提供完整的导航信息如 三维位置、速度和姿态，系统性能高度依赖于发射设备的分布和数量，位置信息不连续平滑 等缺陷。基于微惯性单元的行人导航技术短期精准，但导航误差累积较快。基于摄像头的视 觉定位在复杂环境下视觉传感器标定较慢、特征信息提取错误率高、导航信息计算慢等缺 陷。因此，多传感器融合已成为目前行人导航的主流方案。
[0004] 目前，现有的多传感器融合技术一般包括以下几个步骤：（1)以惯性单元(三轴加 速度计和三轴陀螺仪)的测量数据通过惯性机械编排算法计算跟踪物体的位置、速度和姿 态信息；（2)建立与惯性机械编排算法对应的误差模型，并作为融合滤波器的系统模型；（3) 将其他辅助系统(GPS、WiFi、蓝牙、RFID、GNSS等)建立为融合滤波器的观测模型；（4)通过融 合滤波器的预测和更新过程估计出系统状态量误差;和(5)将系统状态量误差补偿惯性单 元误差和基于惯性机械编排算法的位置、速度和姿态信息，得出最终的位置信息、速度和姿 态信息。
[0005] 现有多传感器融合技术具有以下两个致命缺点（1)在没有其他辅助系统的情况 下，导航误差会迅速累积；（2)当惯性单元与载体不固定的情况下，例如:行人导航中的手机 和行人，传统的多传感器融合技术无法正确估计出载体的信息。因此，现有多传感器融合技 术在很多场景下无法提供准确的行人导航信息。

【发明内容】

[0006] 本发明所要解决的技术问题在于，提供一种基于新型多传感器融合技术的行人导 航装置和方法，达到提升行人导航精度和可用性的效果。
[0007] 为解决上述技术问题，本发明提供一种基于新型多传感器融合技术的行人导航装 置和方法，包括:手持智能设备平台、观测量处理单元以及融合滤波器;手持智能设备利用 自身硬件获取惯性测量单元(Inertial Measurement Unit，IMU)、磁力计、压力计、WiFi、低 耗能蓝牙（Bluetooth Low Energy，BLE)和全球导航卫星系统（Global Navigation Satellite SyStem，GNSS)的原始数据，观测量处理单元处理手持智能设备提供的原始数据 以提供位置或速度等观测量给融合滤波器，融合滤波器利用运动学模型作为系统模型，观 测量处理单元的结果建立观测模型，经过融合滤波器的处理最终得到行人导航结果。
[0008]优选的，手持智能设备平台包括现有智能设备常见的MU、磁力计、压力计、WiFi、 低耗能蓝牙和GNSS等;IMU提供加速度和角速度的原始数据;所述磁力计提供地磁的原始数 据；压力计提供大气气压的原始数据;WiFi提供WiFi接收信号强度（Received Signal Strength，RSS)的原始数据;BLE提供BLERSS的原始数据;GNSS接收机提供GNSS的原始数据； 智能设备平台任何可以提供观测信息的其他传感器都可以包含在所提出的多传感器融合 算法中。
[0009] 优选的，观测量处理单元包括：IMU处理单元、磁力计处理单元、压力计处理单元、 WiFi处理单元、BLE处理单元和GNSS处理单元等；IMU处理单元处理所述IMU提供的加速度和 角速度的原始数据以得到頂U位置信息并传送给所述融合滤波器;磁力计处理单元处理所 述磁力计提供的地磁的原始数据以得到地磁位置信息并传送给所述融合滤波器;压力计处 理单元处理所述压力计提供的大气气压的原始数据以得到高程信息并传送给所述融合滤 波器;WiFi处理单元处理所述WiFi提供的RSS原始数据以得到WiFi位置信息并传送给所述 融合滤波器;BLE处理单元处理所述BLE提供的RSS原始数据以得到BLE位置信息并传送给所 述融合滤波器;GNSS处理单元处理所述GNSS接收机提供的位置和速度信息并传送给所述融 合滤波器。观测量处理单元还包括其他处理单元以处理智能设备平台的其他传感器来得到 位置或速度信息并传送给融合滤波器。
[0010]优选的，融合滤波器包括系统模型和观测模型；系统模型使用运动学模型对待测 目标的位置和速度信息进行预测，并传送给观测模型;观测模型将系统模型预测的位置、速 度信息与观测量处理单元提供的基于頂U、磁力计、压力计、WiFi、BLE和GNSS等的位置、速度 等信息结合，更新待测目标的的最终位置和速度信息。
[0011]优选的，頂u处理单元包括用户运动模式和设备使用模式识别模块、航向角偏差估 计模块、改进航位推算算法模块，用户运动模式和设备使用模式识别模块根据所述手持智 能设备平台的MU和其他可选的硬件(例如磁力计)提供的原始数据识别出静止、行走、跑步 等用户运动模式和手持、短信、电话、导航、口袋、背包等设备使用模式;航向角偏差估计模 块根据所述用户运动模式和设备使用模式识别模块输出的用户运动模式和设备使用模式 和所述手持智能设备平台的IMU和其他可选的硬件(例如磁力计)提供的原始数据估计出航 向角偏差;改进航位推算算法模块根据所述航向角偏差估计模块输出的航向角偏差和所述 手持智能设备平台的IMU和其他可选的硬件(例如磁力计)提供的原始数据得到IMU位置信 息并传送给所述融合滤波器。
[0012]优选的，改进航位推算算法模块包括测姿系统模块、航向角偏差补偿模块、步伐检 测模块、步长估计模块、航位推算算法模块，测姿系统模块根据所述手持智能设备平台的 IMU和其他可选磁力计提供的原始数据识别出手持智能设备的姿态信息;航向角偏差补偿 模块读取航向角偏差估计模块输出的航向角偏差并补偿给行人航向角、输出给航位推算算 法;步伐检测模块算法模块根据所述手持智能设备平台的IMU的原始数据检测出行人的步 数反馈给步长估计模块;步长估计模块根据所述步伐检测模块的结果和所述手持智能设备 平台的IMU的原始数据估计出行人的步长，并反馈给所述的航位推算模块;航位推算模块根 据所述步长估计模块输出的步长信息和所述航向角偏差补偿模块输出的行人航向角信息 计算出IMU位置观测量并反馈给所述的融合滤波器。
[0013]相应地，本发明还提供了一种基于新型多传感器融合技术的行人导航方法，包括 以下步骤：
[0014] (1)手持智能设备利用自身硬件获取頂U、磁力计、压力计、WiFi、BLE和GNSS的原始 数据；（2)观测量处理单元处理手持智能设备提供的原始数据以提供位置或速度等观测量 给融合滤波器；（3)融合滤波器利用运动学模型作为系统模型，观测量处理单元的结果建立 观测模型，经过融合滤波器的处理最终得到行人导航结果。
[0015] 优选的，观测量处理单元处理手持智能设备提供的原始数据以提供位置或速度等 观测量给融合滤波器包括如下步骤：
[0016] (1UMU处理单元处理所述頂U提供的加速度和角速度的原始数据以得到頂U位置 信息并传送给所述融合滤波器；（2)磁力计处理单元处理所述磁力计提供的地磁的原始数 据以得到地磁位置信息并传送给所述融合滤波器；（3)压力计处理单元处理所述压力计提 供的大气气压的原始数据以得到高程信息并传送给所述融合滤波器；（4)WiFi处理单元处 理所述WiFi提供的RSS原始数据以得到WiFi位置信息并传送给所述融合滤波器；（5)BLE处 理单元处理所述BLE提供的RSS原始数据以得到BLE位置信息并传送给所述融合滤波器；（6) GNSS处理单元处理所述GNSS芯片提供的GNSS的原始数据以得到GNSS的位置和速度信息并 传送给所述融合滤波器。
[0017] 优选的，頂U处理单元处理IMU提供的加速度和角速度的原始数据以得到IMU位置 信息并传送给所述融合滤波器包括如下步骤：
[0018] (1)所述用户运动模式和设备使用模式识别模块根据所述手持智能设备平台的 頂U和其他可选的硬件(例如磁力计)提供的原始数据识别出静止、行走、跑步等用户运动模 式和手持、短信、电话、导航、口袋、背包等设备使用模式；
[0019] (2)航向角偏差估计模块根据所述用户运动模式和设备使用模式识别模块输出的 用户运动模式和设备使用模式和所述手持智能设备平台的IMU和其他可选的硬件(例如磁 力计)提供的原始数据估计出航向角偏差；
[0020] (3)改进航位推算算法模块根据所述航向角偏差估计模块输出的航向角偏差和所 述手持智能设备平台的MU和其他可选的硬件(例如磁力计)提供的原始数据得到IMU位置 信息并传送给所述融合滤波器。
[0021] 优选的，所述改进航位推算算法模块包括如下步骤：
[0022] (1)测姿系统模块根据所述手持智能设备平台的IMU和其他可选磁力计提供的原 始数据识别出手持智能设备的姿态信息；
[0023] (2)航向角偏差补偿模块读取航向角偏差估计模块输出的航向角偏差并补偿给行 人航向角、输出给航位推算算法；
[0024] (3)步伐检测模块算法模块根据所述手持智能设备平台的IMU的原始数据检测出 行人的步数反馈给步长估计模块；
[0025] (4)步长估计模块根据所述步伐检测模块的结果和所述手持智能设备平台的IMU 的原始数据估计出行人的步长，并反馈给所述的航位推算模块；
[0026] (5)航位推算模块根据所述步长估计模块输出的步长信息和所述航向角偏差补偿 模块输出的行人航向角信息计算出IMU位置观测量并反馈给所述的融合滤波器。
[0027] 本发明的有益效果为:本发明优化了传统行人导航中MU的使用方法，将其从融合 滤波器的系统模型解放出来变成观测模型，克服了将传统多传感器融合在没有其他辅助系 统的情况下，导航误差会迅速累积的缺点。本发明中的MU处理模块考虑了日常生活中手持 智能设备的多种模式，突破了传统多传感器融合IMU需和载体固定的限制。因此，本发明大 大提高了行人导航的精确度和可用性。
【附图说明】
[0028] 图1为本发明基于新型多传感器融合的行人导航装置的结构示意图。
[0029] 图2为本发明中惯性单元处理模块的结构示意图。
[0030] 图3为本发明中高斯内核支持向量机非线性分类器示意图。
[0031] 图4为本发明中用户运动模式和智能设备使用模式识别支持向量机示意图。
[0032] 图5为本发明中改进的行人航位推测算法示意图。
【具体实施方式】
[0033] 如图1所示，一种基于新型多传感器融合的行人导航装置，包括:手持智能设备平 台1、观测量处理单元2以及融合滤波器3。手持智能设备1利用自身硬件获取惯性测量单元 (Inertial Measurement Unit, IMU)11、磁力计 I2、压力计 l3、WiFil4、低耗能蓝牙 (Bluetooth Low Energy,BLE)15和全球导航卫星系统（Global Navigation Satellite SyStemS，GNSS)16的原始数据，观测量处理单元2处理手持智能设备1提供的原始数据以提 供位置或速度等观测量给融合滤波器3,融合滤波器3利用运动学模型作为系统模型31，观 测量处理单元的结果建立观测模型32,经过融合滤波器3的处理最终得到行人导航结果。上 述新型多传感器融合行人导航装置可以用于各种手持智能设备(包括智能手机、平板电脑、 智能手表等），手持智能设备可以手持或固定于行人身上。图1所示的新型多传感器融合的 行人导航系统颠覆了传统行人导航中IMU的使用方法，将其从融合滤波器的系统模型31解 放出来变成观测模型32,克服了将传统多传感器融合行人导航装置在没有其他辅助系统的 情况下，导航误差会迅速累积的缺点。
[0034] 上述手持智能设备平台1包括现有智能设备常见的頂U 11、磁力计12、压力计13、 WiFi 14、BLE 15和GNSS 16等；頂U 11提供加速度和角速度的原始数据;磁力计12提供地磁 的原始数据；所述压力计13提供大气气压的原始数据;WiFi 14提供WiFi接收信号强度 (Received Signal Strength,RSS)的原始数据;BLE 15提供BLERSS的原始数据;GNSS 16提 供GNSS的原始速度和位置数据。手持智能设备1任何可以提供观测信息的其他传感器都可 以包含在所提出的多传感器融合算法中。
[0035] 上述观测量处理单元2包括：IMU处理单元21、磁力计处理单元22、压力计处理单元 23、WiFi处理单元24、BLE处理单元25和GNSS处理单元26等。頂U处理单元21处理所述頂U 11 提供的加速度和角速度的原始数据以得到IMU位置信息并传送给所述融合滤波器3;磁力计 处理单元22处理所述磁力计12提供的地磁的原始数据以得到地磁位置信息并传送给所述 融合滤波器3;压力计处理单元23处理所述压力计13提供的大气气压的原始数据以得到高 程信息并传送给所述融合滤波器3; WiFi处理单元24处理所述WiFi 14提供的RSS原始数据 以得到WiFi位置信息并传送给所述融合滤波器3 ;BLE处理单元25处理所述BLE 15提供的 RSS原始数据以得到BLE位置信息并传送给所述融合滤波器3 ;GNSS处理单元26处理所述 GNSS 16原始数据以得到图像位置信息并传送给所述融合滤波器3。观测量处理单元2还包 括其他处理单元以处理手持智能设备平台1的其他传感器来得到位置或速度信息并传送给 融合滤波器3。
[0036]上述融合滤波器3包括系统模型31和观测模型32。系统模型31使用运动学模型对 待测目标的位置和速度信息进行预测，并传送给观测模型32;观测模型32将系统模型31预 测的位置、速度信息与观测量处理单元提供的基于頂U 11、磁力计12、压力计13、WiFi 14、 BLE 15和GNSS 16等的位置、速度等信息结合，更新待测目标的的最终位置和速度信息。 [0037]如图2所示，頂U处理单元21包括用户运动模式和设备使用模式识别模块211、航向 角偏差估计模块212、改进航位推算算法模块213,用户运动模式和设备使用模式识别模块 211根据所述手持智能设备平台的IMU 11和其他可选的硬件(例如磁力计12)提供的原始数 据识别出静止、行走、跑步等用户运动模式和手持、短信、电话、导航、口袋、背包等设备使用 模式。航向角偏差估计212模块根据所述用户运动模式和设备使用模式识别模块211输出的 用户运动模式和设备使用模式和所述手持智能设备平台的IMU 11和其他可选的硬件(例如 磁力计12)提供的原始数据估计出航向角偏差。改进航位推算算法模块213根据所述航向角 偏差估计模块212输出的航向角偏差和所述手持智能设备平台1IMU 11和其他可选的硬件 (例如磁力计12)提供的原始数据得到頂U位置信息并传送给所述融合滤波器3。图2中的所 述頂U处理单元考虑了行人多种运动模式和智能设备的多种使用模式，设计了针对多种使 用场景的MU数据处理方法，突破了传统算法中頂U需要和载体固定的限制，提高了行人导 航系统的可用性。
[0038] 用户运动模式和设备使用模式识别模块211使用现有的手持智能设备1相关传感 器输出：頂U 11，磁强计12,测距传感器(可选），光传感器(可选）。頂U 11和磁强计12更新频 率为50-200HZ;后两种传感器的输出为标量，更新为用户行为触发。用户运动模式和设备使 用模式识别算法提取1-3秒内传感器统计数据，做出分类决定。用户运动模式和设备使用模 式识别算法可以有多种实现方式。本发明用高斯内核对偶型支持向量机作为实现的示例。
[0039] 如图3所示，基于高斯内核的支持向量机可以隐含的将特征向量映射到无限维线 性空间，从而达到或超越非线性分类(如传统的KNN)的效果。I 1范数软边际支持向量机原型 如下：
[0040] 训练公式(1): N
[0041 ] min||w||2 + C / ξι i 二 1.
[0042] Φ (xi)+b)^l-|i,
[0043] |i^〇.
[0044] 其中，Xi G Rd，i，= 1，2，…，为训练集合中的特征向量与分类结果，w e Rd为权重向 量，C为可控规范化常量用以平衡对于训练集合中数据的拟合过度与不足，Φ (·)为特征向 量映射函数。
[0045] 分类公式(2):
[0046] f(χ) =￥ΤΦ (x)+b.
[0047] 由于满足KKT条件，l1范数软边际支持向量机对偶型如下：
[0048]训练公式(3):
[0050] s.t.O^ai^C, Μ
[0051] Σ (i(yi = 〇. i-x
[0052] 分类公式(4): Μ
[0053] f ix) = ^ aiy^{xi)rφ(χ) + b. ￡=1
[0054] 引入高斯内核作为高效计算映射与内积的方式如下(5):
[0055] ) = exp(_ | | χ-χ' | 12/22)，>0·
[0056] 则对偶型转可以化为如下形式：
[0057]训练公式(6):
[0059] s.t.O^ai^C, N
[0060] Σ <^yi = 〇 ?： = ：1
[0061] 分类公式(7): Ν
[0062] fix') = ^ αιγ^(χι,χ) + h. ? = 1
[0063] 与传统的分类器ΚΝΝ不同，支持向量机对偶型的最优解h，= 1，2，…，只存在少部 分非零值，因此只需保留少部分训练特征向量（即支持向量)参与在线的分类计算(例如，实 现如图3所示的基于随机产生数据的非线性分类器，KNN需要存储并使用1000个原始训练特 征向量，支持向量机则只需142个支持向量），从而在很大程度上降低了对于处理器电池消 耗与系统内存的需求，比较适合于手持智能设备平台的应用。
[0064] 用户运动模式和设备使用模式识别模块211将行人行为模式分为5类：1.静止;2. 行走;3.跑步;4.自行车;5.驾驶汽车。行人行为模式的识别可以用于应用零速矫正，调节跟 踪滤波器过程噪声的方差，以及调节动态系统马尔可夫过程的相关时间。所述用户运动模 式和设备使用模式识别模块211将设备使用模式分为4类：1.前端平置;2 .耳侧竖置;3 .背 包;4.臂章。手机姿态模式的识别可以用于前进方向的确定(坐标变换），以及调节跟踪滤波 器过程噪声的方差。
[0065] 如图4所示，为应用二级分类器的用户运动模式和设备使用模式识别支持向量机 示意图，包括加速度统计量2111、角速度统计量2112、转动角和倾斜角统计量2113、光和距 离统计量2114、速度反馈统计量2115、特征规范化模块2116、主成分分析模块2117、支持向 量机模块2118、用户运动模式一级分类器2119和设备使用模式二级分类器2110。具体实现 步骤有:线下收集具有代表性的数据集，进行特征向量规范化与主成分分析，应用公式(5) 与（6)进行训练，提取并存储支持向量;在线计算传感器输出统计量，进行特征向量规范化 与主成分提取(与训练集中系数相同），应用存储的支持向量，以及公式(5)与（7)进行二级 分类，决定用户运动模式和设备使用模式。
[0066] 本发明中的航向角偏差估计模块212包含多种不同的方法。当只有頂U 11可用时， 我们采用基于主成分分析方法(Principle Component Analysis，PCA)。行人运动的一个特 征是行人加速和减速的方向都在行进方向。因此可以通过PCA分析加速度计的数据得到行 人的行进方向。当GNSS 16可用时，行人行进的方向可以通过GNSS的速度计算得来。当磁力 计12可用的时候，行人行进的方向也可以通过磁力计12计算得来。手持智能设备的航向角 则通过九轴融合或者六轴融合得来。因此，航向角偏差可以通过各种方法得来的行人行进 方向的融合解和手持智能设备的航向角相减得到，并输出到航向角偏差补偿模块2132。
[0067] 如图5所示，为上述改进航位推算算法模块213示意图，包括测姿系统模块2131、航 向角偏差补偿模块2132、步伐检测模块2133、步长估计模块2134、航位推算算法模块2135， 测姿系统模块2131根据所述手持智能设备平台1的IMU 11和其他可选磁力计12提供的原始 数据识别出手持智能设备1的姿态信息。航向角偏差补偿模块2132读取航向角偏差估计模 块212输出的航向角偏差并补偿给行人航向角、输出给航位推算算法模块2135。步伐检测模 块2133根据所述手持智能设备平台的IMU 11的原始数据检测出行人的步数反馈给步长估 计模块2134。步长估计模块2134根据所述步伐检测模块的结果和所述手持智能设备平台1 的IMU 11的原始数据估计出行人的步长，并反馈给所述的航位推算模块2135。航位推算模 块2135根据所述步长估计模块2134输出的步长信息和所述航向角偏差补偿模块2132输出 的行人航向信息计算出IMU位置观测量并输出给所述的融合滤波器3。
[0068] 测姿系统模块2131根据所述手持智能设备平台1的頂U 11和其他可选磁力计12提 供的原始数据识别出手持智能设备1的姿态信息。测姿系统模块2131采用的算法根据地磁 信息是否可用，选择九轴测姿算法或六轴测姿算法。最终，测姿系统模块2131输出智能设备 1的航向角给航向角偏差补偿模块2132。
[0069] 所述航向角偏差补偿模块2132读取航向角偏差估计模块212输出的航向角偏差并
[0070] 补偿给行人航向角、输出给航位推算算法模块2135。具体计算公式如下：
[0071] θρ= 0d+0Qffset · (1)式中行人航向角，设备航向角，0避响是航向角偏差。
[0072] 步伐检测模块2133根据所述手持智能设备平台1的IMU 11的原始数据检测出行人 的步数反馈给步长估计模块2134。步伐检测可以通过峰值检测、过零检测、相关检测和功率 谱检测等方法检测步伐。本发明考虑到多种用户运动模式和设备使用模式，步伐检测算法 采用峰值检测同时检测IMU 11的加速度数据和陀螺仪数据。
[0073]步长估计模块2134根据所述步伐检测模块2133的结果和所述手持智能设备平台1 的IMU 11的原始数据估计出行人的步长，并输出给所述的航位推算模块2135。步长估算可 以通过加速度积分、钟摆模型、线性模型、经验模型等不同方法计算。本发明考虑到多种用 户运动模式和设备使用模式，步长估算采用如下线性模型：
[0074] Sk-l,k = A · (fk-l+fk)+B · (〇acc,k-l+〇acc,k)+C (2)
[0075] 式中A，B和C是常数，fk-i和fk是k-1时刻和k时刻的步频，和oac;c;,k是k-1时刻 和k时刻的加速度计的方差。
[0076] 航位推算模块2135根据k-Ι时刻的位置[^ rn,^]τ、步长估计模块2134输出的 步长信息sh , k和航向角偏差补偿模块2132输出的航向角信息推算出k时刻的位置[^, k rn,k]T。对应的计算公式如下： Q ]「n't-丨 "I _7] ? 彳 U+WU」 （3)
[0078] 最终，航位推算模块2135输出頂U位置观测量给所述的融合滤波器3。
[0079] 融合滤波器3包括系统模型31和观测模型32。传统的多传感器融合结构一般通过 惯性机械编排算法处理頂U测量数据，并建立相关的融合滤波器系统模型。由于惯性机械编 排中存在很多积分操作，因此当没有外在辅助系统的情况下，传统的多传感器融合结构的 定位误差会迅速累积。本发明克服传统的多传感器融合结构的缺陷，以行人的运动模型作 为系统模型，而IMU相关数据和其他系统一样作为观测模型。
[0080] 融合滤波器3可以采用卡尔曼滤波（Kalman Filter，KF)、自适应卡尔曼滤波 (Adaptive Kalman Filter，AKF)、UKF无损卡尔曼滤波(Unscented Kalman Filter，UKF)或 粒子滤波(Particle Filter，PF)。本发明给出KF的设计范例。其他的滤波器可以参考KF的 设计。融合滤波器3实现为KF的状态向量定义如下：
[0081] x=[re rn ru ve vn vu]T (4)
[0082] 式中re，rn和ru是三维位置（东北天坐标系），ve，vn和vu是对应的三维速度组成。 该KF系统模型32采用经典的运动学模型，定义如下：
[0083] xk+i|k= 〇k,k+ixk|k+?k (5)
[0084] 式中xk+i I k是预测的状态向量，xk I k是在k时刻先前的状态向量，Φ k, k+i是一个6 X 6 转
[0085] 移矩阵： "1 0 0 At 0 0 " 0 1 0 0 Δ? 0 0 0 1 0 0 At
[0086] Φ<·*+1 ~ 〇 〇 〇 I 〇 〇 0 0 0 〇 1 0 0 0 0 0 0 1」 的
[0087] 式中Δ t是两个时刻的时间差。cok是协方差矩阵(? 的处理噪声，定义如 下：
[0088] ?,= [0 0： 0 J (7):
[0089] 式中?和0^是在k时刻东向天坐标系下的速度噪声，按随机游走建模。 mc.k =STf.A-l+"c·^
[0090] " = + nn ' =^-1 +?" ·Δ/
[0091] 式中％·*-1，义*-1和是在k~l时刻东向天坐标系的速度噪声，n e，nn和nu是尚斯 白噪声，At是两个时刻的时间差。
[0092]融合滤波器3实现为KF的测量模型31定义如下：
[0093] zk = HkXk|k+Uk (9)
[0094] 式中Zk是测量向量，Hk是决策矩阵。uk是测量噪声以高斯白噪声为模型，其协方差 矩阵为队随观测量的不同而变化。当观测量来自于頂U 11时，典型的zk和 Hk定义如下：
[0095] zk=[re rn]T _Γι ：〇 ο ο ο ο"
[0096] Hi~j) 1 Ο 〇 Q 〇_ (1〇).
[0097]当观测量来自于磁力计12时，典型的21{和也定义如下：
[0098] zk=[re rn ru]T "1 0 0 0 0 Q"
[0099] Η. ：= Ο 1 Q Ο Ο Q
[οοιοοο. (II)
[0100] 当观测量来自于压力计13时，典型的zdPHk定义如下：
[0101] zk = ru
[0102] Hk=[0 0 1 0 0 0] (12)当观测量来自于WiFi 14时，典型的Zk和Hk定义如下：
[0103] zk=[re rn ru]T '1 Ο： Ο Ο Ο Ο"
[0104] Η, = Ο： ：1 Ο Q Ο Ο
[ο： ο 1 ο ο oj (⑶ J Xk+l\k ~^k.k+lXk\k
[0112] }^=φ^Μ,,+ι^. (16)
[0113] 在更新过程中，根据测量模型更新状态向量和协方差矩阵：
[0114] 丨"） Α =Π-Κ,Η,)Ρ,,丨（卜Κ;Η; )、?( 1 f 17)
[0115] 式中Kk称为卡尔曼增益。
[0116] 尽管本发明就优选实施方式进行了示意和描述，但本领域的技术人员应当理解， 只要不超出本发明的权利要求所限定的范围，可以对本发明进行各种变化和修改。
【主权项】
1. 一种基于新型多传感器融合技术的行人导航装置，其特征在于，包括:手持智能设备 (1)、观测量处理单元(2)以及融合滤波器(3);所述的手持智能设备（1)利用自身硬件获取 观测量原始数据，所述观测量处理单元(2)处理手持智能设备（1)提供的原始数据以提供位 置或速度等观测量给融合滤波器(3)，所述融合滤波器利用运动学模型作为系统模型(32)， 观测量处理单元的结果建立观测模型（33)，经过融合滤波器(3)的处理最终得到行人导航 结果。2. 如权利要求1所述的基于新型多传感器融合技术的行人导航装置，其特征在于，所述 手持智能设备1自身硬件包括:惯性测量单元MU(ll)、磁力计（12)、压力计（13)、WiFi(14)、 低耗能蓝牙BLE(15)和全球导航卫星系统GNSS(16);所述頂U(ll)提供加速度和角速度的原 始数据;所述磁力计（12)提供地磁的原始数据;所述压力计（13)提供大气气压的原始数据； 所述WiFi(14)提供WiFi接收信号强度RSS的原始数据;所述BLE(15)提供BLERSS的原始数 据;所述GNSS接收机(16)提供GNSS的原始数据;手持智能设备（1)还可以包括提供观测信息 的其他传感器。3. 如权利要求1所述的基于新型多传感器融合技术的行人导航装置，其特征在于，观测 量处理单元模块(2)包括：IMU处理单元(21)、磁力计处理单元(22)、压力计处理单元(23)、 WiFi处理单元(24)、BLE处理单元(25)和GNSS处理单元(26);所述頂U处理单元(21)处理所 述IMU(ll)提供的加速度和角速度的原始数据以得到IMU位置信息并传送给所述融合滤波 器(3);所述磁力计处理单元(22)处理所述磁力计（12)提供的地磁的原始数据以得到地磁 位置信息并传送给所述融合滤波器（3);所述压力计处理单元（23)处理所述压力计提供 (13)的大气气压的原始数据以得到高程信息并传送给所述融合滤波器(3);所述WiFi处理 单元处理(24)所述WiFi(14)提供的RSS原始数据以得到WiFi位置信息并传送给所述融合滤 波器(3);所述BLE处理单元处理（25)所述BLE(15)提供的RSS原始数据以得到BLE位置信息 并传送给所述融合滤波器(3);所述GNSS处理单元(26)处理所述GNSS接收机（16)提供的位 置和速度信息并传送给所述融合滤波器(3);观测量处理单元模块(2)还包括其他处理单元 以处理智能设备平台的其他传感器来得到位置或速度信息并传送给融合滤波器(3)。4. 如权利要求1所述的基于新型多传感器融合技术的行人导航装置，其特征在于，所述 頂U处理单元(21)包括用户运动模式和设备使用模式识别模块、航向角偏差估计模块、改进 航位推算算法模块，所述用户运动模式和设备使用模式识别模块根据所述手持智能设备平 台的IMU和其他可选的硬件提供的原始数据识别出静止、行走、跑步等用户运动模式和手 持、短信、电话、导航、口袋、背包等设备使用模式;所述航向角偏差估计模块根据所述用户 运动模式和设备使用模式识别模块输出的用户运动模式和设备使用模式和所述手持智能 设备平台的IMU和其他可选的硬件提供的原始数据估计出航向角偏差;改进航位推算算法 模块根据所述航向角偏差估计模块输出的航向角偏差和所述手持智能设备平台的IMU和其 他可选的硬件提供的原始数据得到IMU位置信息并传送给所述融合滤波器。5. 如权利要求4所述的基于新型多传感器融合技术的行人导航装置，其特征在于，所述 改进航位推算算法模块包括测姿系统模块、航向角偏差补偿模块、步伐检测模块、步长估计 模块、航位推算算法模块，所述测姿系统模块根据所述手持智能设备平台的IMU和其他可选 磁力计提供的原始数据识别出手持智能设备的姿态信息;所述航向角偏差补偿模块读取航 向角偏差估计模块输出的航向角偏差并补偿给行人航向角、输出给航位推算算法;步伐检 测模块算法模块根据所述手持智能设备平台的IMU的原始数据检测出行人的步数反馈给步 长估计模块;步长估计模块根据所述步伐检测模块的结果和所述手持智能设备平台的IMU 的原始数据估计出行人的步长，并反馈给所述的航位推算模块;航位推算模块根据所述步 长估计模块输出的步长信息和所述航向角偏差补偿模块输出的行人航向角信息计算出IMU 位置观测量并反馈给所述的融合滤波器。6. 如权利要求4所述的基于新型多传感器融合技术的行人导航装置，其特征在于，所述 用户运动模式和设备使用模式识别模块采用一种基于高斯内核对偶型支持向量机的二级 分类器，在周期性时间间隔中提取手持智能设备的传感器输出，运用支持向量与高斯内核 对用户运动模式与手机使用模式进行识别与分类。7. 如权利要求1所述的基于新型多传感器融合技术的行人导航装置，其特征在于:计算 IMU，磁强计，可选测距，可选光传感器的输出统计量，及其衍生统计量，以其为特征向量进 行规范化与主成分分析，截取相应于保留绝大多数数据方差的特征值的特征向量参与支持 向量机的线下训练。8. 如权利要求1所述的基于新型多传感器融合技术的行人导航装置，其特征是:手持智 能设备平台的在线分类计算，只需存储并运用降维后的少数支持向量，进行用户运动模式 与设备使用模式的二级分类。9. 一种基于新型多传感器融合技术的行人导航方法，其特征在于，包括如下步骤： (1) 手持智能设备利用自身硬件获取IMU、磁力计、压力计、WiFi、BLE和GNSS的原始数 据； (2) 观测量处理单元处理手持智能设备提供的原始数据以提供位置或速度等观测量给 融合滤波器； (3) 融合滤波器利用运动学模型作为系统模型，观测量处理单元的结果建立观测模型， 经过融合滤波器的处理最终得到行人导航结果。10. 如权利要求9所述的一种基于新型多传感器融合技术的行人导航方法，其特征在 于，观测量处理单元处理手持智能设备提供的原始数据以提供位置或速度等观测量给融合 滤波器包括如下步骤： 所述MU处理单元处理所述MU提供的加速度和角速度的原始数据以得到MU位置信息 并传送给所述融合滤波器； 所述磁力计处理单元处理所述磁力计提供的地磁的原始数据以得到地磁位置信息并 传送给所述融合滤波器； 所述压力计处理单元处理所述压力计提供的大气气压的原始数据以得到高程信息并 传送给所述融合滤波器； 所述WiFi处理单元处理所述WiFi提供的RSS原始数据以得到WiFi位置信息并传送给所 述融合滤波器； 所述BLE处理单元处理所述BLE提供的RSS原始数据以得到BLE位置信息并传送给所述 融合滤波器； 所述GNSS处理单元处理所述GNSS芯片提供的GNSS的原始数据以得到GNSS的位置和速 度信息并传送给所述融合滤波器。11. 如权利要求10所述的一种基于新型多传感器融合技术的行人导航方法，其特征在 于JMU处理单元处理MU提供的加速度和角速度的原始数据以得到MU位置信息并传送给 所述融合滤波器包括如下步骤： 所述用户运动模式和设备使用模式识别模块根据所述手持智能设备平台的IMU和其他 可选的硬件提供的原始数据识别出静止、行走、跑步等用户运动模式和手持、短信、电话、导 航、口袋、背包等设备使用模式； 所述航向角偏差估计模块根据所述用户运动模式和设备使用模式识别模块输出的用 户运动模式和设备使用模式和所述手持智能设备平台的IMU和其他可选的硬件提供的原始 数据估计出航向角偏差； 改进航位推算算法模块根据所述航向角偏差估计模块输出的航向角偏差和所述手持 智能设备平台的IMU和其他可选的硬件提供的原始数据得到頂U位置信息并传送给所述融 合滤波器。12.如权利要求11所述的一种基于新型多传感器融合技术的行人导航方法，其特征在 于，改进航位推算算法模块包括如下步骤： 所述测姿系统模块根据所述手持智能设备平台的IMU和其他可选磁力计提供的原始数 据识别出手持智能设备的姿态信息； 所述航向角偏差补偿模块读取航向角偏差估计模块输出的航向角偏差并补偿给行人 航向角、输出给航位推算算法； 步伐检测模块算法模块根据所述手持智能设备平台的IMU的原始数据检测出行人的步 数反馈给步长估计模块； 步长估计模块根据所述步伐检测模块的结果和所述手持智能设备平台的IMU的原始数 据估计出行人的步长，并反馈给所述的航位推算模块； 航位推算模块根据所述步长估计模块输出的步长信息和所述航向角偏差补偿模块输 出的行人航向角信息计算出IMU位置观测量并反馈给所述的融合滤波器。
【文档编号】G01C21/16GK106017454SQ201610431107
【公开日】2016年10月12日
【申请日】2016年6月16日
【发明人】庄园, 杨军, 戚隆宁, 李由
【申请人】东南大学