CN117308931A - 一种基于wifi和惯性测量单元的导航定位方法 - Google Patents

Abstract

本发明提供一种基于WIFI和惯性测量单元的导航定位方法，包括：根据信道状态信息进行建模，得到WIFI指纹数据库；对WIFI指纹数据库进行自适应的加权K近邻处理，匹配得到目标对象在当前时刻的第一位置信息；获取惯性测量单元采集得到的惯性测量数据，根据惯性测量数据推算出目标对象在下一时刻的第二位置信息；建立一融合定位模型，对第二位置信息进行状态更新，对第一位置信息进行测量更新，并输出位置估计值。有益效果：将WIFI定位和IMU定位互补融合，使用信道状态信息进行数据库建模，避免多径效应的影响；使用自适应加权K近邻算法进行匹配定位，提高指纹定位精度；对WIFI‑PDR进行融合修正，避免WIFI定位的不稳定性和PDR定位带来的累计误差。

Description

一种基于WIFI和惯性测量单元的导航定位方法

技术领域

本发明涉及导航技术领域，尤其涉及一种基于WIFI和惯性测量单元的导航定位方法。

背景技术

在开阔无遮挡的室外环境下，使用全球卫星导航系统就可以实现较为准确的定位，但是当有墙壁、树木等障碍物遮挡时，GNSS信号就会快速衰减，因此，使用GNSS卫星信号并不能够满足室内精确导航与定位的需求。

常见的室内环境，例如商场、医院、写字楼等地点，使用WIFI位置指纹定位技术不需要再安装额外的设备，成本也会相应降低，但是由于室内复杂的环境和WIFI信号的波动性，使得单纯使用WIFI定位技术的定位精度并不高。

WIFI和IMU融合的定位技术克服了单一技术带来的局限性，得到了越来越多的关注，发明专利(201710323757.8)公开了一种基于WIFI与PDR的室内定位方法，该方法通过预先设定初始位置和步长，利用加速度传感器测量的加速度对行人的运动状态进行判定，该方法PDR初始位置为自行定位，没有考虑行人航位推算(Pedestrian Dead Reckoning，PDR)的融合校准，WIFI定位的不稳定性以及PDR定位带来的累计误差，会降低得到的定位坐标精度，并且对于RSSI数据来说由于室内多径效应的影响，使得不同路径上的信号会经历不同程度的衰减和时延，且测量的是信号多径传播的叠加效果，并不能逐一区分多条信号传播路径，使其只能用于实现一些粗糙的室内定位。

发明内容

为了解决以上技术问题，本发明提供了一种基于WIFI和惯性测量单元的导航定位方法。

本发明所解决的技术问题可以采用以下技术方案实现：

一种基于WIFI和惯性测量单元的导航定位方法，包括：

步骤S1，根据信道状态信息进行建模，得到WIFI指纹数据库；

步骤S2，对所述WIFI指纹数据库进行自适应的加权K近邻处理，匹配得到目标对象在当前时刻的第一位置信息；

步骤S3，获取惯性测量单元采集得到的惯性测量数据，根据所述惯性测量数据推算出所述目标对象在下一时刻的第二位置信息；

步骤S4，建立一融合定位模型，对所述第二位置信息进行状态更新，对所述第一位置信息进行测量更新，并输出位置估计值。

优选地，所述步骤S4之后还包括：

将输出的所述位置估计值作为反馈信息，根据所述反馈信息对所述惯性测量单元中陀螺仪和磁力计输出的航向角进行在线修正。

优选地，所述步骤S1包括：

步骤S11，采集所述信道状态信息并进行预处理；

步骤S12，获取预处理后的所述信道状态信息中所有参考点的指纹向量，根据指纹向量集合建立得到所述WIFI指纹数据库。

优选地，所述步骤S11中，所述预处理为离散小波变换处理。

优选地，所述步骤S2包括：

步骤S21，获取与每一参考点的相似度度量值处于一预设搜索范围内的测试点，确定加权K近邻处理中K的自适应取值；

步骤S22，获取确定的K的自适应取值对应的指纹位置，对K个的所述指纹位置进行加权求和得到所述当前时刻的第一位置信息。

优选地，所述步骤S21中，若所述预设搜索范围内未搜索到所述测试点，则增大搜索范围，直至搜索到所述测试点。

优选地，所述步骤S21中，K的自适应取值采用下述公式确定：

其中，sim(r,j)表示第r个测试点与朝向°的第j个参考点间的相似度度量值；b·simR_j表示所述第j个参考点的所述预设搜索范围，b表示搜索范围系数，simR_j表示所述第j个参考点的相似度邻域半径；

所述步骤S22中，采用下述公式计算得到所述当前时刻的第一位置信息：

其中，ω_I＝1/sim(r,I)°，ω_I表示权重，sim(r,I)表示第r个测试点与第I个参考点间的相似度度量值；LOC(x_I,y_I)表示搜索到测试点的第I个参考点的指纹位置；LOC(x,y)表示所述当前时刻的第一位置信息。

优选地，所述步骤S2中，以所述WIFI指纹数据库匹配得到所述第一位置信息作为推算的初始位置。

优选地，所述步骤S3包括：

通过检测所述目标对象的加速度是否超过加速度阈值，且两次超过的时间间隔在预设时间阈值范围内，对所述目标对象进行步态探测；其中，所述加速度阈值通过计步周期内加速度的均值对加速度阈值进行动态调整得到；

基于步频和加速度方差的线性步长模型进行步长估计；

对上一时刻的航向角和所述反馈信息中的航向角进行加权平均得到当前时刻的航向角。

优选地，所述步骤S4包括：

步骤S41，基于拓展卡尔曼滤波构建得到所述融合定位模型，所述融合定位模型的状态方程为：

其中，X_k表示k时刻系统的状态向量；E_k、E_k-1分别表示k时刻、k-1时刻地理坐标系中的东向坐标；N_k、N_k-1分别表示k时刻、k-1时刻地理坐标系中的北向坐标；S_k、S_k-1分别表示所述目标对象在k时刻、k-1时刻的步长；Φ_k、Φ_k-1分别表示所述目标对象在k时刻、k-1时刻的航向角；W_E、W_N分别表示所述东向坐标、所述北向坐标的噪声矩阵；W_S表示所述步长的噪声矩阵；W_Φ表示所述航向角的噪声矩阵；

所述融合定位模型的观测方程为：

其中，表示k时刻WIFI指纹数据库匹配得到的东向坐标；/>表示k时刻WIFI指纹数据库匹配得到的北向坐标；/>表示k时刻惯性测量推算得到的步长；/>表示k时刻惯性测量推算得到的航向角；v_E、v_N分别表示所述东向坐标、所述北向坐标的噪声矩阵；v_S表示所述步长的噪声矩阵；v_Φ表示所述航向角的噪声矩阵；

步骤S42，分别对所述状态方程和所述观测方程求雅可比矩阵，得到对应的状态转移矩阵和观测矩阵：

其中，F_k表示状态转移矩阵；H_k表示观测矩阵；

步骤S42，根据求解得到的所述状态转移矩阵和所述观测矩阵对推算得到的所述第二位置信息进行更新。

本发明技术方案的优点或有益效果在于：

本发明将WIFI定位和IMU定位互补融合，并且使用信道状态信息进行数据库建模，更好的避免多径效应的影响，使用自适应加权K近邻算法进行在线匹配定位，提高指纹定位的精度；利用拓展卡尔曼滤波算法对WIFI-PDR进行融合修正，避免WIFI定位的不稳定性和PDR定位带来的累计误差。

附图说明

图1为本发明的较佳实施例中，基于WIFI和惯性测量单元的导航定位方法的流程示意图；

图2为本发明的较佳实施例中，基于WIFI和惯性测量单元的导航定位方法的算法框图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。

需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。

下面结合附图和具体实施例对本发明作进一步说明，但不作为本发明的限定。

参见图1和图2，本发明的较佳的实施例中，基于现有技术中存在的上述问题，现提供一种基于WIFI和惯性测量单元的导航定位方法，包括：

步骤S1，根据信道状态信息(Channel State Information，CSI)进行建模，得到WIFI指纹数据库；

步骤S2，对WIFI指纹数据库进行自适应的加权K近邻(weighted K-nearestneighbor，WKNN)处理，匹配得到目标对象在当前时刻的第一位置信息；

其中，目标对象可以是人或者移动的物体或其他需要进行定位的对象，本发明实施例以行人为例进行说明。

步骤S3，获取惯性测量单元采集得到的惯性测量数据，根据惯性测量数据推算出目标对象在下一时刻的第二位置信息；

步骤S4，建立一融合定位模型，对第二位置信息进行状态更新，对第一位置信息进行测量更新，并输出位置估计值。

具体的，在本实施例中，根据采集得到的CSI数据建立WIFI指纹数据库，WIFI指纹数据库为CSI离线指纹数据库；然后通过自适应的WKNN在线匹配定位方法，获得行人在当前时刻的匹配位置，即第一位置信息；将WIFI指纹定位匹配到的行人在当前时刻的第一位置信息提供给行人航位推算(Pedestrian Dead Reckoning，PDR)算法，并通过惯性测量单元(IMU)中的陀螺仪、磁力计和加速度计对行人的步态探测、步长估计和航向角估算推算出行人下一时刻的位置。融合定位模型获得行人在当前时刻的位置和航向角的误差量，求解到状态转移矩阵和观测矩阵，使用这些数据对PDR推算出的位置坐标进行更新。

本发明实施例通过将WIFI定位和IMU定位互补融合，并且使用信道状态信息进行数据库建模，能够更好的避免多径效应的影响；使用自适应加权K近邻算法进行在线匹配定位，提高指纹定位的精度；基于融合定位模型对WIFI-PDR进行融合修正，避免WIFI定位的不稳定性和PDR定位带来的累计误差。

作为优选的实施方式，其中，步骤S4之后还包括：

将输出的位置估计值作为反馈信息，根据反馈信息对惯性测量单元中陀螺仪和磁力计输出的航向角进行在线修正。

具体的，在本实施例中，将融合定位模型输出的位置作为反馈信息，用于行人航位推算算法中航向角估算的在线修正。

作为优选的实施方式，其中，步骤S1包括：

步骤S11，采集信道状态信息并进行预处理；

步骤S12，获取预处理后的信道状态信息中所有参考点的指纹向量，根据指纹向量集合建立得到WIFI指纹数据库。

具体的，考虑到大部分室内环境中的WIFI无线设备采集到的CSI数据噪声较大，在本实施例中，需要对采集到的CSI数据包中信道状态信息进行预处理，从而降低噪声；然后，定义经过数据预处理后的第i个参考点的CSI幅值向量为向量长度为30，包含30个子载波的幅度值，第i个参考点的位置坐标为p_i，则第i个参考点的指纹向量为：

假设有m个参考点，将所有参考点的指纹向量集合，即可建立指纹数据库F，算法如下：

F_i＝[H_i，p_i]

其中，ψ(t)表示平方可积函数，w表示信号频率，将a，τ分别定义为伸缩因子和平移因子，CSI数据包中信道状态信息定义为x(t)，W_x(j,k)表示变换后的信道状态信息，H_j表示信道状态信息中单个子载波的振幅，H表示经过预处理后的信道状态信息的振幅信息，F表示建立的WIFI指纹数据库。

作为优选的实施方式，其中，步骤S11中，预处理为离散小波变换处理。

具体的，在本实施例中，使用离散小波变换对信道状态信息进行预处理，可以满足较高的定位实时性要求。CSI数据包中所有的子载波原始信号可以通过此变换分解为高频分量和低频分量，将高频分量的高频系数设为0，并且对其进行反变换，即可得到滤波后的信号，振幅信息相比于相位信息来说更加稳定。

作为优选的实施方式，其中，步骤S2包括：

步骤S21，获取与每一参考点的相似度度量值处于一预设搜索范围内的测试点，确定加权K近邻处理中K的自适应取值；

步骤S22，获取确定的K的自适应取值对应的指纹位置，对K个的指纹位置进行加权求和得到当前时刻的第一位置信息。

具体的，在本实施例中，为了获得较好的定位结果，WKNN算法需要对比不同K值的结果，本发明实施例通过对自适应动态K的WKNN算法进行优化以满足在线匹配定位要求，K的自适应取值通过“多雷达搜索”的方式在基于相似度度量构建的地图上获取，以每一个参考点上的相似度邻域半径(即发射距离)作为搜索范围，搜寻目标，雷达的搜索目标为测试点，雷达个数，即K的自适应取值采用下述公式确定：

其中，sim(r,j)表示第r个测试点与朝向°的第j个参考点间的相似度度量值；b·simR_j表示第j个参考点的预设搜索范围，b表示搜索范围系数，simR_j表示第j个参考点的相似度邻域半径；

作为优选的实施方式，其中，步骤S21中，若预设搜索范围内未搜索到测试点，则增大搜索范围，直至搜索到测试点。

具体的，每个参考点以b·simR_j(b＝1，2，3…)为搜索范围发射信号，不断加大b值直到搜寻到信号为止。

位置估计采用K个最有可能的位置加权求和得到，即当前时刻的第一位置信息采用下述公式计算得到：

其中，ω_I＝1/sim(r,I)°，ω_I表示权重，sim(r,I)表示第r个测试点与第I个参考点间的相似度度量值；LOC(x_I,y_I)表示搜索到测试点的第I个参考点的指纹位置；LOC(x,y)表示当前时刻的第一位置信息。

作为优选的实施方式，其中，步骤S2中，以WIFI指纹数据库匹配得到第一位置信息作为推算的初始位置。

作为优选的实施方式，其中，步骤S3包括：

通过检测目标对象的加速度是否超过加速度阈值，且两次超过的时间间隔在预设时间阈值范围内，对目标对象进行步态探测；其中，加速度阈值通过计步周期内加速度的均值对加速度阈值进行动态调整得到；

基于步频和加速度方差的线性步长模型进行步长估计；

对上一时刻的航向角和反馈信息中的航向角进行加权平均得到当前时刻的航向角。

具体的，在本实施例中，PDR定位的初始位置由WIFI定位提供，然后通过惯性传感器中的陀螺仪、磁力计和加速度计通过对行人的步态探测、步长估计和航向角估算三部分推算出行人下一时刻的位置，假定行人在每一步的航向角不变，则可得PDR模型。

在步态探测过程中，本发明实施例在峰值法的基础上通过加入动态阈值法来对行人进行步态探测，减小由于安装和行人行走习惯带来的误判，提高步态探测的准确性；通过对三轴加速度的合成，减小由于传感器的安装带来的误差；通过计步周期内加速度的均值对加速度阈值进行动态调整，从而满足各种行走行为。

在步长估计过程中，本发明实施例选择基于步频和加速度方差的线性步长模型对步长进行估计。

在航向角估算过程中，本发明实施例提供一种在线修正法，通过上一时刻融合得到的航向角对当前的航向角进行反馈修正，通过对上一时刻航向角和融合得到的航向角进行加权平均得到当前航向角。

PDR算法如下：

其中，δ表示加速度阈值；t_k-1，t_k表示相邻两次加速度正向超过阈值的时间点，阈值初始化设置为重力加速度g，时间阈值范围设为[0.3s,1s]，单个周期内的加速度峰值a_max与δ之间的阈值差范围设为[0.2g,2g]，若a两次超过δ，且时间间隔Δt在时间阈值范围内，阈值差Δλ在阈值差范围内，就可认为走了一步，即t_k-1和t_k是一个计步周期的初始点和结束点。T_k表示步频；δ^a _k表示加速度的方差；a_k和分别表示加速度及其方差；n表示采样点数目；S_k表示步长；α和β为权重因子，γ为经验常量，其具体值根据实际需要或实验训练得出，本发明实施例中对此不作限定；/>和/>为当前时刻和上一时刻行人真正的航向角；表示上一时刻融合定位得到的航行角；ω(t)表示陀螺仪测得的角速度；μ和v分别表示两种航向角的权值，其具体值根据实际需要或实验训练得出，本发明实施例中对此不作限定；E、N分别表示行人的东向坐标和北向坐标；φ表示行走时的航向角；S表示行走的步长。

作为优选的实施方式，其中，步骤S4包括：

步骤S41，基于拓展卡尔曼滤波(Extended Kalman Filter，EKF)构建得到融合定位模型，综合WIFI定位和IMU定位的优点，融合定位模型的状态方程为：

其中，X_k表示k时刻系统的状态向量；E_k、E_k-1分别表示k时刻、k-1时刻地理坐标系中的东向坐标；N_k、N_k-1分别表示k时刻、k-1时刻地理坐标系中的北向坐标；S_k、S_k-1分别表示目标对象在k时刻、k-1时刻的步长；Φ_k、Φ_k-1分别表示目标对象在k时刻、k-1时刻的航向角；W_E、W_N分别表示东向坐标、北向坐标的噪声矩阵；W_S表示步长的噪声矩阵；W_Φ表示航向角的噪声矩阵；

噪声矩阵W_E、W_N、W_S、均为4×1的矩阵，表示如下：

融合定位模型的观测方程为：

其中，表示k时刻WIFI指纹数据库匹配得到的东向坐标；/>表示k时刻WIFI指纹数据库匹配得到的北向坐标；/>表示k时刻惯性测量推算得到的步长；/>表示k时刻惯性测量推算得到的航向角；v_E、v_N分别表示东向坐标、北向坐标的噪声矩阵；v_S表示步长的噪声矩阵；v_Φ表示航向角的噪声矩阵；

步骤S42，分别对状态方程和观测方程求雅可比矩阵，得到对应的状态转移矩阵和观测矩阵：

其中，F_k表示状态转移矩阵；H_k表示观测矩阵；

步骤S42，根据求解得到的状态转移矩阵和观测矩阵对推算得到的第二位置信息进行更新。

具体的，通过EKF获得行人在当前时刻的位置和航向角的误差量，求解到状态转移矩阵和观测矩阵，使用这些数据对PDR推算出的位置坐标进行更新，获得的位置坐标作为反馈信息用于航向角估算的在线修正。

采用上述技术方案的优点或有益效果在于：本发明将WIFI定位和IMU定位互补融合，并且使用信道状态信息进行离线数据库建模，基于离散小波变换对CSI进行降噪预处理，然后建立CSI位置指纹数据库，更好的避免多径效应的影响，使用一种优化的自适应加权K近邻算法进行在线匹配定位，从而提高CSI指纹定位的精度。该方法还考虑由于WIFI定位的不稳定性和PDR定位带来的累计误差，使用拓展卡尔曼滤波融合校准进行减小，利用EKF算法对WiFi-PDR数据进行融合修正，建立融合定位模型，实现了行人的实时定位。

以上仅为本发明较佳的实施例，并非因此限制本发明的实施方式及保护范围，对于本领域技术人员而言，应当能够意识到凡运用本说明书及图示内容所作出的等同替换和显而易见的变化所得到的方案，均应当包含在本发明的保护范围内。

Claims (10)

1.一种基于WIFI和惯性测量单元的导航定位方法，其特征在于，包括：

步骤S1，根据信道状态信息进行建模，得到WIFI指纹数据库；

步骤S2，对所述WIFI指纹数据库进行自适应的加权K近邻处理，匹配得到目标对象在当前时刻的第一位置信息；

步骤S3，获取惯性测量单元采集得到的惯性测量数据，根据所述惯性测量数据推算出所述目标对象在下一时刻的第二位置信息；

步骤S4，建立一融合定位模型，对所述第二位置信息进行状态更新，对所述第一位置信息进行测量更新，并输出位置估计值。

2.根据权利要求1所述的基于WIFI和惯性测量单元的导航定位方法，其特征在于，所述步骤S4之后还包括：

将输出的所述位置估计值作为反馈信息，根据所述反馈信息对所述惯性测量单元中陀螺仪和磁力计输出的航向角进行在线修正。

3.根据权利要求1所述的基于WIFI和惯性测量单元的导航定位方法，其特征在于，所述步骤S1包括：

步骤S11，采集所述信道状态信息并进行预处理；

步骤S12，获取预处理后的所述信道状态信息中所有参考点的指纹向量，根据指纹向量集合建立得到所述WIFI指纹数据库。

4.根据权利要求3所述的基于WIFI和惯性测量单元的导航定位方法，其特征在于，所述步骤S11中，所述预处理为离散小波变换处理。

5.根据权利要求1所述的基于WIFI和惯性测量单元的导航定位方法，其特征在于，所述步骤S2包括：

步骤S21，获取与每一参考点的相似度度量值处于一预设搜索范围内的测试点，确定加权K近邻处理中K的自适应取值；

步骤S22，获取确定的K的自适应取值对应的指纹位置，对K个的所述指纹位置进行加权求和得到所述当前时刻的第一位置信息。

6.根据权利要求5所述的基于WIFI和惯性测量单元的导航定位方法，其特征在于，所述步骤S21中，若所述预设搜索范围内未搜索到所述测试点，则增大搜索范围，直至搜索到所述测试点。

7.根据权利要求5所述的基于WIFI和惯性测量单元的导航定位方法，其特征在于，所述步骤S21中，K的自适应取值采用下述公式确定：

其中，sim(r，j)表示第r个测试点与朝向°的第j个参考点间的相似度度量值；b·simR_j表示所述第j个参考点的所述预设搜索范围，b表示搜索范围系数，simR_j表示所述第j个参考点的相似度邻域半径；

所述步骤S22中，采用下述公式计算得到所述当前时刻的第一位置信息：

其中，ω_I＝1/sim(r，I)°，ω_I表示权重，sim(r，I)表示第r个测试点与第I个参考点间的相似度度量值；LOC(x_I，y_I)表示搜索到测试点的第I个参考点的指纹位置；LOC(x，y)表示所述当前时刻的第一位置信息。

8.根据权利要求1所述的基于WIFI和惯性测量单元的导航定位方法，其特征在于，所述步骤S2中，以所述WIFI指纹数据库匹配得到所述第一位置信息作为推算的初始位置。

9.根据权利要求2所述的基于WIFI和惯性测量单元的导航定位方法，其特征在于，所述步骤S3包括：

通过检测所述目标对象的加速度是否超过加速度阈值，且两次超过的时间间隔在预设时间阈值范围内，对所述目标对象进行步态探测；其中，所述加速度阈值通过计步周期内加速度的均值对加速度阈值进行动态调整得到；

基于步频和加速度方差的线性步长模型进行步长估计；

对上一时刻的航向角和所述反馈信息中的航向角进行加权平均得到当前时刻的航向角。

10.根据权利要求1所述的基于WIFI和惯性测量单元的导航定位方法，其特征在于，所述步骤S4包括：

步骤S41，基于拓展卡尔曼滤波构建得到所述融合定位模型，所述融合定位模型的状态方程为：

其中，X_k表示k时刻系统的状态向量；E_k、E_k-1分别表示k时刻、k-1时刻地理坐标系中的东向坐标；N_k、N_k-1分别表示k时刻、k-1时刻地理坐标系中的北向坐标；S_k、S_k-1分别表示所述目标对象在k时刻、k-1时刻的步长；Φ_k、Φ_k-1分别表示所述目标对象在k时刻、k-1时刻的航向角；W_E、W_N分别表示所述东向坐标、所述北向坐标的噪声矩阵；W_S表示所述步长的噪声矩阵；W_Φ表示所述航向角的噪声矩阵；

所述融合定位模型的观测方程为：

其中，表示k时刻WIFI指纹数据库匹配得到的东向坐标；/>表示k时刻WIFI指纹数据库匹配得到的北向坐标；/>表示k时刻惯性测量推算得到的步长；/>表示k时刻惯性测量推算得到的航向角；v_E、v_N分别表示所述东向坐标、所述北向坐标的噪声矩阵；v_S表示所述步长的噪声矩阵；v_Φ表示所述航向角的噪声矩阵；

步骤S42，分别对所述状态方程和所述观测方程求雅可比矩阵，得到对应的状态转移矩阵和观测矩阵：

其中，F_k表示状态转移矩阵；H_k表示观测矩阵；

步骤S42，根据求解得到的所述状态转移矩阵和所述观测矩阵对推算得到的所述第二位置信息进行更新。