[发明专利]一种基于位置指纹和PDR算法的融合人行定位方法有效
申请号: | 202110674546.5 | 申请日: | 2021-06-17 |
公开(公告)号: | CN113566820B | 公开(公告)日: | 2023-05-16 |
发明(设计)人: | 武畅;杨帆;吴鹏;张莹;游远;周奕 | 申请(专利权)人: | 电子科技大学 |
主分类号: | G01C21/20 | 分类号: | G01C21/20;G01C21/00;G01C21/16;G01S5/02 |
代理公司: | 北京正华智诚专利代理事务所(普通合伙) 11870 | 代理人: | 何凡 |
地址: | 611731 四川省成*** | 国省代码: | 四川;51 |
权利要求书: | 查看更多 | 说明书: | 查看更多 |
摘要: | |||
搜索关键词: | 一种 基于 位置 指纹 pdr 算法 融合 定位 方法 | ||
1.一种基于位置指纹和PDR算法的融合人行定位方法,其特征在于,包括以下步骤:
S1、采集参考点定位数据,并进行坐标转换,得到平面坐标系下定位数据,具体为:
从传感器上获取参考点定位数据,并通过欧拉角法进行坐标转换,得到导航坐标系到平面坐标系的转换矩阵,表示为:
其中,C为转换矩阵,θ为俯仰角,φ为航向角,γ为横滚角;
S2、利用位置指纹算法根据步骤S1平面坐标系下定位数据计算第一定位坐标;
S3、利用PDR算法根据步骤S1平面坐标系下定位数据计算第二定位坐标;具体包括以下分步骤:
S31、利用自适应阈值行走检测法根据步骤S1平面坐标系下定位数据检测步数;具体包括以下分步骤:
S311、利用步骤S1平面坐标下定位数据中各方向上的加速度,计算合成加速度,表示为:
其中,为合成加速度,ax、ay、az分别为X、Y、Z三个方向上的加速度值;
S312、采用平滑滤波法对步骤S311中合成加速度进行处理,表示为:
其中,为平滑滤波后的合成加速度,N为滤波数据的长度,t为当前时刻,为前t-h时刻的合成加速度值,h为时间刻度;
S313、根据当前时刻的合成加速度值判断上一时刻的合成加速度值是否处于局部峰值,若是则进入步骤S314,否则回到步骤S311;
S314、判断步骤S313中局部峰值是否处于预设峰值阈值范围,若是则进入步骤S315,否则返回步骤S311;
S315、计算当前波峰和上一波峰的时间差,并判断时间差是否处于预设时间间隔阈值范围,若是则步数加1,进入步骤S316,否则回到步骤S311;
S316、记录当前时刻和上一时刻的波峰值以及时间差,并判断当前记录次数是否处于预设次数,若是则进入步骤S317,否则回到步骤S311;
S317、计算预设次数下全部波峰以及时间差的均值,并利用该计算后的波峰均值以及时间差均值更新步骤S314中预设峰值阈值范围;其中利用波峰均值以及时间差均值更新步骤S314中预设峰值阈值范围,更新公式为:
athr=anew-(amax-athr)/2
amax=anew+(amax-athr)/2
其中,anew为波峰均值,amax为预设峰值中最大值,athr为预设峰值中最小值;
S318、判断是否满足预设停止计步条件,若满足则结束行走检测,否则回到步骤S311;
S32、利用非线性模型根据合成加速度计算单步步长;
S33、利用步骤S32的单步步长与步骤S1平面坐标系下定位数据中方向数据,计算第二定位坐标,表示为:
其中,(x0,y0)为待测点的初始位置,αk和Lk分别为第k步的方向角和步长,K为总步数,(xk,yk)为第二定位坐标;
S4、利用当前人行运动数据与历史人行运动数据计算融合判断值,并判断融合判断值是否满足预设融合阈值,若满足则进入步骤S5,否则以第一定位坐标为最终定位坐标;
S5、利用扩展卡尔曼滤波器对步骤S2第一定位坐标以及步骤S3第二定位坐标进行一阶融合,得到一阶融合数据;具体包括以下分步骤:
S51、采用扩展卡尔曼滤波器计算当前状态的状态预测向量,表示为:
其中,rk为当前状态的状态预测向量,为位置指纹算法计算的第k-1步状态最优估计对应的位置坐标,Lk为PDR算法计算的第k步的步长,αk为PDR算法计算的第k步的方向角,wk为过程噪声向量;
S52、利用过程噪声矩阵以及状态转移矩阵对步骤S51中状态预测向量的协方差矩阵进行更新,更新公式表示为:
其中,Pk为更新后的协方差矩阵,为位置指纹算法计算的历史状态最优估计的协方差矩阵,Qk为过程噪声矩阵,Φk为状态转移矩阵,为状态转移矩阵的转置;
S53、利用步骤S52中更新后的协方差矩阵计算当前状态的增益向量,表示为:
Kk=PkHT[HPkHT+Rk]-1
其中,Kk为当前状态的增益向量,H为观测矩阵,HT为观测矩阵的转置,Rk为测量噪声矩阵;
S54、利用步骤S53中增益向量、步骤S51中状态预测向量以及第二定位坐标中观测向量计算当前状态的最优估计,得到一阶融合数据,表示为:
其中,为当前状态的最优估计,zk为PDR算法计算的当前状态的观测向量;
S6、获取地图信息中建筑约束条件,利用粒子滤波根据步骤S5中一阶融合数据进行二阶融合,得到最终定位坐标;具体包括以下分步骤:
S61、采用粒子滤波将步骤S5中一阶融合数据作为初始位置,生成一组均匀分布的粒子;
S62、利用PDR算法更新位置坐标,更新公式表示为:
xk=xk-1+Lk-1*sinαk-1
yk=yk-1+Lk-1*cosαk-1
其中,xk和yk是第二定位坐标中当前状态坐标,xk-1和yk-1为第二定位坐标中历史状态坐标,Lk-1为第二定位坐标中历史状态步长,αk-1为第二定位坐标中历史状态坐标的方向角;
S63、利用测量权重与地图权重计算步骤S61中粒子权重,表示为:
wi=weighti_measure*weighti_map
其中,wi为第i个粒子权重,weighti_measure为测量权重;weighti_map为地图权重;
S64、对步骤S63中粒子权重进行归一化,得到粒子综合权重,表示为:
其中,w'p是计算的综合权重,wp是第p个粒子权重,P是粒子总数;
S65、对步骤S64中粒子综合权重对当前粒子进行重采样,得到最终定位坐标,表示为:
其中,X为最终融合坐标,是第二定位坐标中当前状态重采样后第p个粒子坐标。
2.根据权利要求1所述的一种基于位置指纹和PDR算法的融合人行定位方法,其特征在于,所述步骤S4包括以下分步骤:
S41、利用当前人行运动数据与历史人行运动数据计算融合判断值,表示为:
D=(s2-s1)(t2-t1)
其中,D为融合标准,t1为上一次利用PDR算法对行人位置校正的初始时间,s1为初始时间下行人的运动距离,t2为行人当前的测试时间,s2为当前时间下行人的运动距离;
S42、判断步骤S41中融合判断值是否满足预设融合阈值,若满足预设融合阈值,则进入步骤S5,否则以第一定位坐标为最终定位坐标。
该专利技术资料仅供研究查看技术是否侵权等信息,商用须获得专利权人授权。该专利全部权利属于电子科技大学,未经电子科技大学许可,擅自商用是侵权行为。如果您想购买此专利、获得商业授权和技术合作,请联系【客服】
本文链接:http://www.vipzhuanli.com/pat/books/202110674546.5/1.html,转载请声明来源钻瓜专利网。