基于计算机视觉与PDR的行人室内定位与AR导航方法

基于计算机视觉与pdr的行人室内定位与ar导航方法
技术领域
1.本发明属于定位导航技术领域，尤其涉及一种基于计算机视觉与pdr的行人室内定位与ar导航方法。


背景技术：

2.随着城市建设的发展，大型室内场景变得越来越多，最常使用的gps定位技术在室内因为信号过弱而无法使用，但是商铺导航、室内ar游戏等应用都需要以室内定位导航系统为基础，所以人们对新型室内定位导航系统的需求越来越大。目前面向室内的定位技术有各自的优点但也存在各自的局限性：pdr(行人航迹推算算法)技术通过读取手机imu模块的数据计算出用户的步长，再结合步频和方向角信息计算得到用户行走路径的长度和方向，最后得到用户的位置，pdr技术在步长计算上有很多种模型，本方法使用的模型定位精度高但在计算用户步长的时候不同用户具有不同的方程参数，用户使用pdr之前需要手动标定这些参数，这一过程增大了pdr的使用难度，此外pdr存在积分漂移问题，连续使用时间越久pdr的误差越大。视觉定位技术定位精度较高、使用成本较小，但易受环境光线影响并在单调环境下无法成功定位。所以研究一种融合型定位方式是当下的热点。目前主流的融合型室内定位技术包括基于粒子滤波的wifi、pdr融合技术，但该技术的定位精度较差；还有应用于机器人室内定位导航的视觉定位加惯性定位技术，但该技术无法适用于行人的室内定位。
3.近些年ar(augmented reality增强现实)产品正在不断出现，未来带ar效果的导航系统很有希望成为主流导航工具之一，并会作为其他室内ar产品的一部分。目前的ar导航系统大部分是应用在车辆上的，专门帮助行人在室内进行导航的ar导航系统还很少，少量行人ar室内导航系统的定位方式也是采用主流的wifi定位、蓝牙定位等，这些定位方式定位效果较差，导致ar导航效果也较差，并且它们没有建图过程，无法做到显示三维地图的功能。


技术实现要素：

4.本发明的目的在于针对室内行人定位困难的问题，基于计算机视觉与pdr技术设计新型融合定位方案，实现室内低成本精确定位。针对二维导航地图用户体验感较差的问题，使用slam技术(实时建图定位技术)完成室内环境建图与显示，同时使用ar技术完成ar导航功能，实现更好的导航体验。
5.本发明的目的是通过以下技术方案实现的：一种基于计算机视觉与pdr的行人室内定位与ar导航方法，该方法包括以下步骤：
6.(1)通过深度摄像机采集室内rgbd图像发送到云端服务器，在云端服务器构建室内三维点云地图，对所述室内三维点云地图进行坐标线性变换校准，得到旋转矩阵；
7.(2)基于计算机视觉和坐标变换对移动终端进行定位，得到移动终端世界坐标，包括：
8.(2.1)通过移动终端采集室内rgb图像发送到云端服务器；
9.(2.2)云端服务器根据移动终端发送的图像，使用最大似然透视n点算法计算得到移动终端在室内三维点云地图中的位置，结合步骤(1)中的旋转矩阵得到移动终端世界坐标；
10.(3)在移动终端构建pdr步长估计模型，根据步骤(2)得到的移动终端世界坐标计算步长，根据步长估计pdr步长估计模型中的身高参数和个异性参数，完成pdr自动初始化，在移动终端使用pdr计算得到移动终端世界坐标，然后将世界坐标上传至云端服务器；
11.(4)使用加权融合定位，云端服务器根据步骤(2)和步骤(3)得到的移动终端世界坐标得到融合后的移动终端世界坐标；在步骤(2)的视觉定位工作环境不满足要求时，提高pdr定位权重，降低视觉定位权重，在步骤(3)的pdr定位工作时间超过阈值时，使用视觉定位结果更新pdr当前位置；
12.(5)在云端服务器，基于移动终端世界坐标，根据导航算法计算得到路径规划结果并返回移动终端，在移动终端根据路径规划结果显示ar导航标志及环境三维点云地图。
13.进一步地，所述云端服务器存放步骤(1)建图时深度摄像机采集的rgb图片、深度图片、记录采集图片时间戳的txt文件，步骤(2)视觉定位时移动终端采集的rgb图片、记录采集图片时间戳的txt文件，以及深度摄像机和移动终端相机的内外参数。
14.进一步地，所述步骤(1)中，在云端服务器使用slam算法构建室内三维点云地图；为了能够找到视觉定位坐标系下的xz平面和z轴，在建图过程中预设一段在世界坐标系下xz平面和z轴的运动轨迹。
15.进一步地，所述步骤(1)中，在云端服务器对室内三维点云地图进行坐标线性变换校准，具体为：
16.①
第一次旋转变换，将视觉定位坐标系的xz平面旋转到与世界坐标系的xz平面重合：根据slam算法返回的关键帧深度摄像机所在位置构建数据矩阵pm×3＝[x，y，z]，x，y，z分别是视觉定位坐标系下的x，y，z轴位置数据向量，m为在建图过程中预设轨迹的关键帧数量；在数据矩阵pm×3中找到一个平面ω：z＝ax+by，满足如下最小化问题：
[0017][0018]
其中，a，b是平面方程系数，xi，yi，zi是pm×3中的数据点在x，y，z轴的坐标值；求解最小化问题得到平面方程系数解a0，b0；
[0019]
根据平面ω的参数，得到第一次旋转变换的旋转矩阵r
xz
：
[0020][0021]
其中，r
x
，rz分别为绕x，z轴旋转方向的旋转矩阵；
[0022][0023]
②
第二次旋转变换，用于确定绕y轴旋转方向的旋转矩阵ry，消去绕y轴旋转的自由度，得到过原点的直线z＝kx；
[0024][0025]
其中，k为直线斜率，
[0026]
③
计算得到总体旋转矩阵r＝r
yrxz
。
[0027]
进一步地，所述步骤(1)中，在云端服务器使用slam算法构建室内三维点云地图，调用点云库pcl得到稠密化后的点云地图，调用基于八叉树的三维地图创建工具octomap提供的octree类，创建一个八叉树对象，并从点云地图中读取坐标插入到八叉树中，获取每个点的体素信息，确定地图中的可行通路，最终得到带连通信息的三维稠密点云地图。
[0028]
进一步地，所述步骤(1)中构建的室内三维点云地图存放在云端服务器中，在实时导航时，将室内三维点云地图下发到移动终端，并在导航界面进行展示。
[0029]
进一步地，所述步骤(3)中，在移动终端构建的pdr步长估计模型如下：
[0030][0031]
其中，sl表示步长，a
′
＝0.371，b
′
＝0.227是模型的两个经验参数，f为步频参数，h为身高参数，c为个异性参数。
[0032]
进一步地，所述步骤(3)中，根据步骤(2)得到的移动终端世界坐标计算步长，根据pdr得到步频，使用最小二乘法计算得到身高参数和个异性参数。
[0033]
进一步地，所述步骤(4)中，加权融合定位模型如下：
[0034]
p
evaluate
＝w
visual
*p
visual
+w
pdr
*p
pdr
[0035]
其中，p
evaluate
是当前位置的估计值，p
visual
是通过视觉定位得到的当前位置估计值，p
pdr
是通过pdr得到的当前位置估计值，w
visual
和w
pdr
是视觉定位和pdr定位对应的权重系数，w
visual
+w
pdr
＝1。
[0036]
进一步地，所述步骤(4)中，视觉定位工作环境不满足要求的情况包括：工作在纹理条件单调的环境中，工作在光照条件不良的环境中，或者工作在空旷环境中。
[0037]
进一步地，所述步骤(5)中，导航算法采用a*导航算法。
[0038]
本发明的有益效果是：本发明方法能够自动完成初始化，实现室内精确定位，具有ar导航功能，并能够在导航界面上显示环境三维点云地图。本方法主要包括建图、定位与导航三个部分。建图时，本方法使用深度摄像机获取环境深度图，在云端服务器完成三维点云地图构建。定位时，移动终端调用云端服务器上的视觉定位算法计算pdr中的参数完成自动初始化，之后采集环境rgb图和移动终端imu信息，使用加权融合解决视觉定位性能受环境影响大和pdr定位漂移问题。导航时，移动终端根据导航算法结果生成ar导航标志，并把三维点云地图显示在界面上，实现更好的导航体验。
附图说明
[0039]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
[0040]
图1为一实施例提供的基于计算机视觉与pdr的行人室内定位与ar导航方法流程图；
[0041]
图2为一实施例提供的融合定位流程图。
具体实施方式
[0042]
为了更好的理解本技术的技术方案，下面结合附图对本技术实施例进行详细描述。
[0043]
应当明确，所描述的实施例仅仅是本技术一部分实施例，而不是全部的实施例。基于本技术中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其它实施例，都属于本技术保护的范围。
[0044]
在本技术实施例中使用的术语是仅仅出于描述特定实施例的目的，而非旨在限制本技术。在本技术实施例和所附权利要求书中所使用的单数形式的“一种”、“所述”和“该”也旨在包括多数形式，除非上下文清楚地表示其他含义。
[0045]
本发明提供一种基于计算机视觉与pdr的行人室内定位与ar导航方法，如图1所示，该方法包括以下步骤：
[0046]
(1)通过深度摄像机采集室内rgbd图像发送到云端服务器，在云端服务器构建室内三维点云地图，对所述室内三维点云地图进行坐标线性变换校准，得到旋转矩阵；
[0047]
(2)基于计算机视觉和坐标变换对移动终端进行定位，得到移动终端的世界坐标，包括：
[0048]
(2.1)行人作为移动终端用户，通过移动终端采集室内rgb图像发送到云端服务器；
[0049]
(2.2)云端服务器根据移动终端发送的图像，使用最大似然透视n点算法(mlpnp)计算得到移动终端在室内三维点云地图中的位置，结合步骤(1)中的旋转矩阵得到移动终端的世界坐标；
[0050]
(3)在移动终端构建pdr步长估计模型，根据步骤(2)得到的移动终端世界坐标计算步长，根据步长估计pdr步长估计模型中的身高参数和个异性参数，完成pdr自动初始化，在移动终端使用pdr计算得到移动终端的世界坐标，移动终端将世界坐标上传至云端服务器；
[0051]
(4)使用加权融合定位，云端服务器根据步骤(2)和步骤(3)得到的移动终端世界坐标得到融合后的移动终端世界坐标；在步骤(2)的视觉定位工作环境不满足要求时，提高pdr定位权重，降低视觉定位权重，在步骤(3)的pdr定位工作时间超过阈值时，使用视觉定位结果更新pdr当前位置；
[0052]
(5)在云端服务器，基于移动终端世界坐标，根据导航算法计算得到路径规划结果并返回给移动终端，在移动终端根据路径规划结果显示ar导航标志及环境三维点云地图。
[0053]
云端服务器存放以下数据但不限于此：步骤(1)建图时深度摄像机采集的rgb图片、深度图片、记录采集图片时间戳的txt文件，步骤(1)坐标线性变换校准时用到的代码，步骤(1)建图后得到的二进制三维点云地图文件，步骤(2)视觉定位时移动终端采集的rgb图片、记录采集图片时间戳的txt文件，深度摄像机和移动终端相机的内外参数，步骤(2)视觉定位时移动终端调用的.so文件，步骤(5)路径规划时用到的导航算法代码；构建的室内
三维点云地图存放在云端服务器，在实时导航时，将室内三维点云地图下发到移动终端，并在导航界面进行展示。
[0054]
在一个实施例中，在云端服务器使用orb-slam3算法构建室内三维点云地图，为了能够找到视觉定位坐标系下的xz平面和z轴，在建图过程中预设一段在世界坐标系下xz平面和z轴的运动轨迹。在云端服务器对室内三维点云地图进行坐标线性变换校准，具体为：
[0055]
①
第一次旋转变换，将视觉定位坐标系的xz平面旋转到与世界坐标系的xz平面重合：根据orb-slam3算法返回的关键帧深度摄像机所在位置构建数据矩阵pm×3＝[x，y，z]，x，y，z分别是视觉定位坐标系下的x，y，z轴位置数据向量，m为在建图过程中预设轨迹的关键帧数量；在数据矩阵pm×3中找到一个平面ω：z＝ax+by，满足如下最小化问题：
[0056][0057]
其中，a，b是平面方程系数，xi，yi，zi是pm×3中的数据点在x，y，z轴的坐标值；求解最小化问题得到平面方程系数解a0，b0；
[0058]
根据平面ω的参数，得到第一次旋转变换的旋转矩阵r
xz
：
[0059][0060]
其中，r
x
，rz分别为绕x，z轴旋转方向的旋转矩阵；
[0061][0062]
②
第二次旋转变换，用于确定绕y轴旋转方向的旋转矩阵ry，消去绕y轴旋转的自由度，得到过原点的直线z＝kx；
[0063][0064]
其中，k为直线斜率，
[0065]
③
计算得到总体旋转矩阵r＝r
yrxz
。
[0066]
在一个实施例中，在云端服务器使用slam算法构建室内三维点云地图，调用点云库pcl，将保存的三维点云数据读入填充到地图中，对其进行体素滤波和离散值滤波，得到每个经过滤波的关键帧点云，得到稠密化后的点云地图，然后调用基于八叉树的三维地图创建工具octomap提供的octree类，创建一个八叉树对象，并从点云地图中读取坐标插入到八叉树中，获取每个点的体素信息，确定地图中的可行通路，最终得到带连通信息的三维稠密点云地图。
[0067]
在一个实施例中，在移动终端构建的pdr步长估计模型如下：
[0068][0069]
其中，sl表示步长，a
′
＝0.371，b
′
＝0.227是模型的两个经验参数，f为步频参数，h为身高参数，c为个异性参数。
[0070]
在一个实施例中，根据步骤(2)得到的移动终端世界坐标计算步长，具体方法为使用视觉定位算法计算当前位置信息pi＝{xi，yi，zi}，其中pi代表计算得到的拍照时刻的相机位置向量，xi代表x轴方向的位置信息，yi代表y轴方向的位置信息，zi代表z轴方向的位置信息，计算得到与前一次拍照之间走过的路程：
[0071][0072]
其中，li代表第i个视觉定位间隔内走过的距离，x
i-1
代表前一拍照时刻x轴方向的位置信息，y
i-1
代表前一拍照时刻y轴方向的位置信息，z
i-1
代表前一拍照时刻z轴方向的位置信息，根据pdr得到拍照间隔内的总步数，具体方法为读取当前时刻加速度传感器信息ai＝{a
x
，ay，az}，其中ai代表当前时刻的加速度向量，a
x
代表当前时刻x轴方向的加速度大小，ay代表当前时刻y轴方向的加速度大小，az代表当前时刻z轴方向的加速度大小，计算得到当前时刻的总加速度大小如果测得|ai|＞threshold
high
，然后在0.125s到0.75s内测得|ai|小于threshold
low
，最后在接下来的0.25s到1.5s内测得|ai|大于threshold
high
，那么总步数step就增加1。其中threshold
high
为加速度上限阈值，在pdr初始化阶段得到，得到threshold
high
的具体方法为式中|aj|为测到的第j个加速度大小；其中threshold
low
为加速度下限阈值，在pdr初始化阶段得到，得到threshold
low
的具体方法为一次视觉定位成功后记录下当前的step值，计算得到平均步长平均步频其中timei代表第i个视觉定位间隔的时间长度，计算完成后把step清零，重复上述过程5次得到5组和然后使用最小二乘法计算得到身高参数和个异性参数。
[0073]
在一个实施例中，加权融合定位模型如下：
[0074]
p
evaluate
＝w
visual
*p
visual
+w
pdr
*p
pdr
[0075]
其中，p
evaluate
是当前位置的估计值，p
visual
是通过视觉定位运算出来的当前位置估计值，p
pdr
是通过pdr运算出来的当前位置估计值，w
visual
和w
pdr
是视觉定位和pdr定位对应的权重系数，w
visual
+w
pdr
＝1，云端服务器根据步骤(2)和步骤(3)得到的移动终端世界坐标得到融合后的移动终端世界坐标。在步骤(2)的视觉定位工作环境不满足要求时，提高pdr定位权重w
pdr
，降低视觉定位权重w
visual
，移动终端用户根据室内环境手动调整权重大小。在步骤(3)的pdr定位工作时间超过阈值并且视觉定位工作环境满足要求时，使用视觉定位结果更新pdr当前位置，防止pdr定位漂移。具体地，视觉定位工作环境不满足要求的情况包括但不限于：工作在纹理条件单调的环境中，工作在光照条件不良的环境中，或者工作在空旷环境中。
[0076]
在一个实施例中，步骤(5)中的导航算法采用a*导航算法，将地图上1m内的区域视为拓扑图上的一个节点，进而把导航问题变成图论问题。然后在移动终端部署arcore工具包，云端服务器返回移动终端导航方向信息，移动终端根据方向信息调用arcore工具包的3d物体显示api在地面放置对应的3d箭头，完成ar导航效果的生成。
[0077]
还需要说明的是，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、商品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、商品或者设备所固有的要
素。在没有更多限制的情况下，由语句“包括一个
……”
限定的要素，并不排除在包括所述要素的过程、方法、商品或者设备中还存在另外的相同要素。
[0078]
上述对本说明书特定实施例进行了描述。其它实施例在所附权利要求书的范围内。在一些情况下，在权利要求书中记载的动作或步骤可以按照不同于实施例中的顺序来执行并且仍然可以实现期望的结果。另外，在附图中描绘的过程不一定要求示出的特定顺序或者连续顺序才能实现期望的结果。在某些实施方式中，多任务处理和并行处理也是可以的或者可能是有利的。
[0079]
在本说明书一个或多个实施例使用的术语是仅仅出于描述特定实施例的目的，而非旨在限制本说明书一个或多个实施例。在本说明书一个或多个实施例和所附权利要求书中所使用的单数形式的“一种”、“所述”和“该”也旨在包括多数形式，除非上下文清楚地表示其他含义。还应当理解，本文中使用的术语“和/或”是指并包含一个或多个相关联的列出项目的任何或所有可能组合。
[0080]
应当理解，尽管在本说明书一个或多个实施例可能采用术语第一、第二、第三等来描述各种信息，但这些信息不应限于这些术语。这些术语仅用来将同一类型的信息彼此区分开。例如，在不脱离本说明书一个或多个实施例范围的情况下，第一信息也可以被称为第二信息，类似地，第二信息也可以被称为第一信息。取决于语境，如在此所使用的词语“如果”可以被解释成为“在
……
时”或“当
……
时”或“响应于确定”。
[0081]
以上所述仅为本说明书一个或多个实施例的较佳实施例而已，并不用以限制本说明书一个或多个实施例，凡在本说明书一个或多个实施例的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本说明书一个或多个实施例保护的范围之内。