[发明专利]一种适用于可穿戴设备的室内导航算法

摘要

本发明提供一种适用于可穿戴设备的室内导航算法，采用了同时构建WI‑Fi指纹数据库并导航的办法，基于惯性敏感组件、气压计、磁力计等传感器实现室内无缝导航能力。其中，导航算法采用数据融合的方式，集成了惯性传感器数据，WI‑Fi无线网卡数据，气压计数据和磁力计数据，并进行导航解算从而输出导航结果。与此同时，通过无线网卡收集到的附近WI‑Fi信号信息实时生成指纹数据库进而辅助导航。本发明是基于惯性器件和WI‑Fi指纹识别的室内导航算法，基于低成本的MEMS惯性传感器、气压计、磁力计等器件，辅助以WI‑Fi定位算法，能够快速的生成并更新WI‑Fi指纹数据库辅助航迹推算，进而大幅提升室内导航能力，可适用于穿戴设备上，能够有效的完成室内导航定位。

主权项

一种适用于可穿戴设备的室内导航算法，其特征在于以下步骤：步骤1：初始化阶段在产品初始化阶段，需要导入室内地图并指定初始位置，用户基于提前内置的室内地图集选择当下室内地图，并手动指定目前所在位置；如没有该地图，可通过网络数据库进行下载；步骤2：基于多传感器的航迹推算a)预处理陀螺、加计数据将三轴陀螺信号分别记为NX、NY、NZ，对其进行低通滤波，滤除外界高频噪声GX＝B0·NX+B1·NX_1+B2·NX_2+B3·NX_3+B4·NX_4+B5·NX_5+...B6·NX_6+B7·NX_7‑(A1·GX_1+A2·GX_2+A3·GX_3+A4·GX_4+...A5·GX_5+A6·GX_6+A7·GX_7)GY＝B0·NY+B1·NY_1+B2·NY_2+B3·NY_3+B4·NY_4+B5·NY_5+...B6·NY_6+B7·NY_7‑(A1·GY_1+A2·GY_2+A3·GY_3+A4·GY_4+...A5·GY_5+A6·GY_6+A7·GY_7)GZ＝B0·NZ+B1·NZ_1+B2·NZ_2+B3·NZ_3+B4·NZ_4+B5·NZ_5+...B6·NZ_6+B7·NZ_7‑(A1·GZ_1+A2·GZ_2+A3·GZ_3+A4·GZ_4+...A5·GZ_5+A6·GZ_6+A7·GZ_7)其中，每个采样时刻采集到的数据为NX，滤波后的输出值为GX，NX_1为上次采样值，NX_2为上上次采样值，其它类同；GX_1为上次滤波值，GX_2为上上次滤波值，其它类同；A1‑A7，B0‑B7为滤波器参数，需根据陀螺加计信号进行适当选择；加速度计同理，得到陀螺输出GX、GY、GZ和加计输出AX、AY、AZ；b)磁力计确定航向静止时只进行定位，移动时初始时刻以磁航向作为初始航向，不进行初始对准；航向角ψ＝磁力计航向角俯仰角θ＝0横滚角γ＝0c)捷联惯性解算计算C11＝cos(γ)·cos(ψ)+sin(γ)·sin(θ)·sin(ψ)C12＝cos(θ)·sin(ψ)C13＝sin(γ)·cos(ψ)‑cos(γ)·sin(θ)·sin(ψ)C21＝‑cos(γ)·sin(ψ)+sin(γ)·sin(θ)·cos(ψ)C22＝cos(θ)·cos(ψ)C23＝‑sin(γ)·sin(ψ)‑cos(γ)·sin(θ)·cos(ψ)C31＝‑cosθ·sinγC32＝sinθC33＝cosθ·cosγ计算ΔφnbXb=ΔφXb-Δφx]]>ΔφnbYb=ΔφYb-Δφy]]>ΔφnbZb=ΔφZb-Δφz]]>d)用陀螺数据进行姿态角零速校正平台偏角修正Cbn=1-φUφNφU1-φE-φNφE1·Cbn(-)]]>e)用加计数据进行位置零速校正λ(+)＝λ(‑)‑δλλ(‑)＝λ(+)速度修正VE(+)＝VE(‑)‑δVEVN(+)＝VN(‑)‑δVNVE(‑)＝VE(+)VN(‑)＝VN(+)f)气压计阻尼高度VU＝VU+K1(hb‑h)VU(‑)＝VU其中：K1＝0.0005若满足高度阻尼计算条件，则按照下式进行阻尼计算：h＝h+K2·(hb‑h)h(‑)＝h其中：K2＝0.01基于以上四步判断步速并进行基本航迹推算；步骤3：WI‑Fi指纹数据库的生成与更新在本阶段，WI‑Fi无线网卡实时接收来自附近无线接入点(AP,Access Point)的烽火信号(Beacon Signal)，其信号中包含了每个AP的MAC地址信息和信号强度(RSSI)信息；WI‑Fi指纹数据库即包含了在室内环境下任意某点位置能够接收到来自附近AP发出的烽火信号的数据强度值的集合；假设每个位置都可以用来自不同无线接入点的RSSI向量表示，那么某位置a可以表示为：r→=RSS1RSS2......RSSn]]>代表着该位置的RSS值向量，而RSS1到RSSn代表在随机位置a接收到的来自不同无线接入点的信号强度；在一个完整的Wi‑Fi指纹数据库中，应该包含着每个采样点的位置信息和信号强度信息；因此，该区域的Wi‑Fi指纹数据库可以写为：R=x1y1RSS11RSS12...RSS1nx2y2RSS21RSS22...RSS2n..................xmymRSSm1RSSm2...RSSmn]]>其中xi和yi分别代表采样点的坐标位置，m表示采样点的数量；在本方案中，使用了一种快速实时生成并更新数据库的方式，即采用了基于高斯过程的机器学习方法；假设每个采样点可以被写为：D＝{(xi，yi)|i＝1，2，...，n}其中x表示输入向量(即坐标)，同时y表示测量值；假设这些数据取自于一个加噪的过程：yi＝f(xi)+ε其中误差ε服从均值为零，方差为的高斯分布；通过优选出的协方差函数(kernel)k(xi，xj)，我们可以得出f(xi)和f(xj)之间的关系；使用协方差函数：k(xi,xj)=cov(f(xi),f(xj))=exp(-12|xi-xj|2)]]>实现高斯过程回归；利用这种办法可以通过有限的采样点快速生成整个环境下的数据库；刷新时间为1秒/次，每刷新一次更新一次WI‑Fi指纹数据库；步骤4：多信息源融合导航算法a)根据步骤3的融合wifi信号比对数据，圈定室内位置，融合惯导阻尼气压计数据并wifi信号，圈定楼层Δφx＝(C11·ωe'+C21·ωn'+C31·ωu')·TsΔφy＝(C12·ωe'+C22·ωn'+C32·ωu')·TsΔφz＝(C13·ωe'+C23·ωn'+C33·ωu')·Ts其中：du1＝(ω8‑ω7)×(VN‑K1·VrN)‑ω7·u1du2＝(ω5‑ω6)×(VN‑K1·VrN+u1)‑ω6·u2du3＝(ω4‑ω3)×(VE‑K1·VrE)‑ω3·u3du4＝(ω1‑ω2)×(VE‑K1·VrE+u3)‑ω2·u4du5＝C×B×(VN‑K1·VrN+u1+u2)‑ω9·u5du6＝ω10·u5‑ω10·u6u1＝u1+du1×TSu2＝u2+du2×TSu3＝u3+du3×TSu4＝u4+du4×TSu5＝u5+du5×TSu6＝u6+du6×TSωe′=-B×(VN-K1·VrN+u1+u2+K1×VrN)RN+h]]>ωn′=ωn+A×(VE-K1·VrE+u3+u4+K1×VrE)RE+h+DRN+h·(u5-u6)]]>e=1298.25]]>u1、u2、u3、u4、u5、u6初值均为0b)设计扩展卡尔曼算法实现室内导航定位φK/K-1=I+F·TF+(F·TF)22]]>QK-1=Q·TF+(FQ+(FQ)T)·TF22]]>XK/K‑1＝φK/K‑1·XK‑1PK/K-1=φK/K-1·PK-1·φK/K-1T+QK-1]]>PK‑1＝PK/K‑1XK‑1＝XK/K‑1KK=PK/K-1·HKT·(HK·PK/K-1·HKT+RK)-1]]>XK＝XK/K‑1+KK·(ZK‑HK·XK/K‑1)PK＝(I‑KK·HK)·PK/K‑1PK‑1＝PK当满足零速校正条件时：δλ＝X′K (2)δDN=X‾K(21)×0.7]]>δDU=X‾K(22)×0.7]]>否则：δλ＝0δDN、δDU保持不变；●量测矩阵F=F1F207×1307×7F=F1F2F309×1309×709×2]]>其中：F2=000000000000000000C11·f‾ZC11·f‾XC13·f‾Z0000C21·f‾ZC21·f‾XC23·f‾ZC11·ω‾ZC11·ω‾YC12·ω‾ZC13·ω‾Y000C21·ω‾ZC21·ω‾YC22·ω‾ZC23·ω‾Y000C31·ω‾ZC31·ω‾YC32·ω‾ZC33·ω‾Y000000000000000000000000000000000000000000000]]>F3=00000100000000000001000000′]]>零速校正时量测矩阵：H=1000000000000000000000010000000000000000000000100000000000000000000001000000000000000000]]>P＝diag(6e‑4，6e‑4，1，1，6e‑6，6e‑6，3.0e‑4，1e‑12，1e‑12，1e‑12，2.5e‑5，2.5e‑5，2.5e‑5，0，0，0，0，0，0，0,0,0)Q＝diag(0，0，5.e‑5，5.e‑5，5.e‑13，5.e‑13，5.e‑13，0，0，0，0，0，0,0,0,0,0,0，0，0,0,0)R＝diag(2.6e‑10，2.6e‑10)航迹推算过程中更换wifi接入点时，要对P1,1、P2,2进行重置：P1,1＝6e‑4P2,2＝6e‑4●零速校正滤波参数Q＝diag(0，0，0，0，5.e‑13，5.e‑13，5.e‑13，0，0，0，0，0，0,0,0,0,0,0，0，0，0，0)R＝diag(2.6e‑10，2.6e‑10，9，9)步骤5：导航结果显示导航结果将会通过无线网络传输至终端显示器进行显示(手机屏幕、手持设备屏幕等)，从而完成室内导航的任务。