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

权利要求书

1.一种适用于可穿戴设备的室内导航算法，其特征在于以下步骤：

步骤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

横滚角γ＝0

c)捷联惯性解算

计算

C₁₁＝cos(γ)·cos(ψ)+sin(γ)·sin(θ)·sin(ψ)

C₁₂＝cos(θ)·sin(ψ)

C₁₃＝sin(γ)·cos(ψ)-cos(γ)·sin(θ)·sin(ψ)

C₂₁＝-cos(γ)·sin(ψ)+sin(γ)·sin(θ)·cos(ψ)

C₂₂＝cos(θ)·cos(ψ)

C₂₃＝-sin(γ)·sin(ψ)-cos(γ)·sin(θ)·cos(ψ)

C₃₁＝-cosθ·sinγ

C₃₂＝sinθ

C₃₃＝cosθ·cosγ

计算

为b系下X轴惯性器件测量的角增量，Δφ_x为X轴陀螺零位误差，为x轴陀螺b系相对于n系的角增量；定义类似；

d)用陀螺数据进行姿态角零速校正

平台偏角修正

φ_U、φ_N、φ_E、分别为天向、北向、东向平台偏角，和姿态矩阵；

e)用加计数据进行位置零速校正

L＝L(-)-δL

λ＝λ(-)-δλ

L(-)＝L

λ(-)＝λ

L、λ分别为纬度、经度；

注：此处为循环，第二拍计算时，将第一拍计算的经纬度记为λ(-)、L(-)，第三拍计算时将第二拍计算的经纬度记为λ(-)、L(-)，以此类推；

速度修正

V_E＝V_E(-)-δV_E

V_N＝V_N(-)-δV_N

V_E(-)＝V_E

V_N(-)＝V_N

V_E、V_N分别为东向速度、北向速度；

注：此处为循环，第二拍计算时，将第一拍计算的东速北速记为V_E(-)、V_N(-)，第三拍计算时将第二拍计算的经纬度记为V_E(-)、V_N(-)，以此类推；

f)气压计阻尼高度

V_U＝V_U(-)+K₁(h_b-h)

V_U(-)＝V_U

V_U、h_b、h分别为天向速度、机体系高度和实际高度；

注：此处为循环，第二拍计算时，将第一拍计算的天向速度记为V_U(-)，第三拍计算时将第二拍计算的经纬度记为V_U(-)，以此类推；

其中：K₁＝0.0005

若满足高度阻尼计算条件，则按照下式进行阻尼计算：

h＝h(-)+K₂·(h_b-h(-))

h(-)＝h

注：此处为循环，第二拍计算时，将第一拍计算的实际高度记为h(-)，第三拍计算时将第二拍计算的经纬度记为h(-)，以此类推；

其中：K₂＝0.01

基于以上六步判断步速并进行基本航迹推算；

步骤3：WI-Fi指纹数据库的生成与更新

在本阶段，WI-Fi无线网卡实时接收来自附近无线接入点(AP,Access Point)的烽火信号(Beacon Signal)，其信号中包含了每个AP的MAC地址信息和信号强度RSSI信息；WI-Fi指纹数据库即包含了在室内环境下任意某点位置能够接收到来自附近AP发出的烽火信号的数据强度值的集合；

假设每个位置都可以用来自不同无线接入点的RSSI向量表示，那么某位置a可以表示为：

代表着该位置的RSS值向量，而RSS₁到RSS_n代表在随机位置a接收到的来自不同无线接入点的信号强度；在一个完整的Wi-Fi指纹数据库中，应该包含着每个采样点的位置信息和信号强度信息；因此，区域的Wi-Fi指纹数据库可以写为：

其中x_i和y_i分别代表采样点的坐标位置，m表示采样点的数量；在本方案中，使用了一种快速实时生成并更新数据库的方式，即采用了基于高斯过程的机器学习方法；假设每个采样点可以被写为：

Loca＝{(x_i，y_i)|i＝1，2，...，n}

其中x表示输入向量(即坐标)，同时y表示测量值；假设这些数据取自于一个加噪的过程：

y_i＝f(x_i)+ε

其中误差ε服从均值为零，方差为的高斯分布；通过优选出的协方差函数(kernel)k(x_i，x_j)，我们可以得出f(x_i)和f(x_j)之间的关系；使用协方差函数，f(x_i)代表xi点的函数值，同理，f(x_j)代表xj点的函数值：

实现高斯过程回归；利用这种办法可以通过有限的采样点快速生成整个环境下的数据库；刷新时间为1秒/次，每刷新一次更新一次WI-Fi指纹数据库；

步骤4：多信息源融合导航算法

a)根据步骤3的融合wifi信号比对数据，圈定室内位置，融合惯导阻尼气压计数据并wifi信号，圈定楼层

Δφ_x＝(C₁₁·ω_e′+C₂₁·ω_n′+C₃₁·ω_u′)·Ts

Δφ_y＝(C₁₂·ω_e'+C₂₂·ω_n'+C₃₂·ω_u')·Ts

Δφ_z＝(C₁₃·ω_e'+C₂₃·ω_n'+C₃₃·ω_u')·Ts

其中：

du₁＝(ω₈-ω₇)×(V_N-K₁·V_rN)-ω₇·u₁

du₂＝(ω₅-ω₆)×(V_N-K₁·V_rN+u₁)-ω₆·u₂

du₃＝(ω₄-ω₃)×(V_E-K₁·V_rE)-ω₃·u₃

du₄＝(ω₁-ω₂)×(V_E-K₁·V_rE+u₃)-ω₂·u₄

du₅＝C×B×(V_N-K₁·V_rN+u₁+u₂)-ω₉·u₅

du₆＝ω₁₀·u₅-ω₁₀·u₆

u₁＝u₁(-)+du₁×T_S

u₂＝u₂(-)+du₂×T_S

u₃＝u₃(-)+du₃×T_S

u₄＝u₄(-)+du₄×T_S

u₅＝u₅(-)+du₅×T_S

u₆＝u₆(-)+du₆×T_S

u₁(-)＝u₁

u₂(-)＝u₂

u₃(-)＝u₃

u₄(-)＝u₄

u₅(-)＝u₅

u₆(-)＝u₆

注：此处为循环，第二拍计算时，将第一拍计算的实际高度记为u(-)，第三拍计算时将第二拍计算的经纬度记为u(-)，以此类推；

D＝sin L

E＝-cos L

R_E＝R·[1+e·sin²(L)]

R_N＝R·[1-2e+3e·sin²(L)]

u₁、u₂、u₃、u₄、u₅、u₆初值均为0

R、R_E、R_N分别为地球平均半径、地球长半轴半径、地球短半轴半径

V_rE、V_rN分别为测量速度(如里程计等)实时分解的东向和北向的速度

Ts是采样时间，ω_e'，ω_n'，ω_u'是阻尼回路的东向、北向、天向的角速率；

u₁,u₂,u₃,u₄,u₅,u₆,是公式中间变量，没有量纲，给定初值和增量即可计算；

ω₁、ω₂、ω₃、ω₄、ω₅、ω₆、ω₇、ω₈、ω₉、ω₁₀、A，B，C，D,E,K₁为计算系数，没有量纲，根据实际系统选择；

b)设计扩展卡尔曼算法实现室内导航定位

δL、δλ分别为纬度误差和经度误差；

δV_E、δV_N分别为东向速度误差和北向速度误差；

φ_E、φ_N、φ_U分别为东向平台偏角、北向平台偏角和天向平台偏角；

δD_X、δD_Y、δD_Z、分别代表X轴方向的角速度增量、Y轴方向的角速度增量、Z轴方向的角速度增量，X轴方向的速度增量、Y方向的速度增量、Z方向的速度增量；

δα₁、δα₂、δα₃、δα₄、δε分别表示测量角增量和速度增量的惯性器件间的5个安装偏角；

δK_AY、δK_AZ分别表示测量加速度的Y方向和Z方向的惯性器件的刻度系数误差；

X_K/K-1＝φ_K/K-1·X_K-1

P_K-1＝P_K/K-1

X_K-1＝X_K/K-1

X_K＝X_K/K-1+K_K·(Z_K-H_K·X_K/K-1)

P_K＝(I-K_K·H_K)·P_K/K-1

P_K-1＝P_K

φ_K/K-1代表卡尔曼滤波估计中的φ的一步估计，T_F为滤波时间，Q_K-1为上一时刻噪声，X_K/K-1为X的一步预测，P_K/K-1为P的一步预测，X_K为k时刻的估计值，P_K为K时刻的噪声值，F为矩阵，P为设计的噪声误差阵初值；其中I为单位阵；Z_K为k时刻量测值，H_K为量测阵，Q为系统噪声方差阵，K_K为滤波增益；代表H_K的转置，代表φ_K/K-1的转置；

当满足零速校正条件时：

δL＝X′_K (1)

δλ＝X′_K (2)

X′_K(1)为X_K转置的第一个数据，X′_K(2)为X_K转置的第二个数据；

(21)为X_K一个周期内平均值的第21个数据，(22)为X_K一个周期内平均值的第22个数据；

否则：

δL＝0

δλ＝0

δD_N、δD_U保持不变；

·量测矩阵

其中：f_E、f_N、f_U分别代表东向、北向、天向的比力，即加速度计算值，分别代表一个周期内机体系X轴、Y轴、Z轴的比力的平均值，即加速度在机体系三个方向分量的平均值，ω_N、ω_U分别代表北向、天向的角速度，即角速度计算值，分别代表一个周期内机体系X轴、Y轴、Z轴的角速度的平均值，即角速度在机体系三个方向分量的平均值

零速校正时量测矩阵：

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接入点时，要对P_1,1、P_2,2进行重置：

P_1,1＝6e-4

P_2,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：导航结果显示

导航结果将会通过无线网络传输至终端显示器进行显示，从而完成室内导航的任务。

2.根据权利要求1所述的算法，其特征在于：根据多敏感组件进行惯性导航，并利用wifi信号进行实时比对修正，实现室内导航。