利索能及
我要发布
收藏
专利号: 2020106268122
申请人: 广东工业大学
专利类型:发明专利
专利状态:已下证
更新日期:2025-08-05
缴费截止日期: 暂无
联系人

摘要:

权利要求书:

1.一种基于卡尔曼滤波的室内行人定位方法,其特征在于,其步骤包括:步骤1:获取待测行人预先携带的移动端中的加速度数据,并基于所述移动端中的加速度数据通过PDR算法计算所述待测行人相对于各个已知位置坐标的UWB基站的第k步的位置坐标估值与相对速度vi(k),相对速度vi(k)作为基于卡尔曼滤波的IMM算法的LOS模型和NLOS模型的观测值;

步骤2:通过获取所述UWB基站与所述移动端之间的超宽带通信数据计算所述待测行人相对第i个所述UWB基站的第k步的测距ri(k),其中,ri(k)=di(k)+εi(k),i=1,...,N,di(k)和εi(k)分别为所述待测行人第k步的相对于第i个所述UWB基站的测距真实值和在不同视距环境下的测距误差值,εi(k)用白高斯噪声表示为 根据LOS环境和NLOS环境,mε和 分别为:

2 2

式中,mNLOS、σLOS、σNLOS分别表示为NLOS环境的期望值、LOS环境的方差和NLOS环境的方差;

步骤3:根据所述测距真实值与所述不同视距环境下的测距误差值,分别建立基于卡尔曼滤波的IMM算法的LOS模型和NLOS模型;

步骤3具体包括:

步骤3.1:所述待测行人相对于第i个所述UWB基站的状态向量表示为:表示为待测行人的第k步相对于第i个所述UWB基站的速度真实值,转换为动态状态方程是:xi(k)=Fxi(k‑1)+Ebi(k‑1),T

zi(k)=[ri(k) vi(k)]其中, bi(k‑1)为过程驱动噪声,ri(k)=di(k)+εi(k),Tk表示时间间隔;

步骤3.2:根据LOS模型和NLOS模型条件,动态状态方程可以转化为,xi(k)=Fxi(k‑1)+Ebi(k‑1)

T

zi(k)=[ri(k) vi(k)] ,其中,vi(k)表示为待测行人相对于第i个基站的相对速度,mε由所述步骤2中根据LOS环境或NLOS环境选定,从而确定基于卡尔曼滤波的IMM算法的LOS模型和NLOS模型;

步骤4:通过马尔科夫状态转移概率矩阵对所述LOS模型和所述NLOS模型的权重进行转换,并通过基于卡尔曼滤波的IMM算法分别计算出所述LOS模型和所述NLOS模型的相应测距,并根据测距精准度分别对所述测距进行评估权重并加权,以获得平滑的精准测距值平滑的精准测距值作为扩展卡尔曼滤波的观测值;

步骤5:基于扩展卡尔曼滤波算法,将基于PDR算法所获得的位置坐标估值与基于卡尔曼滤波的IMM算法所获得的精准测距值融合,进而计算并确定所述待测行人的位置坐标;

步骤6:重复执行所述步骤1‑步骤5,进而获得所述待测行人的每一步位置坐标,从而实现精准定位与追踪所述待测行人轨迹;

所述步骤4具体包括:

步骤4.1:基于功率自适应卡尔曼滤波的IMM平滑器功率自适应调整,具体为通过引用环境参数Θi(k)来表示所述待测行人的第k步相对于第i个所述UWB基站信道之间所处的环境,环境参数定义为:

其中,Θi(k)的初始值Θi(0)=0;

环境变化程度计算得到τi(k)=|Θi(k)‑Θi(k‑1)|,根据环境变化程度获得马尔科夫状态转移概率矩阵w为

步骤4.2:当q,h∈1,2,1是LOS平滑器,2是NLOS平滑器,根据基于卡尔曼滤波的IMM算(m)

法,选取马尔科夫状态转移概率矩阵w (m=1,2,3),则所述待测行人的第k‑1步从模型q到模型h的混合概率为:

(m)

其中, 为w 中的某一概率,归一化因子 可以计算为:卡尔曼滤波的初始状态向量为 和协方差矩阵为Wi,0h(k‑1|k‑1),由第k‑1步的状态向量 和协方差Wi,q(k‑1|k‑1)和混合概率ui,q|h(k‑1|k‑1)计算得到:

步骤4.3:匹配卡尔曼滤波,具体为根据所述步骤3.1中的动态状态方程,模型h计算为:xi,h(k|k)=xi,h(k|k‑1)+Ki,h(k)ei,h(k),其中, Ki,h(k)为卡尔曼增益,ei,h(k)为预测误差,H为diag(11),Ki,h(k)和协方差矩阵Wi,h(k|k)可以计算为:

T T T ‑1

Ki,h(k)=Wi,h(k|k‑1)H[HWi,h(k|k‑1)H+HRH]Wi,h(k|k)=[I‑Ki,h(k)H]Wi,h(k|k‑1)

2 2

其中, 为测量噪声矩阵,(σε,i,hση,i)表示为测量噪声矩阵的方差,I是单位矩阵,F为系数矩阵, 为卡尔曼滤波的初始状态向量;

步骤4.4:模型概率更新,具体为残差协方差矩阵Si,h(k)和似然函数Λi,h(k)计算分别为,

T

Si,h(k)=HWi,h(k|k‑1)H+RΛi,h(k)=N(ei,h(k);0,Si,h(k)),其中,N(ei,h(k);0,Si,h(k))为ei,h(k)的高斯密度函数,其均值为0,协方差为Si,h(k),模型概率更新为:其中,

步骤4.5:模型融合,具体为模型h与模型q融合后的 和相应的协方差矩阵Wi,h(k|k)得到:

从而得到经过基于功率自适应的卡尔曼滤波的IMM平滑器的精准测距其精准测距将作为扩展卡尔曼滤波的观测值。

2.根据权利要求1所述的基于卡尔曼滤波的室内行人定位方法,其特征在于,所述步骤

1具体包括:

步骤1.1:通过加速度峰值检测方法检测所述待测行人的步行频率 将所述待测行人第K步的时间记录为t′k,则得到所述待测行人的步行频率 与其上一步的时间间隔Tk的关系为

步骤1.2:基于PDR算法对所述待测行人在第k步进行位置估值并得到位置坐标用矩阵T

表示为P(k|k‑1)=[xp(k|k‑1),yp(k|k‑1)];

步骤1.3:通过PDR算法计算所述待测行人相对于第i个所述UWB基站的相对速度,PDR算法为

其中, 表示为待测行人的第k‑1步的x值, 表示为待测行人的第k ‑ 1 步的 y 值 , 表示 为 待 测行 人 的 第k ‑ 1 步的 角度 值 ,与 为PDR算法的第K‑1步输出值,χ(k‑1)为所述移动端中的角速度,S(k)为第k步的步长,可以计算为其中o为常数,根据性别男与性别女分别设定为0.3139和0.2975,h为所述待测行人的身高,则根据所述UWB基站的已知坐标得到所述待测行人相对于第i个所述UWB基站的相对速度vi(k)计算为, Pi表示为基站的二维坐标;另外,相对速度vi(k)也可计算为 其中, 与hi(k)分别表示所述待测行人的第k步相对于第i个所述UWB基站的速度真实值和速度误差。

3.根据权利要求1所述的基于卡尔曼滤波的室内行人定位方法,其特征在于,所述步骤

5包括:

步骤5.1:扩展卡尔曼滤波的动态状态方程为:VM(k‑1)为过程驱动噪声,其为均值为0和协方差矩阵为Q的白高斯噪声, 表示为待测行人第k‑1步的x轴坐标值,S(k)表示为第k步的步长, 表示为第k‑1步的行人方向余弦角, 表示为待测行人第k‑1步的y轴坐标值, 表示为第k‑1步的行人方向正弦角, 表示为第k‑1步的行人方向角,χ(k‑1)表示为第k‑1步的角速度,Tk表示为时间间隔;通过一阶泰勒泰勒函数将状态方程线性化得到:而观测方程为:

GM(k)是测量噪声,其为均值为0和协方差矩阵为D的白高斯噪声, 表示为待测行人第k步的二维坐标,观测方程的雅克比矩阵为:步骤5.2:状态估计预测为

观测估计预测为

状态协方差矩阵预测计算为 UM(k‑1|k‑

1)为第k‑1步的状态协方差矩阵更新,卡尔曼增益为

则状态估计更新为XM(k|k)=XM(k|k‑1)+OM(k)(ZM(k)‑ZM(k|k‑1)),ZM(k)=[d1(k)…dN(k)φ(k)],进而确定所述待测行人位置坐标,其中,由基于功率自适应的卡尔曼IMM滤波算法得到,φ(k)为所述移动端中的Z轴角度,状态协方差更新为UM(k|k)=[I‑OM(k)JM(k)]UM(k|k‑1),I是单位矩阵。

4.根据权利要求3所述的基于卡尔曼滤波的室内行人定位方法,其特征在于,所述步骤T

6中的所述待测行人的每一步位置坐标表示为P(k|k)=[xp(k),xp(k)] , 由所述步骤5.2中的状态估计更新公式的 得到。

5.一种基于卡尔曼滤波的室内行人定位装置,基于权利要求1所述的基于卡尔曼滤波的室内行人定位方法,其特征在于,包括:第一获取模块,用于获取移动端内的惯性测量单元的加速度数据;

第一计算模块,用于基于预设的PDR算法通过所述第一获取模块获取的加速度数据计算待测行人相对于各个所述UWB基站的第k步的位置坐标估值与相对速度;

第二获取模块,用于根据基站与移动端之间的超宽带通信数据来获取待测行人相对第i个所述UWB基站的第k步的测距、测距真实值与在不同视距环境下的测距误差值;

第一处理模块,用于根据所述第二获取模块获取的所述测距真实值与不同视距环境下的测距误差值分别建立基于功率自适应的卡尔曼滤波的IMM算法的LOS模型和NLOS模型;

第二处理模块,用于基于预设的马尔科夫状态转移概率矩阵对所述LOS模型和所述NLOS模型的权重进行转换;

第二计算模块,用于根据预设的基于功率自适应的卡尔曼滤波的IMM算法对经所述马尔科夫状态转移概率矩阵转换后的所述LOS模型和所述NLOS模型分别计算并得出对应的测距,并根据测距精准度分别对测距进行评估权重并加权,以获得平滑的精准测距值;

第三计算模块,用于基于预设的扩展卡尔曼滤波算法将所述第一计算模块得出的位置坐标估值与基于功率自适应的卡尔曼滤波的IMM平滑器所获得的精准测距值融合计算,进而获得所述待测行人的位置坐标与轨迹。

6.一种基于卡尔曼滤波的室内行人定位系统,基于权利要求1所述的基于卡尔曼滤波的室内行人定位方法,其特征在于,包括:至少三个UWB基站,设于室内环境内预设位置上,所述UWB基站设有第一超宽带模块;

移动端,设于待测行人身上,所述移动端设有惯性测量单元以及与所述第一超宽带模块超宽带通讯连接的第二超宽带模块;

接收器,用于接收所述惯性测量单元所发送的加速度数据,接收所述第一超宽带模块与所述第二超宽带模块分别发出的测距信息,所述测距信息包括所述待测行人相对第i个所述UWB基站的第k步的测距、测距真实值与不同视距环境下的测距误差值;

服务器,用于根据预设的PDR算法,将所述接收器接收的所述加速度数据进行计算,进而确定待测行人相对于各个所述UWB基站的第k步的位置坐标估值与相对速度,根据所述接收器接收的所述测距真实值与所述不同视距环境下的测距误差值的建立基于功率自适应的卡尔曼滤波的IMM平滑器的LOS模型和NLOS模型,根据基于预设的马尔科夫状态转移概率矩阵对所述LOS模型和所述NLOS模型的权重进行转换;

基于功率自适应的卡尔曼滤波的IMM平滑器,用于对经所述马尔科夫状态转移概率矩阵转换后的所述LOS模型和所述NLOS模型分别计算并得出对应的测距,并根据测距精准度分别对测距进行评估权重并加权,以获得平滑的精准测距值;

终端,用于根据预设的扩展卡尔曼滤波算法将所述服务器计算的位置坐标估值与基于功率自适应的卡尔曼滤波的IMM平滑器所获得的精准测距值融合计算,进而获得所述待测行人的位置坐标与轨迹。