本发明属于室内行人定位技术领域,涉及一种室内环境已知条件下基于微机电系统(micro-electro-mechanicalsystem,mems)技术的行人惯性导航系统(inertialnavigationsystem,ins)同步定位、匹配算法结构设计与位置信息融合的三维地图匹配方法。
背景技术:
近些年,随着物流、智慧病房、新概念超市的出现与发展,室内导航受到了学术界及工程界的广泛关注。mems-ins的集成化与小型化促使其成为导航领域的主导技术。
然而,惯性器件误差会随时间积累,如果不对惯性器件误差进行有效的修正,最终会导致行人的位置轨迹发散。为了解决惯性导航位置误差的问题,不同的地图匹配算法被提出。例如粒子滤波算法、基于主航向的地图匹配算法、基于隐形马尔科夫的地图匹配算法等。但这些算法更多的关注行人的二维信息,行人室内行走不仅包括二维平面坐标的移动,还可以通过楼梯进行高度上的改变。因此,行人室内三维地图匹配方法具有重要的理论意义及应用价值。
y.m′eneroux等人提出通过平均hausdorff距离和面积差两种常见的度量方法来解决匹配精度与网络质量指数之间的关系问题,同时还提供了参考网络的影响上限;而xiaoz等人提出采用条件随机场(conditionalrandomfields,crf)算法对导航输出位置与室内状态点进行数学建模来解决惯性导航位置误差发散的问题。然而,前者作为一种室外地图匹配所考虑的更多是为车辆进行路径匹配,行人室内行走比较随意,路径概念相对较弱,因此,该方法不适用于室内环境;而后者虽然考虑了室内的特殊环境,以状态点辅助地图匹配,但忽视了室内的高度信息,只是对单一的楼层进行实验研究,从而导致了只能呈现行人二维的位置信息。
为了有效实现行人在室内环境条件下三维地图匹配,将状态点与传感器采集的导航输出位置信息进行条件随机场模型进行联立,实现行人室内三维定位。通过三维位置定位,提高算法精度与地图匹配的正确率,并简化算法的复杂性是非常重要的。
技术实现要素:
针对佩戴mems-ins传感器的行人在室内环境条件下导航位置导致的发散问题,本发明提供了一种面向室内行人导航的三维地图匹配方法。利用捆绑于行人脚面上的mems-ins传感器采集行人室内的运动信息并解算行人的速度、位置与航向,分析室内结构并创建状态点,根据导航输出位置信息与状态点位置信息建立条件随机场模型,实现行人室内三维定位。
为实现上述技术目的,本发明采用的技术方案为行人室内三维地图匹配方法,包括以下步骤:
步骤1:数据采集,初步解算室内行人的三维位置和航向。
步骤1.1,行人通过佩戴mems-ins传感器,采集行人运动数据,行人运动数据包括:三轴加速度数据和三轴陀螺数据。
步骤1.2,应用捷联解算算法对采集的行人运动数据进行三维位置和航向信息进行求解。
步骤2:提取条件随机场模型的观测点。
步骤2.1,以定长行走距离提取室内行人的水平二维位置信息,零速时刻提取行人高度信息,获取crf模型的观测点,并分别记录室内行人的二维位置与高度信息的采样时刻。
步骤3:建立室内电子地图,并根据室内结构信息创建状态点,存储状态点坐标。
步骤3.1,利用已知的室内地图创建室内电子地图,并将室内电子地图存储于导航计算机中。
步骤3.2,求取电子地图中数值最小与最大的坐标点作为状态点的取值范围,然后以等间距的状态点覆盖整个室内范围,存储状态点信息。
步骤3.3,以台阶高度为标准,添加高度信息上的状态点。
步骤4,基于条件随机场算法的二维位置地图匹配算法。
步骤4.1,根据二维位置观测点坐标与各个状态点坐标之间关系建立特征方程;
步骤4.2,根据观测点的方位信息与与之对应时刻状态点之间的方位角建立特征方程。
步骤4.3,建立基于条件随机场的二维地图匹配数学模型,并求取以二维位置为观测序列条件下状态序列的最大概率,最大概率序列即为位置的最佳状态匹配。
步骤5,基于条件随机场算法的高度信息地图匹配算法。
步骤5.1,依据台阶的高度和行人行走的台阶的极限值,将行人每一步行走的高度划分为不同状态。
步骤5.2,建立以高度为观测点和与该观测点对应的状态点之间的特征方程。
步骤5.3,求取之前所有相邻观测点高度信息与匹配的状态点高度之间的均方差。
步骤5.4,以均方差作为另一个特征,根据相邻状态点高度与各个状态点的高度之差的关系建立特征方程。
步骤5.5,建立基于条件随机场的高度地图匹配数学模型,并求取以高度为观测序列条件下状态序列的最大概率,最大概率序列即为高度的最佳状态匹配。
步骤6:二维位置与高度信息融合
步骤6.1,利用二维位置最佳匹配的状态序列查询相应的观测序列的采样时间进行存储,同时利用高度最佳匹配状态序列查询相应观测序列的采样时间进行存储。
步骤6.2,利用临近时间的方法对二维最佳匹配点与高度最佳匹配点进行组合。
步骤6.3,将匹配的三维位置对惯性导航系统输出的三维位置信息进行修正。
与现有技术相比,本发明具有以下有益效果:
第一,实现行人三维位置定位,算法精度高;第二,采用二维位置与高度信息分开匹配的方法,简化算法的复杂性;第三,采用临近时刻对二维位置与高度信息进行融合,提高了地图匹配的正确率。
附图说明
图1是本发明所涉及方法的框架结构图;
图2是行人佩戴传感器的方式;
图3是惯性导航解算的系统流程图;
图4是经过处理前后室内结构对比图;
图5是经过处理后的电子地图和状态点;
图6是室内三维定位系统的流程图;
图7是惯性导航输出的三维位置信息;
图8是三维室内地图匹配输出行人的匹配轨迹。
具体实施方式
以下结合附图对本发明作进一步详细说明。
本发明所涉及方法的框架结构如图1所示,包括以下步骤:
步骤1:数据采集,初步解算室内行人的三维位置和航向。
步骤1.1,行人通过佩戴mems-ins传感器,采集行人运动数据,行人佩戴传感器的方式如图2所示。行人运动数据数据包括:三轴加速度数据、三轴陀螺数据。
步骤1.2,采用捷联解算算法对采集的行人运动数据进行三维位置航向信息进行求解,惯性导航解算的系统流程图如图3所示。
基于物理学,在采样间隔很短的条件下,速度位置关系满足:
其中,pn表示行人的三维位置坐标;t表示采样时刻,dn表示行人行走的距离;t为采样间隔;vn表示行人的速度信息,an表示行人加速度;
步骤2:提取条件随机场模型的观测点。
步骤2.1,以定长行走距离提取水平二维位置信息,零速时刻提取行人高度信息,获取crf模型的观测点,并分别记录行人水平二维位置与高度信息的采样时刻。
(1)水平二维位置观测点提取模型如下,当满足如下条件:
记录此刻位置点坐标作为二维位置观测点,同时记录下相应的时间:
time1(tob1)=t
其中distance表示当前时刻与上一时刻采样点坐标的欧氏距离;pob1(tob1)表示tob1观测点的二维坐标;
(2)高度观测点提取模型如下,当满足条件:
ifv(t)==0
记录此刻高度信息作为高度观测点,同时记录下相应的时间:
time2(tob2)=t
其中hob2(tob2)表示第tob2个高度观测点;time2表示高度观测点对应的时刻;
步骤3:建立室内电子地图,并根据室内结构信息创建状态点,存储状态点坐标。
步骤3.1,利用已知的室内地图创建室内电子地图,电子地图如图4所示,并将电子地图存储于导航计算机中。
步骤3.2,求取电子地图中数值最小与最大的坐标点作为状态点的取值范围,然后以等间距的状态点覆盖整个取值范围,存储所有状态点坐标信息(x,y)。
(1)求解电子地图中数值最小与最大的坐标点:
pmin=(xmin,ymin)=min(x,y)
pmax=(xmax,ymax)=max(x,y)
其中<pmin,pmax>表示状态点所覆盖范围的最小位置点、最大位置点。
(2)根据最大位置点、最小位置点得到状态点的分布范围为:
r1=pmin=(xmin,ymin)
r2=(xmin,ymax)
r3=(xmax,ymin)
r4=pmax=(xmax,ymax)
其中,<r1,r2,r3,r4>表示状态点范围矩阵的四个顶点坐标。
(3)状态点坐标求解,选取最小的坐标点作为第一个状态点,选取threshold长度作为状态点之间的间隔,状态点模型如下:
state1(0,0)=r1
state1=(is,js)=r1+(is×threshold,js×threshold)<r4
其中state1为所有的状态点总称;(is,js)表示状态点存储的状态。
步骤3.3,由于行人行走楼道时每一步高度变化量为台阶高度的整数倍,因此,添加楼梯区域内的状态点高度是以台阶高度为标准,设台阶高度stair_high结合二维位置的状态点分布的规律,台阶高度的运算模型如下:
state2(n)=0+n×stair_high
其中,state2为所有台阶高度状态点集合,n表示个数,state2(n)由台阶个数决定。结合state1与state2得状态点三维坐标,存储所有状态点坐标信息。则三维状态点与数字地图如图5所示。
步骤4,基于条件随机场算法的二维位置地图匹配算法,匹配及融合方法流程图如图6所示。
步骤4.1,根据二维位置观测点坐标与各个状态点坐标之间关系建立特征方程;二维位置对应的状态点被表示为sp。
其中,fc表示观测点坐标与状态点坐标之间的关系;sp(tob1)表示tob1时刻状态点的(x,y)坐标,并且
步骤4.2,计算观测点的方位信息,并根据观测点的方位信息与该观测点的方位信息所对应时刻状态点之间的方位角建立特征方程。
其中σθ表示观测方位角误差的协方差;b(sp(tob1-1),sp(tob1))表示tob1-1与tob1时刻状态点之间的函数;θ(sp(tob1-1),sp(tob1)表示tob1-1与tob1时刻状态点之间方位角函数,该函数以地图坐标系x轴正方向为参考。
步骤4.3,建立基于条件随机场的二维地图匹配数学模型,并求取以二维位置为观测序列条件下状态序列的最大概率,最大概率序列即为位置的最佳状态匹配。
其中,λp,μp分别表示二维地图匹配数学模型中每个特征对应的权值,在此将权值全部设置为1;i,l表示特征函数的个数;zob1是规范化因子。
步骤5,基于条件随机场算法的高度信息地图匹配算法。
步骤5.1,依据台阶的高度和行人行走的台阶的极限值,估计行人每一步行走所对应的状态。
利用行人每次行走所跨越的台阶的极限,假设行人每次最多跨过nt个台阶,则每次行人行走的高度变化量应该在(-nt,nt)这个范围内。因此,行人每次行走的所对应的高度的状态为:
sh=(((-nt)×stair_high),((1-nt)×stair_high),…,(nt×stair_high))
步骤5.2,建立以高度为观测点和与之对应的状态点之间的特征方程。
其中,sh(tob2)表示tob2时刻状态点高度;hob2(tob2)表示tob2时刻观测点高度;g表示观测点与状态点高度之间的函数关系;σh表示状态点与观测点之间高度之间的协方差;stair_high表示每个台阶的高度;b(sh(tob2-1),sh(tob2))表示tob2-1与tob2时刻状态点之间的函数;h(sh(tob2-1),sh(tob2))表示tob2-1与tob2时刻状态点之间的相对高度函数。
步骤5.3,求取之前所有观测点高度信息与匹配的状态点高度之间的均方差。
δhob2(tob2)=hob2(tob2)-sh*(tob2)
δhob2观测点高度和匹配状态点高度之间的误差矢量,
步骤5.4,以均方差作为另一个特征,根据相邻状态点高度与各个状态点的高度之差的关系建立特征方程。
gc(sh(tob2),hob2(tob2),s_hob2(tob2))=(hob2(tob2)-sh(tob2))-s_hob2(tob2)
其中,σs表示高度误差协方差;s_hob2(tob2)表示tob2时刻之前所有观测误差的协方差。
步骤5.5,建立基于条件随机场的高度地图匹配数学模型,并求取以高度为观测序列条件下状态序列的最大概率,最大概率序列即为高度的最佳状态匹配。
其中,λh,μh表示特征对应的权值,在此将权值全部设置为1。i,l表示特征函数的个数;zob2是规范化因子。
步骤6:二维位置与高度信息融合
步骤6.1,利用二维位置最佳匹配的状态序列查询相应的观测序列的采样时间进行存储,同时利用高度最佳匹配状态序列查询相应观测序列的采样时间进行存储。
步骤6.2,利用临近时间的方法对二维最佳匹配点与高度最佳匹配点进行组合。
time=|time1(tob1)-time2(kob2)|kob2=1…tob2
其中要求:0<tob1<tob2。找出time中最小的值,并将对应的kob2标记为
s*(tob1)=<sp*(tob1),s(tob1)>
其中,s(tob1)表示tob1时刻采用临近点融合得到的行人高度信息;s*为最终行人的轨迹信息。
步骤6.3,将匹配的三维位置对惯性导航系统输出的三维位置信息进行修正反馈。修正反馈的数学模型如下:
pn(t)=s*(tob1)
为了验证算法的有效性,进行了实验验证。以某室内办公环境为例,实验场所包括走廊,楼道两种室内环境。惯性导航系统解算出的三维位置信息如图7所示。可见,导航输出无论二维轨迹还是高度信息都存在误差,定位精度不准确。基于条件随机场的三维室内地图匹配算法如图8所示。实验结果显示该方法匹配的结果具有很高的准确性和有效性。