一种基于行人导航的协同航向估计方法

1.本发明涉及室内定位技术领域，尤其涉及一种基于行人导航的协同航向估计方法。


背景技术：

2.导航是手机中的一个重要的应用程序。由于全球导航卫星系统(gnss)信号很容易损坏或无法在城市峡谷和室内环境的使用。同时在外部，特别是在露天环境中，专业级gnss接收器可以通过实时运动定位(rtk)获得高精度的定位位置；然而消费者级接收器，如智能手机，难以获得厘米级定位稳定性。所以在一些复杂的场景中，如封闭建筑，目前仍缺乏用于实现准确、稳定、随时随地定位移动用户的技术手段。
3.到目前为止，在智能尺度室内定位方面已经开发了一些研究或产品，如无线信号定位(超宽带(uwb)、蓝牙低能量(ble))、惯性传感器定位(系带惯性导航系统(sins)、行人死亡计算(pdr))、匹配定位(地图匹配或指纹匹配)、视觉定位(视觉同步定位和映射(vslam)、可见光定位。
4.但是行人导航还需要解决无缝定位问题，特别是在的恶劣环境下。同时行人导航过程中，航向是决定整个系统精度和稳定性的核心问题。因此，确保可靠的航向是定位的关键。
5.迄今为止，已经进行了几项研究来提高航向精度。
6.例如：吴等人，提出了一种基于自适应卡尔曼滤波器的航向估计方法，其利用扩展卡尔曼滤波器(ekf)融合imu测量来计算航向。
7.郭等人，采用互补主成分分析(pca)和旋转方法(ra)方法估计智能手机状态，避免180
°
方向模糊，减少累积误差的影响，提高航向精度。
8.ilyas等人，提出了一种磁异常检测(mad)和补偿算法，该算法在ekf更新步骤中只加入健康磁强计数据，以减少零速度更新的ins中的漂移。
9.邓等人，提出了一种基于旋转矩阵和主成分分析的航向估计方法来评价行人航向。
10.abdulrahim等人，提出了一种集成了零速度更新(zupt)和建立航向信息的鞋装imu方法，以减少航向漂移。
11.asraf等人，提出了pdrnet，其采用一个深度学习的pdr框架，用于用户定位，其中包括一个智能手机位置识别分类网络，然后改变标题和距离回归网络，其他数据源用于航向估计。
12.liew等，结合接收到的信号强度指示器(rssi)来纠正行人航向。
13.然而，上述方法仍面临着如下问题：imu测量误差较大，特别是对于消费水平的设备，行人随机切换智能手机姿势，无线信号或地磁信号容易被干扰，进而导致基于无线信号的航向校正算法容易受到多路径效应的影响，从而导致低稳定性，即用于测航向的磁强计测量在室内环境中容易受到磁干扰的影响。


技术实现要素：

14.为解决上述问题，本发明提供一种基于行人导航的协同航向估计方法，结合了智能手机的内部嵌入式结构传感器，如陀螺仪、加速度计、磁力计和相机测量，提高了航向估计精度。
15.为实现上述目的，本发明提供了一种基于行人导航的协同航向估计方法，包括以下步骤：
16.s1、处理智能手机内部mems传感器的测量值，以获得标题：
17.s11、使用旋转矩阵将智能手机的身体坐标系转换为导航坐标系；
18.s12、通过加速度计和磁力计的测量值来计算智能手机的航向；
19.s2、结合陀螺仪的集成角度和初始方向来估计智能手机的航向；
20.s3、使用互补滤波器计算智能手机的标题；
21.s4、结合相机图像和深度的航向识别学习。
22.优选的，步骤s11中的身体坐标系转换为导航坐标系的公式为：
[0023][0024]
式中，分别为基于x-轴、y-轴和z-轴的旋转矩阵；
[0025]
旋转矩阵公式为：
[0026][0027]
式中，ψ为载体的航偏角，机头左偏为正，γ为载体横滚角，右机翼向上为正，θ为载体俯仰角，机头向上抬起为正。
[0028]
优选的，载体横滚角γ计算公式如下：
[0029]
γ＝arctan2(-accx,accz)
[0030]
式中，accx和accz分别为经x轴和z轴加速度计获得测量值；
[0031]
载体俯仰角θ计算公式如下：
[0032][0033]
式中，accy为经y轴加速度计获得测量值。
[0034]
优选的，步骤s12具体包括以下步骤：
[0035]
s121、计算磁北ψm：
[0036][0037]
式中，分别为磁力计三轴测量值；
[0038]
s122、计算真北ψ
truenorth
：
[0039]
ψ
truenorth
＝ψm+δψ-γ
[0040]
式中，δψ为磁偏赤，其由国际地磁场模型中得到；γ表示真北网格收敛。
[0041]
优选的，步骤s2的具体计算公式为：
[0042][0043]
式中，ωi是陀螺仪输出的角度更新率，tk和t
k+1
分别是开始时刻和结束时刻，heading
initial
是初始航向。
[0044]
优选的，步骤s3采用如下融合算法：
[0045][0046]
式中，是由acc计算的航向以及mag测量；是上一时刻融合航向；α是平滑因子；δt是采样周期，即从开始时刻t
k-1
到结束时刻tk；
[0047]
其中：
[0048][0049][0050]
式中，rc表示等效的时间常数；fc是截止频率。
[0051]
优选的，步骤s4具体包括以下步骤：
[0052]
s41、计算行人航向与地面参考线的；
[0053]
s42、采用嵌入式智能手机的摄像头来捕捉图像，并利用采用神经学和深度学习的方法进行识别实时显示的地面基准线。
[0054]
优选的，步骤s41具体包括以下步骤：
[0055]
s411、将智能手机的垂直轴py投影到局部地面gy，结合参考线ab和交点a，创建一个局部地面坐标系，局部地面坐标系中参考线ab的方位角计算公式如下：
[0056][0057]
s412、获取世界大地坐标系和智能手机摄像头坐标系之间的变换公式：
[0058][0059]
式中，r和t分别为世界大地坐标系和智能手机摄像头坐标系之间的旋转矩阵和原点平移矩阵；dx和dy分别为行和列的一个像素，其单位为mm；f为摄像头的焦距；zc为转换系数；
[0060]
s413、带入参考线端点a和b，得到：
[0061][0062]
s414、在不考虑坐标系的旋转和平移的情况下，结合上式计算ab的两个点的坐标分量的差，获得以下公式：
[0063][0064]
此时参考线ab的方位角α记为：
[0065][0066]
优选的，步骤s42具体包括以下步骤：
[0067]
s421、收集地面参考线图片，作为数据集；
[0068]
s422、使用增强算法扩展数据集；
[0069]
s423、将扩展后的数据集分为测试集和训练集；
[0070]
s424、在训练集中的训练图片创建标签，并将标签转换为json文件，最后将标签转换为单通道png；
[0071]
s425、将训练集输入unet对识别模型进行训练；
[0072]
s426、将测试集输入训练完毕的识别模型，输出图像包含预测的标签，即地面参考线。
[0073]
本发明具有以下有益效果：
[0074]
1、基于行人导航的协同航向估计方法，结合了智能手机的内部嵌入式结构传感器，如陀螺仪、加速度计、磁力计和相机测量，提高了航向估计精度。
[0075]
2、基于移动深度学习技术的实时航向校正算法，实现了基于智能手机的行人快速航向校正，进一步提高了行人导航和定位精度。
[0076]
下面通过附图和实施例，对本发明的技术方案做进一步的详细描述。
附图说明
[0077]
图1为本发明的一种基于行人导航的协同航向估计方法的流程图；
[0078]
图2为本发明的实施例的坐标系转换图；
[0079]
图3为本发明的实施例的点p在世界坐标系和图像平面坐标系中的投影图；
[0080]
图4为本发明的实施例的像素坐标系和图像平面坐标系示意图；
[0081]
图5为本发明的实施例的unet网络架构图；
[0082]
图6为本发明的试验例的验证结果图
[0083]
图7为本发明的试验例的沿第一参考线识别结果图；
[0084]
图8为本发明的试验例的第二参考线识别结果图。
具体实施方式
[0085]
以下将结合附图对本发明作进一步的描述，需要说明的是，本实施例以本技术方案为前提，给出了详细的实施方式和具体的操作过程，但本发明的保护范围并不限于本实施例。
[0086]
如图1所示，一种基于行人导航的协同航向估计方法，包括以下步骤：
[0087]
s1、处理智能手机内部mems传感器的测量值，以获得标题：
[0088]
s11、使用旋转矩阵将智能手机的身体坐标系转换为导航坐标系；
[0089]
优选的，步骤s11中的身体坐标系转换为导航坐标系的公式为：
[0090][0091]
式中，分别为基于x-轴、y-轴和z-轴的旋转矩阵；
[0092]
旋转矩阵公式为：
[0093][0094]
式中，ψ为载体的航偏角，机头左偏为正，γ为载体横滚角，右机翼向上为正，θ为载体俯仰角，机头向上抬起为正。
[0095]
优选的，载体横滚角γ计算公式如下：
[0096]
γ＝arctan2(-accx,accz)
[0097]
式中，accx和accz分别为经x轴和z轴加速度计获得测量值；
[0098]
载体俯仰角θ计算公式如下：
[0099][0100]
式中，accy为经y轴加速度计获得测量值。
[0101]
s12、通过加速度计和磁力计的测量值来计算智能手机的航向；
[0102]
优选的，步骤s12具体包括以下步骤：
[0103]
s121、计算磁北ψm：
[0104][0105]
式中，分别为磁力计三轴测量值；
[0106]
s122、计算真北ψ
truenorth
：
[0107]
ψ
truenorth
＝ψm+δψ-γ
[0108]
式中，δψ为磁偏赤，其由国际地磁场模型中得到；γ表示真北网格收敛。
[0109]
s2、结合陀螺仪的集成角度和初始方向来估计智能手机的航向；
[0110]
优选的，步骤s2的具体计算公式为：
[0111]
[0112]
式中，ωi是陀螺仪输出的角度更新率，tk和t
k+1
分别是开始时刻和结束时刻，heading
initial
是初始航向。
[0113]
s3、使用互补滤波器计算智能手机的标题；
[0114]
优选的，步骤s3采用如下融合算法：
[0115][0116]
式中，是由acc计算的航向以及mag测量；是上一时刻融合航向；α是平滑因子；δt是采样周期，即从开始时刻t
k-1
到结束时刻tk；
[0117]
其中：
[0118][0119][0120]
式中，rc表示等效的时间常数；fc是截止频率。
[0121]
s4、结合相机图像和深度的航向识别学习。
[0122]
优选的，步骤s4具体包括以下步骤：
[0123]
s41、计算行人航向与地面参考线的；
[0124]
优选的，步骤s41具体包括以下步骤：
[0125]
s411、将智能手机的垂直轴py投影到局部地面gy，结合参考线ab和交点a，创建一个局部地面坐标系，局部地面坐标系中参考线ab的方位角计算公式如下：
[0126][0127]
s412、如图2所示，由于智能手机身体坐标系和相机坐标系的原始点在不同的位置：一个是智能手机身体中心，另一个是摄像头的照明中心，px和cx方向相同，cy(+)、cz(+)和py(+)、pz(+)方向相反，这意味着智能手机坐标系沿px旋转180度，转换原始点偏差(6x、6y、6z)。然后可以得到摄像头的坐标系。实际上，如果只有旋转和坐标原点平移，那么角度偏差不能改变。如图3所示，xcocyc表示相机坐标系，u-v为像素坐标系，xoy为图像坐标系。p(xw,yw,zw)为世界大地坐标系中的点p的坐标，p(x，y)为图像上对应的点。
[0128]ow
—x
wywzw
是世界大地坐标系，单位为m；oc—xcyczc为相机坐标系，单位为m；o-xy为中的图形坐标系，单位为mm；u-v为像素中的像素坐标系，p为世界大地坐标系中的真点，p为(x、y)中的图形坐标系中的点，像素坐标为(u、v)。如图4所示，图像坐标系和像素坐标系都是平面坐标系，并且坐标轴指向同一方向。因此变换不会产生任何失真。
[0129]
像素坐标系和图像坐标系都是具有相同的尺度、相同的轴、不同的原点的平面坐标系，故获取世界大地坐标系和智能手机摄像头坐标系之间的变换公式：
[0130][0131]
式中，r和t分别为世界大地坐标系和智能手机摄像头坐标系之间的旋转矩阵和原点平移矩阵；dx和dy分别为行和列的一个像素，其单位为mm；f为摄像头的焦距；zc为转换系数；
[0132]
s413、带入参考线端点a和b，得到：
[0133][0134]
s414、在不考虑坐标系的旋转和平移的情况下，结合上式计算ab的两个点的坐标分量的差，获得以下公式：
[0135][0136]
此时参考线ab的方位角α记为：
[0137][0138]
s42、采用嵌入式智能手机的摄像头来捕捉图像，并利用采用神经学和深度学习的方法进行识别实时显示的地面基准线。
[0139]
优选的，步骤s42具体包括以下步骤：
[0140]
s421、收集地面参考线图片，作为数据集；
[0141]
本实施例中收集了1000多张地面参考线图片；
[0142]
s422、使用增强算法扩展数据集；
[0143]
增强算法包括autoaug和randaug；
[0144]
s423、将扩展后的数据集分为测试集和训练集；
[0145]
s424、在训练集中的训练图片创建标签，并将标签转换为json文件，最后将标签转换为单通道png；
[0146]
s425、将训练集输入unet对识别模型进行训练；
[0147]
如图5所示，unet最初用于医学图像的分割，unet的设计结合了经典的编解码器结构、卷积、池化和relu激活函数。此外，填充不用于防止位置偏移；使用+conv插值而不是上采样算法，如转置卷积，允许特征图保持均匀的色差和跳过连接，以保持更好的信息融合。
[0148]
s426、将测试集输入训练完毕的识别模型，输出图像包含预测的标签，即地面参考线。
[0149]
试验例：
[0150]
在不同背景、不同光、暗水平和不同智能手机姿态的不同场景中，对该识别模型进行了试验。由智能手机相机拍摄的原始rgb照片的大小，如华为mate20(相机是hma-al00)，有一个宽度高达2976像素，高度为3968像素，水平和垂直分辨率为96dpi。显然，这些图片太大了，故需要调整原始图片和标签的大小到一个可接受的大小。此时，需要平衡识别的时间延迟和准确性，需要输入适当大小的图片。
[0151]
在本试验例中，考虑到识别时间延迟和精度的平衡，所有训练后的jpg图像和标签图像的大小都调整为64*64。并以8：1：1的比例划分训练、测试和验证数据集。一旦准备工作完成，就开始训练这个模型。在这项研究中，唯一的目标是检测图像的地面参考线。图像的其他部分可以设置为背景。
[0152]
如图6所示，验证结果表明，所设计的网络能较好地提取出目标。
[0153]
在本试验中，训练数据集包含了800多张图片，将训练期的数量设置为200个，在这项研究中，在这项研究中，将经过训练的.h5模型转换为.tflite型号，可以在华为mate20智能手机上运行。智能手机相机捕捉到的实时图像的大小为1080*1920，而输入张量的大小为64*64，因此有必要调整图像的大小。输出图像包含预测的标签，即参考线。
[0154]
现场验证：
[0155]
选择华为mate20作为手持式测试。试验地点选择在学校的运动场上，地面上的白线为直线作为试验的地面参考线。应该注意的是，深度学习模型在本试验例训练大量不同类型的地面参考线，如车道线和盲巷，因此本试验例的算法可以实现实时识别和计算广泛的地面参考线。在本试验例中使用两种模态试验：一种是运动学试验：行人手里拿着智能手机，沿着地面参考线行走，另一个是静态测试。
[0156]
在运动学试验中，沿着两条地面参考线进行了运动学试验。首先，它的方位角是0.0142，第二个是180.0142。对于每条地面参考线，重复了测试三次。
[0157]
如图7所示，行人沿着第一条地面第一参考线行走(方位角为0.0142)。三个图形显示了不同的标题策略：上面的图形是“acc+mag”计算标题，中心的图形是“acc+mag+gyro”融合标题，下面的图形是结合imu测量的视觉融合结果。
[0158]
如图8所示，行人沿着第一条地面第二参考线行走(位角为180.0142)。它包含三个图形，并显示了不同的标题策略：上面的图形是“acc+mag”计算标题，中心图形是“acc+mag+gyro”融合标题，下面的图形是结合imu测量的视觉融合结果。
[0159]
表1为运动学航向试验统计结果表
[0160][0161]
表1中测试1-测试3是第二条参考线的平均航向偏差(方位为180.0142)，测试4-测试6是第一个参考线的平均航向偏差(方位为0.0142)
[0162]
在静态试验中，将智能手机安装在设备上，智能手机的纵轴与地面参考线倾斜一个角度，保证智能手机可以实时捕捉和识别地面参考线。
[0163]
表2为静态试验试验统计结果表
[0164][0165][0166]
表2中的测试1、测试2、测试3表示第二参考线(180.0142)上的重复试验的平均航向偏差，其余表示第一参考线上的重复试验结果(0.0142)。
[0167]
基于上述试验，从统计结果发现在，行人动态模参考线1和参考线2，acc+mag+陀螺仪融合标题精度分别提高了5％和15％，分别比acc+mag标题策略，提出视觉融合imu算法标题精度平均提高了5％(参考线1)和6％(参考线2)。在静态模式下，平均偏差在一定程度上降低，与acc+mag航向策略相比，acc+mag+陀螺融合航向精度的平均提高分别为14％和6％。综上所述，acc+mag+陀螺融合航向算法在动态和静态模式下都优于acc+mag航向策略，本发明提出的视觉融合imu航向算法进一步降低了航向偏差。从而证明了对提高行人导航和定位精度的影响。
[0168]
因此，本发明采用上述基于行人导航的协同航向估计方法，结合了智能手机的内部嵌入式结构传感器，如陀螺仪、加速度计、磁力计和相机测量，提高了航向估计精度。
[0169]
最后应说明的是：以上实施例仅用以说明本发明的技术方案而非对其进行限制，尽管参照较佳实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依
然可以对本发明的技术方案进行修改或者等同替换，而这些修改或者等同替换亦不能使修改后的技术方案脱离本发明技术方案的精神和范围。

技术特征：
1.一种基于行人导航的协同航向估计方法，其特征在于：包括以下步骤：s1、处理智能手机内部mems传感器的测量值，以获得标题：s11、使用旋转矩阵将智能手机的身体坐标系转换为导航坐标系；s12、通过加速度计和磁力计的测量值来计算智能手机的航向；s2、结合陀螺仪的集成角度和初始方向来估计智能手机的航向；s3、使用互补滤波器计算智能手机的标题；s4、结合相机图像和深度的航向识别学习。2.根据权利要求1所述的一种基于行人导航的协同航向估计方法，其特征在于：步骤s11中的身体坐标系转换为导航坐标系的公式为：式中，分别为基于x-轴、y-轴和z-轴的旋转矩阵；旋转矩阵公式为：式中，ψ为载体的航偏角，机头左偏为正，γ为载体横滚角，右机翼向上为正，θ为载体俯仰角，机头向上抬起为正。3.根据权利要求2所述的一种基于行人导航的协同航向估计方法，其特征在于：载体横滚角γ计算公式如下：γ＝arctan2(-accx,accz)式中，accx和accz分别为经x轴和z轴加速度计获得测量值；载体俯仰角θ计算公式如下：式中，accy为经y轴加速度计获得测量值。4.根据权利要求1所述的一种基于行人导航的协同航向估计方法，其特征在于：步骤s12具体包括以下步骤：s121、计算磁北ψ
m
：式中，分别为磁力计三轴测量值；s122、计算真北ψ
truenorth
：ψ
truenorth
＝ψ
m
+δψ-γ式中，δψ为磁偏赤，其由国际地磁场模型中得到；γ表示真北网格收敛。5.根据权利要求1所述的一种基于行人导航的协同航向估计方法，其特征在于：步骤s2的具体计算公式为：
式中，ω
i
是陀螺仪输出的角度更新率，t
k
和t
k+1
分别是开始时刻和结束时刻，heading
initial
是初始航向。6.根据权利要求1所述的一种基于行人导航的协同航向估计方法，其特征在于：步骤s3采用如下融合算法：式中，是由acc计算的航向以及mag测量；是上一时刻融合航向；α是平滑因子；δt是采样周期，即从开始时刻t
k-1
到结束时刻t
k
；其中：其中：式中，rc表示等效的时间常数；f
c
是截止频率。7.根据权利要求1所述的一种基于行人导航的协同航向估计方法，其特征在于：步骤s4具体包括以下步骤：s41、计算行人航向与地面参考线的；s42、采用嵌入式智能手机的摄像头来捕捉图像，并利用采用神经学和深度学习的方法进行识别实时显示的地面基准线。8.根据权利要求7所述的一种基于行人导航的协同航向估计方法，其特征在于：步骤s41具体包括以下步骤：s411、将智能手机的垂直轴py投影到局部地面gy，结合参考线ab和交点a，创建一个局部地面坐标系，局部地面坐标系中参考线ab的方位角计算公式如下：s412、获取世界大地坐标系和智能手机摄像头坐标系之间的变换公式：式中，r和t分别为世界大地坐标系和智能手机摄像头坐标系之间的旋转矩阵和原点平移矩阵；dx和dy分别为行和列的一个像素，其单位为mm；f为摄像头的焦距；zc为转换系数；s413、带入参考线端点a和b，得到：
s414、在不考虑坐标系的旋转和平移的情况下，结合上式计算ab的两个点的坐标分量的差，获得以下公式：此时参考线ab的方位角α记为：9.根据权利要求7所述的一种基于行人导航的协同航向估计方法，其特征在于：步骤s42具体包括以下步骤：s421、收集地面参考线图片，作为数据集；s422、使用增强算法扩展数据集；s423、将扩展后的数据集分为测试集和训练集；s424、在训练集中的训练图片创建标签，并将标签转换为json文件，最后将标签转换为单通道png；s425、将训练集输入unet对识别模型进行训练；s426、将测试集输入训练完毕的识别模型，输出图像包含预测的标签，即地面参考线。

技术总结
本发明公开了一种基于行人导航的协同航向估计方法，包括以下步骤：S1、处理智能手机内部MEMS传感器的测量值，以获得标题：S11、使用旋转矩阵将智能手机的身体坐标系转换为导航坐标系；S12、通过加速度计和磁力计的测量值来计算智能手机的航向；S2、结合陀螺仪的集成角度和初始方向来估计智能手机的航向；S3、使用互补滤波器计算智能手机的标题；S4、结合相机图像和深度的航向识别学习。本发明采用上述基于行人导航的协同航向估计方法，结合了智能手机的内部嵌入式结构传感器，如陀螺仪、加速度计、磁力计和相机测量，提高了航向估计精度。提高了航向估计精度。提高了航向估计精度。


技术研发人员：叶俊华 章怿钦 朱伟 王泽 范晓燕 周素茵 徐爱俊
受保护的技术使用者：浙江农林大学
技术研发日：2023.06.19
技术公布日：2023/9/20