一种基于智能手机PDR与视觉修正的行人定位方法

一种基于智能手机pdr与视觉修正的行人定位方法
技术领域
1.本发明涉及行人导航技术领域，具体是一种基于智能手机pdr与视觉修正的行人定位方法。


背景技术：

2.随着人们对位置服务需求的提高，室内定位技术成为一个研究热点。由于信号的遮挡与干扰，卫星导航系统在大多数情况下无法满足用户室内定位服务的需求。为了解决在复杂室内环境下卫星信号遮挡的问题，研究人员提出了许多室内定位方法。典型的室内定位技术包括wi-fi指纹、蓝牙、射频识别、超宽带、视觉、航位推算等。随着微电子技术的发展，基于移动智能终端mems传感器的行人航位推算（pedestrian dead recking, pdr）因其具有极强自主性、连续性和不需要事先部署基站的便捷性而得到研究者的青睐。
3.目前，绝大多数的智能手机中内置有加速度计、陀螺仪、磁力计等传感器。行人航位推算是利用智能手机的惯性传感器对行人位置进行估计的一种自主相对定位算法，通过对行人进行步态检测、步长估计和航向计算来推算出行人的行走路线和位置。然而，由于智能手机内置的mems传感器精度有限且惯性传感器件自身存在累积误差，导致pdr在长时间进行位置估算时定位误差会越来越大。另外，传统的pdr方法只能实现在二维平面的行人位置估计，当行人发生上下楼高度位置变化时，pdr则无法进行准确的定位。
4.为了解决pdr误差累积的问题，许多学者提出将pdr与其他室内定位手段融合的解决方案，如利用wi-fi、蓝牙、地磁等额外信息对pdr定位结果进行修正，从而减小pdr的定位误差累积。但是，借助wi-fi和蓝牙等外部信号的辅助手段需要预先在室内场景中部署大量的基础设施，且依赖外部信号，易受环境中信号的干扰。基于室内磁场特征辅助的pdr方法需要在离线阶段花费大量时间和精力构建细粒度的信号指纹数据库，基于地图信息约束的pdr定位方法则对高精度室内地图的绘制提出了较高的要求。上述方案利用绝对位置定位技术与pdr算法进行融合，虽然可以解决pdr误差累积的问题，但是需要额外布设基础设施，增加了定位系统的成本，也在一定程度上削弱了惯性导航的自主性和连续性的优势，在实际应用中缺乏明显的局限性。因此，研究一种低成本的、不依赖外部设施的辅助pdr进行精确鲁棒的行人室内定位技术有着重要的应用价值。
5.近年来，计算机视觉技术迅速发展，视觉slam算法也不断发展成熟。基于视觉特征地图的全局定位技术与slam的回环检测原理相同，本质上是一种信息检索的方法，利用视觉特征匹配的方式对用户的位置进行估计。基于视觉定位技术的实施不受外部环境限制，只需要用户提供一个摄像头来获取当前图像即可，并且当前智能手机中均内置有摄像头传感器。因此，在行人航位推算过程中可以借助智能手机内置的摄像头传感器进行视觉定位，来辅助修正pdr方法的累积误差，从而达到提高定位精度的目的。但是，虽然传统的视觉匹配方法可以得到定位信息，但是图像查询与匹配效率较低，无法满足实时性要求，难以得到实际应用部署。


技术实现要素：

6.针对上述现有技术中的不足，本发明提供一种基于智能手机pdr与视觉修正的行人定位方法，不仅可以获得较好的定位性能提升，同时将传统pdr的应用场景从二维平面拓展至三维空间中，具有实际的研究意义和应用价值。
7.为实现上述目的，本发明提供一种基于智能手机pdr与视觉修正的行人定位方法，包括如下步骤：步骤1，建立待测区域的视觉特征地图，其过程为：采用视觉传感器在待测区域采集场景图像，基于视觉slam算法进行同步定位与建图，将slam建图结果以关键帧为基本组织形式保存为地图数据库，用于后续在线视觉定位使用。
8.步骤2，基于视觉特征地图的全局定位确定行人的初始位置与航向角。
9.步骤3，pdr定位：在初始位置与航向角的基础上基于pdr对行人航位进行推算，并推算行人的行走距离，其过程为：通过分析智能手机加速度计的输出数据进行行人步态检测，当检测出行人一步发生后，根据加速度值计算行人此步的步长，通过陀螺仪输出的角速率信息计算行人前进的方向。在已知起始位置和初始航向的前提下，根据得到的步长和航向角便可推算出行人每一时刻的位置。
10.步骤4，视觉定位：当行人的行走距离达到设定阈值后，基于视觉特征地图的全局定位得到行人当前时刻的全局定位信息，其过程为：利用pdr方法推算行人的行走距离达到设定阈值后，采用智能手机的摄像头拍摄当前场景图像，并检测当前帧的特征点与描述子信息。利用pdr的先验位置信息，将其与离线建立好的特征地图进行特征匹配寻找候选关键帧，然后建立当前帧与候选帧间2d-3d匹配，从而获得当前时刻的全局定位信息。
11.步骤5，以视觉定位结果为定位参考，对pdr定位结果进行修正，并将修正后的pdr定位结果作为行人新的初始位置与航向角后重复步骤3-5。其中，采用基于扩展卡尔曼滤波方法（ekf）将pdr与视觉定位结果松组合融合。pdr是一种相对定位方法，在定位过程中存在累积误差的问题，需要借助绝对位置信息来进行修正。而基于视觉特征地图的视觉定位结果是一种绝对位置信息且无误差漂移，因此可间接性地利用视觉定位结果来修正pdr的累积误差，不仅可以提高定位精度，同时可将传统pdr的应用场景从二维平面拓展至三维空间中。
12.本发明提供的一种基于智能手机pdr与视觉修正的行人定位方法，该方法利用智能手机内置的加速度计、陀螺仪传感器实现行人航位推算，同时借助手机的摄像头传感器拍摄场景图像，基于词袋模型进行视觉特征匹配定位，将pdr与视觉定位结果采用扩展卡尔曼滤波算法(ekf)进行松组合融合，得到行人位置的融合定位结果。通过间接性地调用视觉定位来修正pdr位置和航向角误差，不仅可以获得较好的定位性能提升，同时将传统pdr的应用场景从二维平面拓展至三维空间中，具有实际的研究意义和应用价值。
附图说明
13.为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图示出的结构获得其他的附图。
14.图1为本发明实施例中基于智能手机pdr与视觉修正的行人定位方法的流程图；图2为本发明实施例中单帧关键帧所包含的信息示意图；图3为本发明实施例中pdr定位的流程图；图4为本发明实施例中视觉定位的流程图。
15.本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。
具体实施方式
16.下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
17.需要说明，本发明实施例中所有方向性指示（诸如上、下、左、右、前、后
……
）仅用于解释在某一特定姿态（如附图所示）下各部件之间的相对位置关系、运动情况等，如果该特定姿态发生改变时，则该方向性指示也相应地随之改变。
18.另外，在本发明中如涉及“第一”、“第二”等的描述仅用于描述目的，而不能理解为指示或暗示其相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本发明的描述中，“多个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。
19.在本发明中，除非另有明确的规定和限定，术语“连接”、“固定”等应做广义理解，例如，“固定”可以是固定连接，也可以是可拆卸连接，或成一体；可以是机械连接，也可以是电连接，还可以是物理连接或无线通信连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通或两个元件的相互作用关系，除非另有明确的限定。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。
20.另外，本发明各个实施例之间的技术方案可以相互结合，但是必须是以本领域普通技术人员能够实现为基础，当技术方案的结合出现相互矛盾或无法实现时应当认为这种技术方案的结合不存在，也不在本发明要求的保护范围之内。
21.如图1所示为本实施例公开的一种基于智能手机pdr与视觉修正的行人定位方法，其主要包括如下步骤1-步骤5。
22.步骤1，建立待测区域的视觉特征地图视觉特征地图的建立技术是指利用一定的传感器信息，将不同时刻视觉信息中的视觉特征转化为统一的、可用于全局定位的特征地图，建立视觉特征地图本质上是一个同步建图与定位（slam）的过程。
23.考虑到视觉定位的实时性以及视觉特征的尺度不变性和旋转不变性要求，本实施例采用基于orb特征的视觉slam算法在待测区域离线建立视觉特征地图。局部地图的建立采用局部ba优化算法，通过最小化相机的重投影误差同时优化各相机位姿和各特征点的空间位置。
24.假设相机的位姿为，对应的李群为，特征点的空间位置为，观测数据为像素坐标，构建关于观测误差的最小二乘问题，为：
其中，为在相机位姿处观察路标所产生的数据；为观测方程，为与当前帧共视的关键帧个数，为共视的地图点个数。
25.slam建图得到的视觉特征地图以关键帧为基本组织形式保存为地图数据。参考图2，在每个关键帧中，包含了关键帧在地图坐标系下的位姿、特征点的像素坐标及三维空间位置、特征点的特征描述子，完整的视觉特征地图由建图区域的所有关键帧组成。在具体实施过程为，关键帧的筛选/确定过程采用两个标准：1)当前帧与上一个关键帧的平均视差大于设定阈值keyframe_parallax，通常设置为10左右；2)当前帧跟踪到的特征点数目低于设定阈值track_num，一般设置为50左右。
26.步骤2，基于视觉特征地图的全局定位确定行人的初始位置与航向角在具体实施过程中，当行人初次进入待测区域时，可以调用基于视觉特征地图的位置识别算法，在步骤1已建立视觉特征地图的区域内计算得到视觉全局定位结果，即可以将该视觉全局定位结果作为行人的初始位置与航向角。其中，视觉全局定位结果的获取过程与步骤4相同，在此先不赘述。
27.步骤3，pdr定位：在初始位置与航向角的基础上基于pdr对行人航位进行推算，并推算行人的行走距离基于pdr对行人航位进行推算的过程为：通过分析智能手机加速度计输出数据进行步态检测，在检测到行人一步发生后，根据加速度值计算该步的步长，根据陀螺仪的角速率数据计算行人的航向角。在已知行人前一时刻位置的基础上，根据计算得到的步长和航向信息便可推算出行人当前时刻的位置，因此位置更新过程为：式中，为行人在第k步时的位置，为行人在第k-1步时的位置，为在第k步时的航向角，为第k步的步长。
28.参考图3，基于pdr对行人航位进行推算的过程具体为：行人的行走过程具有周期性的变化规律。根据行人行走过程中的运动特点，通过分析加速度计的三轴加速度变化规律可精确推算出行走步数。由于行人行走过程中身体的抖动以及传感器的误差等原因，在获取原始加速度数据后需要采用平滑滤波的方法对其进行预处理，即：其中，为t时刻滤波后的加速度，为第时刻的加速度，m为滑动窗口的大小。在具体实施过程中，滑动窗口大小的选择与加速度数据的采集频率和步频有关，一般设置
为5左右可取得较好的步态检测效果。
29.将原始加速度数据平滑滤波后便可进行步态检测。由于行人持有手机的姿态并不固定，如果采用单轴加速度值进行步态检测会遇到周期特征不明显的问题，因此将三轴的合成加速度作为步态检测的判定依据，其大小计算为：其中，、、分别代表平滑滤波后的加速度在轴、轴、轴的分量；再根据合成加速度的大小以及连续两个待定步之间的时间间隔来判断一步是否发生：假设时刻的合成加速度为第步时间内的峰值，记为。则应满足：其中，为t-1时刻的合成加速度，为t 1时刻的合成加速度；判定一步发生的具体准则为：其中，为加速度峰值阈值；为相邻峰值的时间间隔，即第步的持续时间，和为时间间隔的阈值下限和上限。
30.考虑到行人行走速度快慢的影响，需要对加速度峰值阈值和时间间隔阈值、作进一步的动态设置。峰值阈值限定在m/s2之间，并根据当前加速度峰值与前两个时刻加速度峰值的均值之比进行动态调整如下：其中，为第k步计算时的峰值阈值，为第k 1步计算时的峰值阈值，为第步合成加速度峰值与前三步合成加速度平均峰值的比值。对于最初三步计算时的峰值阈值设置为12左右。
31.时间间隔阈值、的设置需要结合行人在正常行走时的频率范围0.5-5hz进行考虑。具体动态调整如下：
在检测到行人一步发生后，对该步的步长和航向进行估计。采用weinbeng步长模型估计第步的步长，即：其中，是步长系数，对于不同的行人取值不同，与每个人的身高、步频等因素有关；、是第步过程中合成加速度的最大值和最小值。
32.在具体实施过程中，基于陀螺仪的航向估计只能为pdr提供一个相对航向估计值，在已知初始航向信息的前提下，对陀螺仪输出的角速率进行积分计算得到当前时刻的航向，为：其中，为初始航向角；为陀螺仪相对于导航坐标系z轴的角速率测量值，为第步的航向角变化量，为航位推算的初始时刻，为第步时所对应的时刻。
33.步骤4，视觉定位：当行人的行走距离达到设定阈值后，基于视觉特征地图的全局定位得到行人当前时刻的全局定位信息当采用pdr方法推算行人的行走距离达到设定阈值后，调用基于视觉特征地图的位置识别算法，在步骤1已建立视觉特征地图的区域内计算得到视觉全局定位结果。视觉位置识别与视觉slam中回环检测原理相同，全局定位的第一步可以看作是闭环检测的过程。首先提取当前帧的orb特征点和特征描述子，基于词袋模型（bag-of-words, bow）计算当前帧的词袋向量信息。然后在由关键帧组成的视觉特征地图中，利用pdr推算的位置先验信息，通过计算不同图像的词袋向量之间的距离（即图像间的相似度）寻找与当前帧相似的地图中的关键帧，这些关键帧会作为候选帧用于进一步的精定位过程。
34.参考图4，基于视觉特征地图的进行全局定位的过程具体包括：建立视觉特征地图的字典：视觉slam中特征描述子的字典是通过对大量图像的特征进行聚类得到的，步骤1的视觉特征地图是已建立好的，可以通过对视觉特征地图中出现的所有特征点进行聚类，生成专门针对此视觉特征地图的orb字典。
35.字典的训练是基于k-means算法得到n个单词(word)：。为了提高图像匹配和查询的效率，使用k叉树来表达字典，叶子层即为所谓的单词。构建完k叉树字典后，采用tf-idf（term frequency-inverse document frequency）方法为每个单词赋予权值。idf的思想是，某单词在字典中出现的频率越低，分类图像的区分度就越高：
其中，为单词的idf值，为字典中所有特征数量，为单词中的特征数量；tf的思想是，某单词在一幅图像中出现的次数越多，它的区分度越高。假设图像中单词出现了次，一共出现的单词次数为，则单词的tf值为：最终得到单词的权值为：对于某幅图像a，它的特征点对应到多个单词，计算if-idf值得到描述该幅图像的词袋向量，为：其中，为字典的所有单词个数，为单词的tf-idf值，为图像a的词袋向量。
36.图像之间的相似度计算一般利用词袋向量距离的范数形式，即：式中，为图像a与图像b之间的相似度，为图像b的词袋向量，为词袋向量的第个分量，为词袋向量的第个分量，、表示词袋向量和的第i个分量，也就是每个视觉单词的权重值，与、表示的含义相同；获取智能手机上摄像头采集的当前帧图像，计算出当前帧与视觉特征地图中pdr推算位置附近所有关键帧的相似度后，选择相似度最高的若干帧作为候选帧，进行特征匹配与pnp位姿求解，得到精确的全局定位信息，其具体实施过程为：特征匹配是指判断不同图像的特征点之间的对应关系，特征点之间的相似度通常采用特征描述子距离进行度量。对于orb特征的brief二进制描述子，通常采用汉明距离来表述相似度，即：其中，表示异或运算；、分别表示两幅图像中orb特征点的brief描述子。
37.基于汉明距离的特征相似度度量方法，采用快速近似最近邻（flann）对特征点进行匹配。考虑到可能会出现误匹配的问题，使用随机采样一致性（ransc）对匹配进行筛选，
剔除一些错误匹配的点对。
38.在获得当前帧与候选帧的特征匹配关系之后，由于视觉特征地图中已知候选帧特征点的三维坐标，采用pnp（perspective-n-point）方法求解当前帧相对于地图的位姿。pnp是利用3d-2d点对求解位姿的方法，3d点由视觉特征地图中得到，2d点是当前帧的特征点。采用非线性优化的方式，把pnp问题构建成一个关于最小化重投影误差的非线性最小而成问题。
39.考虑有个三维空间点及投影点，待求相机的位姿，它的李群表示为。假设某空间点坐标为，其投影的像素坐标为。由于相机的位姿未知及观测点噪声的影响，3d点的投影位置与观测位置存在一个误差。将所有的重投影误差求和，构建一个最小二乘问题，迭代求解最优的相机位姿，使其最小化，即：其中，表示尺度因子；为相机内参矩阵。
40.在计算出每个候选帧与当前帧之间的平移与旋转后，通过ransac方法剔除一些异常的候选帧。最后将剩余的候选帧中所有地图点投影到当前帧搜索特征匹配，如果匹配的数目大于设置阈值，则接收相机位姿结果，否则不进行视觉修正，跳过后面步骤5的滤波融合步骤。
41.计算得到相机的位姿结果后，相机的位置作为行人当前时刻的位置参考信息，将相机的姿态矩阵转换为欧拉角，可得到行人当前时刻的参考航向角信息，为：其中，为姿态矩阵r中第2行第1列位置的元素，为姿态矩阵r中第1行第1列位置的元素。
42.步骤5，以视觉定位结果为定位参考，对pdr定位结果进行修正，并将修正后的pdr定位结果作为行人新的初始位置与航向角后重复步骤3-5。
43.在具体实施过程中，基于扩展卡尔曼滤波方法（ekf）将pdr与视觉定位结果松组合融合，以视觉位置识别结果作为定位参考，不仅可以修正pdr的累积误差，提高定位精度，同时可解决pdr在三维空间中行人定位问题。
44.在扩展卡尔曼滤波方法ekf的预测阶段，第步时行人的状态转移方程如下：
其中，为第步的状态预测向量，即通过行人航位推算pdr得到的行人第步的位置坐标和航向角；为通过扩展卡尔曼滤波方法efk对第步进行最优估计获得的状态向量，即通过视觉修正后得到的行人第步的位置坐标和航向角、、；初始值设置为pdr的初始位置和航向角，即；表示状态转移方程中的非线性函数；表示上一次调用视觉定位结果来修正pdr定位结果所对应的步数；为过程噪声向量；将状态转移方程中的非线性函数在附近进行线性化处理，舍去高阶部分后得到第步所对应的状态矩阵，为：其中，表示将非线性函数在附近进行线性化处理；之后对预测变量的协方差矩阵进行更新，为：其中，表示采用扩展卡尔曼滤波方法ekf对第步状态最优估计值的协方差矩阵，其初始值设为；表示预测模型本身所带来的
过程噪声矩阵，由行人航位推算方法pdr各要素的平均误差组成，其中、表示位置平均误差，表示航向角平均误差。
45.在扩展卡尔曼滤波方法ekf的更新阶段，系统的观测方程为：其中，为观测矩阵；表示在第步由视觉定位识别得到的观测向量，、为第k步时视觉定位的位置信息，为第k步时视觉定位的航向角；为观测误差向量；表示在第步由pdr定位识别得到的观测向量，、为第k步时pdr定位的位置信息，为第k步时pdr定位的航向角。
46.计算第步的ekf增益矩阵，为：其中，为第步对应的观测噪声协方差矩阵，通过下式计算：其中，为窗口长度，为第步时视觉位置识别得到的观测向量，为第步时pdr推算得到的状态向量。
47.计算行人第步状态的最优估计值，为：同时更新状态最优估计值的协方差矩阵，用于下次卡尔曼滤波方法ekf计算，为：其中，为单位矩阵。
48.在具体计算过程中，对于行人的高度位置，可以直接采用视觉定位结果中的高度值，从而实现行人在三维空间中的室内定位。
49.以上所述仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是在本发明的发明构思下，利用本发明说明书及附图内容所作的等效结构变换，或直接/间接运用在其他相关的技术领域均包括在本发明的专利保护范围内。