本发明涉及定位,尤其涉及一种基于泛在定位信号增强的个性化pdr定位方法及系统。
背景技术:
1、基于位置的服务(location-based services)具有诸多的应用市场,包括自动泊车、手机寻车、人员管理等,在室内外定位领域中非常普遍,越来越受到人们的关注。在基于行人的室内外定位中,由于室内外定位环境的复杂性以及行人之间的个体差异,容易导致步长估计不准确;同时,由于人的行走会带来运动加速度,这将降低水平姿态角的解算精度,进而降低对磁力计观测值的调平精度,最后影响航向角的解算精度。这两个因素同时导致了pdr(行人航迹推算)定位算法累积误差过大,因此有关室内外定位中pdr定位算法的研究已成为一个迫切需要解决的科学问题,相关的定位算法和技术成果不断涌现。
2、pdr算法主要包括两部分内容:步长估计和航向估计。目前,在常见的pdr定位算法中,其步长估计大多数采用步长估计模型的方法,基于离线训练好的模型参数实现实时的步长估计;其航向估计,主要是基于mars(磁力计、陀螺仪和加速度计)传感器,融合形成一套ahrs(航姿参考系统)算法,实现实时的航向估计。这两者共同组成了pdr定位算法,最终获得行人的定位结果。
3、本技术发明人在实施本发明的过程中,发现现有技术的方法,至少存在如下技术问题:
4、现有技术中,由于室内外定位环境的复杂性以及行人之间的个体差异,容易导致步长估计不准确;同时由于行人行走会带来运动加速度,容易导致水平姿态角不准,无法实现对磁力计观测值的准确调平,从而导致航向估计不准。两个因素共同导致了pdr定位算法累积误差过大。
5、由此可知,现有技术中的方法存在由于室内外定位环境的复杂性以及行人之间的个体差异导致步长估计不准确,以及由于行人运动加速度存在导致的航向估计不准,最终导致pdr定位算法累积误差过大的问题。
技术实现思路
1、本发明提出一种基于泛在定位信号增强的个性化pdr定位方法及系统,用以解决或者至少部分解决现有技术中存在的定位误差较大的技术问题。
2、为了解决上述技术问题,本发明技术方案为:
3、第一方面提供了基于泛在定位信号增强的个性化pdr定位方法,包括:
4、采用wrls算法基于slam里程计输出的后端实时位置参数对预设pdr步长估计模型的参数进行估计,得到估计参数,wrls算法为递归加权最小二乘算法;
5、根据得到的估计参数获得pdr步长估计模型,并利用pdr步长估计模型对步长进行估计;
6、采用后端实时姿态参数增强的ahrs算法对slam里程计输出的后端实时姿态参数和传感器数据进行融合,得到行人的航向,将估计得到的步长和行人的航向作为pdr定位方法的定位结果,后端实时姿态参数增强的ahrs算法为后端实时姿态参数增强的航姿参考系统算法。
7、在一种实施方式中,采用wrls算法基于slam里程计输出的后端实时位置参数对预设pdr步长估计模型的参数进行估计通过下述公式实现:
8、
9、
10、pk=(i-kkhk)pk-1 (3)
11、其中,k表示步数,kk表示增益矩阵,pk-1表示估计的第k-1步步长估计模型参数的估计方差,hk表示第k步步长估计模型的系数矩阵,rk表示第k步slam后端实时观测的步长的方差,表示估计的第k步步长估计模型的参数,作为估计参数,表示估计的第k-1步步长估计模型的参数,zk表示第k步slam后端实时观测的步长,即slam算法输出的后端实时位置参数,pk表示估计的第k步步长估计模型参数的估计方差。
12、在一种实施方式中,传感器数据包括磁力计数据、陀螺仪数据和加速度计数据。
13、在一种实施方式中,采用后端实时姿态参数增强的ahrs算法对slam里程计输出的后端实时姿态参数和传感器数据进行融合,包括:
14、定义系统状态向量、系统状态方程和量测方程,其中,系统状态向量的公式为:
15、
16、其中:δx表示系统状态向量,φ1×3表示失准角误差,表示陀螺零偏;
17、系统状态方程为:
18、δxt,t-1=φt-1δxt-1,t-1+wt
19、其中:δxt-1,t-1表示t-1时刻的系统状态向量,δxt,t-1表示预测得到的t时刻的系统状态向量,wt表示系统过程噪声,φt-1表示状态转移矩阵;
20、φt-1表示为:
21、
22、其中:i3×3表示单位矩阵,03×3表示0矩阵,表示t时刻b系相对于nc系的姿态变换矩阵,b(t)表示t时刻b系,即载体系,nc表示计算的世界坐标系,δt表示t-1时刻到t时刻的时间间隔;
23、量测方程为:
24、δzt=htδxt,t-1+vt
25、其中:δzt表示量测闭合差向量,ht表示量测矩阵,vt表示量测噪声,在量测方程中,slam里程计的后端实时姿态参数进行量测更新的量测方程为:
26、
27、
28、
29、其中,δz表示slam里程计后端实时姿态参数的量测闭合差向量,φ表示失准角误差,表示slam里程计输出的后端实时姿态参数的量测值,其形式如下所示:
30、
31、表示取其2:4的元素,v表示后端实时姿态参数的量测噪声,由sins姿态机械编排得到,表示对求共轭,表示四元数乘法,|φ|表示对φ求模值;
32、基于定义的系统状态向量、系统状态方程和量测方程基于卡尔曼滤波进行系统状态向量的求解:
33、δxt,t-1=φt-1δxt-1,t-1 (1)
34、
35、
36、δxt,t=δxt,t-1+kt(zt-htδxt,t-1) (4)
37、pt,t=(i-ktht)pt,t-1 (5)
38、其中,δxt,t-1表示预测出的t时刻的系统状态向量,φt-1表示状态转移矩阵,δxt-1,t-1表示t-1时刻的系统状态向量,pt,t-1表示预测出的t时刻的系统状态向量的方差,pt-1,t-1表示t-1时刻的系统状态向量的方差,qt-1表示系统过程噪声矩阵,kt表示增益矩阵,ht表示量测矩阵,rt表示t时刻的slam后端实时观测的姿态的方差,δxt,t表示估计的t时刻的系统状态向量,zt表示表示t时刻slam后端实时观测的姿态,即slam算法输出的后端实时姿态参数,pt,t表示估计的t时刻系统状态向量的方差。
39、基于同样的发明构思,本发明第二方面提供了基于泛在定位信号增强的个性化pdr定位系统,包括:
40、参数估计模块,用于采用wrls算法基于slam里程计输出的后端实时位置参数对预设pdr步长估计模型的参数进行估计,得到估计参数,wrls算法为递归加权最小二乘算法;
41、步长估计模块,用于根据得到的估计参数获得pdr步长估计模型,并利用pdr步长估计模型对步长进行估计;
42、航向估计模块,用于采用后端实时姿态参数增强的ahrs算法对slam里程计输出的后端实时姿态参数和传感器数据进行融合,得到行人的航向,将估计得到的步长和行人的航向作为pdr定位方法的定位结果,后端实时姿态参数增强的ahrs算法为后端实时姿态参数增强的航姿参考系统算法。
43、基于同样的发明构思,本发明第三方面提供了一种计算机可读存储介质,其上存储有计算机程序,该程序被执行时实现第一方面所述的方法。
44、基于同样的发明构思,本发明第四方面提供了一种计算机设备,包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序,所述处理器执行所述程序时实现第一方面所述的方法。
45、与现有技术相比,本发明提供的技术方案至少具有以下技术效果:
46、在实际的行人室内外定位环境中,当slam(同步定位与地图构建)正常工作时,在slam的里程计输出结果中,其后端实时位置参数具有很高的位置精度,其后端实时姿态参数具有很高的姿态精度。本发明采用其后端实时位置参数,基于wrls算法,对步长估计模型参数进行个性化估计,使得步长估计模型可以适应行人自身的步态特征以及实时的定位环境,进而提高步长估计精度;在ahrs算法中,融合其后端实时姿态参数,来增强水平姿态角的解算精度,以实现对磁力计观测更为精准的调平,进而提高航向角的估计精度。最终提高了pdr定位方法的精度。