CN108106613B - 基于视觉辅助的定位方法与系统 - Google Patents

Abstract

本发明提供了一种基于视觉辅助的定位方法，包含以下步骤：数据采集判断步骤：获取来自惯性传感器的惯性传感器数据，根据惯性传感器数据判断步态动作是否发生；步长估算步骤：当判定步态动作发生时，根据惯性传感器数据估算得到步长；偏航角计算步骤：获取来自视觉陀螺仪的视觉数据，对视觉数据与惯性传感器数据进行卡尔曼滤波获得偏航角；位置计算步骤：根据步长与偏航角，计算获得位置信息。相应地，本发明还提供了一种基于视觉辅助的定位系统。本发明不仅在行人航向上做了融合，还在步长估计上做了大量离线测量，找到最佳的步长因子，为行人在室内的定位提供了可靠的保证。

Description

基于视觉辅助的定位方法与系统

技术领域

本发明涉及室内定位技术，具体地，涉及一种基于视觉辅助的定位方法与系统。

背景技术

基于位置的服务在军事、急救和商业领域都有着十分重要的应用。然而在室外占主导地位的GNSS(Global Navigation Satellite System)并不能在室内提供定位服务。日益普及的智能手机无疑是个人应用中基于位置服务的主要载体，而智能手机丰富的环境感知能力使其具备了在室内定位用户的能力。

随着廉价的基于微机电系统(Micro-Electro-Mechanical Systems MEMS)的IMU传感器的普及，行人航位推算(Pedestrian Dead Reckoning,PDR)成为近10年中室内定位领域的研究热点。传统的PDR定位方法就是通过获得手机已有的惯性传感器如加速度计，磁力仪，陀螺仪等，基于粒子滤波的方法，逐步迭代获得行人的室内位置。然而，由于惯性传感器数据存在漂移，随着时间的推移，这种误差就会越来越大，导致PDR定位的精度随着时间的推移而减小。

发明内容

针对现有技术中的缺陷，本发明的目的是提供一种基于视觉辅助的定位方法与系统。

根据本发明提供的基于视觉辅助的定位方法，包含以下步骤：

数据采集判断步骤：获取来自惯性传感器的惯性传感器数据，根据惯性传感器数据判断步态动作是否发生；

步长估算步骤：当判定步态动作发生时，根据惯性传感器数据估算得到步长；

偏航角计算步骤：获取来自视觉陀螺仪的视觉数据，对视觉数据与惯性传感器数据进行卡尔曼滤波获得偏航角；

位置计算步骤：根据步长与偏航角，计算获得位置信息。

优选地，所述步长估算步骤包含以下任一个或全部步骤：

离线步长估算步骤：根据步长估计模型计算获取步长，所述步长估计模型通过统计多个相同或不同身高、体重、性别人员信息进行离线生成；

在线步长估算步骤：根据惯性传感器数据，按如下公式计算获得步长：

SL_k＝α·freq+β

式中：SL_k为k时刻行人的步长；

k为采样时刻；

α、β为比例因子；

freq为步频，惯性传感器数据包含步频。

优选地，所述惯性传感器数据包含磁力航向角θ^m，所述视觉数据包含视觉航向角θ^v；

在偏航角计算步骤中，对θ^m与θ^v按如下公式进行卡尔曼滤波：

式中：

为k时刻偏航角变化率；

θ_k为k时刻偏航角；

Δt为k-1时刻到k时刻之间的时间间隔；

为k-1时刻偏航角变化率；

θ_k-1为k-1时刻偏航角；

ω_k-1为k-1时刻过程噪声，服从高斯分布；

为k时刻视觉航向角θ^v变化率；

为k时刻磁力航向角θ^m；

v_k为k时刻观测噪声，服从高斯分布。

优选地，所述视觉航向角θ^v通过以下步骤获取：

高频滤波步骤：将进入到视觉陀螺仪的图像帧进行Canny边缘检测，对所述图像帧进行高频滤波，获得滤波图像；

霍夫变换步骤：利用霍夫变换检测滤波图像中直线线条；

线条筛选步骤：筛选出滤波图像中垂直于图像平面的结构线条；

消失点确认步骤：根据结构线条，确定滤波图像中的消失点；

视觉航向角估算步骤：利用得到的消失点估算得到视觉航向角，公式如下：

式中：x为像素横坐标；

u为像素中心点的横坐标；

f_x为相机焦距；

线条筛选步骤中，在所有直线线条中选取左侧线条l_l与右侧线条l_r，通过调整左侧线条l_l与右侧线条l_r的角度调整阈值，以获取所述结构线条；

消失点确认步骤中，消失点通过如下公式获取：

vp＝{dot|dot＝l_l∩l_r}

式中：vp为消失点；

dot为线l_l和l_r的交点；

消失点确认步骤还包含消失点误差消除步骤，所述误差消除步骤包含以下步骤：

步骤S1：求出所有消失点的横纵坐标的中位数，将检测到的与中位数差别大于设定值的消失点剔除掉；

步骤S2：求剩下的所有消失点的方差。

优选地，所述位置计算步骤包含以下任一个步骤：

直接位置更新步骤：将步长与偏航角代入到如下公式进行位置更新：

卡尔曼滤波位置更新步骤：将步长与偏航角代入到如下公式进行卡尔曼滤波位置更新：

式中：x_k+1为k+1时刻行人位置横坐标；

x_k为k时刻行人位置横坐标；

SL_k为k时刻行人步长；

θ_k为k时刻偏航角；

y_k+1为k+1时刻行人位置纵坐标；

y_k为k时刻行人位置纵坐标；

为k+1时刻偏航角变化率；

为k时刻偏航角变化率；

θ_k+1为k+1时刻偏航角；

Δt为k时刻到k+1时刻之间的时间间隔；

w₁,w₂,w₃,w₄为相互独立的系统过程噪声，均服从高斯分布。

本发明还提供了一种基于视觉辅助的定位系统，包含以下模块：

数据采集判断模块：获取来自惯性传感器的惯性传感器数据，根据惯性传感器数据判断步态动作是否发生；

步长估算模块：当判定步态动作发生时，根据惯性传感器数据估算得到步长；

偏航角计算模块：获取来自视觉陀螺仪的视觉数据，对视觉数据与惯性传感器数据进行卡尔曼滤波获得偏航角；

位置计算模块：根据步长与偏航角，计算获得位置信息。

优选地，所述步长估算模块包含以下任一个或全部模块：

离线步长估算模块：根据步长估计模型计算获取步长，所述步长估计模型通过统计多个相同或不同身高、体重、性别人员信息进行离线生成；

在线步长估算模块：根据惯性传感器数据，按如下公式计算获得步长：

SL_k＝α·freq+β

式中：SL_k为k时刻行人的步长；

k为采样时刻；

α、β为比例因子；

freq为步频，惯性传感器数据包含步频。

优选地，所述惯性传感器数据包含磁力航向角θ^m，所述视觉数据包含视觉航向角θ^v；

在偏航角计算模块中，对θ^m与θ^v按如下公式进行卡尔曼滤波：

式中：

为k时刻偏航角变化率；

θ_k为k时刻偏航角；

Δt为k-1时刻到k时刻之间的时间间隔；

为k-1时刻偏航角变化率；

θ_k-1为k-1时刻偏航角；

ω_k-1为k-1时刻过程噪声，服从高斯分布；

为k时刻视觉航向角θ^v变化率；

为k时刻磁力航向角θ^m；

v_k为k时刻观测噪声，服从高斯分布。

优选地，所述视觉航向角θ^v通过以下模块获取：

高频滤波模块：将进入到视觉陀螺仪的图像帧进行Canny边缘检测，对所述图像帧进行高频滤波，获得滤波图像；

霍夫变换模块：利用霍夫变换检测滤波图像中直线线条；

线条筛选模块：筛选出滤波图像中垂直于图像平面的结构线条；

消失点确认模块：根据结构线条，确定滤波图像中的消失点；

视觉航向角估算模块：利用得到的消失点估算得到视觉航向角，公式如下：

式中：x为像素横坐标；

u为像素中心点的横坐标；

f_x为相机焦距；

线条筛选模块中，在所有直线线条中选取左侧线条l_l与右侧线条l_r，通过调整左侧线条l_l与右侧线条l_r的角度调整阈值，以获取所述结构线条；

消失点确认模块中，消失点通过如下公式获取：

vp＝{dot|dot＝l_l∩l_r}

式中：vp为消失点；

dot为线l_l和l_r的交点；

消失点确认模块还包含消失点误差消除模块，所述误差消除模块包含以下模块：

模块M1：求出所有消失点的横纵坐标的中位数，将检测到的与中位数差别大于设定值的消失点剔除掉；

模块M2：求剩下的所有消失点的方差。

优选地，所述位置计算模块包含以下任一个模块：

直接位置更新模块：将步长与偏航角代入到如下公式进行位置更新：

卡尔曼滤波位置更新模块：将步长与偏航角代入到如下公式进行卡尔曼滤波位置更新：

式中：x_k+1为k+1时刻行人位置横坐标；

x_k为k时刻行人位置横坐标；

SL_k为k时刻行人步长；

θ_k为k时刻偏航角；

y_k+1为k+1时刻行人位置纵坐标；

y_k为k时刻行人位置纵坐标；

为k+1时刻偏航角变化率；

为k时刻偏航角变化率；

θ_k+1为k+1时刻偏航角；

Δt为k时刻到k+1时刻之间的时间间隔；

w₁,w₂,w₃,w₄为相互独立的系统过程噪声，均服从高斯分布。

与现有技术相比，本发明具有如下的有益效果：

1、本发明不仅在行人航向上做了融合，还在步长估计上做了大量离线测量，找到最佳的步长因子，为行人在室内的定位提供了可靠的保证。

2、本发明为避免在仅使用PDR的情况下测得的行人航向受到环境中的干扰而不断发生变化，引入了视觉辅助，航向得到了很好的修正，从而定位结果也得到了很大的改善。

附图说明

通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：

图1为本发明原理图；

图2为本发明提供的基于视觉辅助的定位方法流程图；

图3为视觉航向角获取步骤流程图。

具体实施方式

下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进。这些都属于本发明的保护范围。

在本发明的描述中，需要理解的是，术语“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。

如图1、图2所示，本发明提供的基于视觉辅助的定位方法包含以下步骤：数据采集判断步骤：获取来自惯性传感器的惯性传感器数据，根据惯性传感器数据判断步态动作是否发生；步长估算步骤：当判定步态动作发生时，根据惯性传感器数据估算得到步长；偏航角计算步骤：获取来自视觉陀螺仪的视觉数据，对视觉数据与惯性传感器数据进行卡尔曼滤波获得偏航角；位置计算步骤：根据步长与偏航角，计算获得位置信息。所述惯性传感器包含三轴加速度计，三轴陀螺仪，三轴磁力计，由惯性传感器检测到的数据如加速度值、角速度、磁场强度、磁场方向等作为PDR系统的输入，由PDR估计得出的PDR系统行人步长与PDR系统航向角，分别记为SL和θ^p(即图1中左边的“航向θ”)。实际使用过程中，上述的惯性传感器与视觉陀螺仪可以是存在于便携式设备中，如智能手机、平板电脑、可穿戴智能眼镜等。

实施例中，所述步长估算步骤包含以下任一个或全部步骤：离线步长估算步骤：根据步长估计模型计算获取步长，所述步长估计模型通过统计多个相同或不同身高、体重、性别人员信息进行离线生成；在线步长估算步骤：根据惯性传感器数据，按如下公式计算获得步长：

SL_k＝α·freq+β

式中：SL_k为k时刻行人的步长；k为采样时刻；α、β为比例因子；freq为步频，惯性传感器数据包含步频。

所述惯性传感器数据包含磁力航向角θ^m，所述磁力航向角θ^m由磁力计MARG测量获得，磁力航向角θ^m属于PDR系统航向角θ^p的一部分；所述视觉数据包含视觉航向角θ^v(即图1中右边的“航向θ”)；在偏航角计算步骤中，对θ^m与θ^v按如下公式进行卡尔曼滤波：

式中：

为k时刻偏航角变化率；θ_k为k时刻偏航角；Δt为k-1时刻到k时刻之间的时间间隔；

为k-1时刻偏航角变化率；θ_k-1为k-1时刻偏航角；ω_k-1为k-1时刻过程噪声，服从高斯分布；

为k时刻视觉航向角θ^v变化率；

为k时刻磁力航向角θ^m；v_k为k时刻观测噪声，服从高斯分布。

通过摄像头得到一段过程的原始数据之后(可以是视频，也可以是图像)，首先要做的是筛选原始数据，即剔除模糊的数据，然后将这些图像数据放入视觉陀螺仪(VG)系统中。具体地，所述视觉航向角θ^v通过以下步骤获取：高频滤波步骤：将进入到视觉陀螺仪的图像帧进行Canny边缘检测，对所述图像帧进行高频滤波，获得滤波图像；霍夫变换步骤：利用霍夫变换检测滤波图像中直线线条；线条筛选步骤：筛选出滤波图像中垂直于图像平面的结构线条；消失点确认步骤：根据结构线条，确定滤波图像中的消失点；视觉航向角估算步骤：利用得到的消失点估算得到视觉航向角，公式如下：

式中：x为像素横坐标；u为像素中心点的横坐标；f_x为相机焦距。

霍夫变换是图像处理中从图像中识别几何形状的基本方法之一。霍夫变换的基本原理在于利用点与线的对偶性，将原始图像空间的给定的曲线通过曲线表达形式变为参数空间的一个点。这样就把原始图像中给定曲线的检测问题转化为寻找参数空间中的峰值问题。也即把检测整体特性转化为检测局部特性。比如直线、椭圆、圆、弧线等。本发明就是利用了霍夫变换来检测直线，进而寻找消失点。理论上，霍夫变换步骤能够检测到当前图像帧中所有的直线线条，但是完全横线条和垂直线条是不需要的，因为没有信息能够提取，需要的仅仅是沿着行人行进方向的直线，即垂直图象平面的直线。因此在线条筛选步骤中，在所有直线线条中选取左侧线条l_l与右侧线条l_r，通过调整左侧线条l_l与右侧线条l_r的角度调整阈值，以获取所述结构线条。左侧线条l_l与右侧线条l_r角度越小，表示阈值越小，检测到的满足要求的线条就越多，反之越少。在得到满足要求的结构线条之后，消失点就可以确定了，消失点确认步骤中，消失点通过如下公式获取：

vp＝{dot|dot＝l_l∩l_r}

式中：vp为消失点；dot为线l_l和l_r的交点。

一般地，任一图像帧中得到的消失点数量是不一样的，由于存在噪声，可能会导致消失点分布不集中，将所有消失点进行统计平均，能够很好的消除误差。具体地，消失点确认步骤还包含消失点误差消除步骤，所述误差消除步骤包含以下步骤：步骤S1：求出所有消失点的横纵坐标的中位数，将检测到的与中位数差别大于设定值的消失点剔除掉；步骤S2：求剩下的所有消失点的方差。如果方差较大，说明消失点不稳定，视觉信息的可信度就很小，反之可信度较大。

所述位置计算步骤包含以下任一个步骤：直接位置更新步骤：将步长与偏航角代入到如下公式进行位置更新：

卡尔曼滤波位置更新步骤：将步长与偏航角代入到如下公式进行卡尔曼滤波位置更新：

式中：x_k+1为k+1时刻行人位置横坐标；x_k为k时刻行人位置横坐标；SL_k为k时刻行人步长；θ_k为k时刻偏航角；y_k+1为k+1时刻行人位置纵坐标；y_k为k时刻行人位置纵坐标；

为k+1时刻偏航角变化率；

为k时刻偏航角变化率；θ_k+1为k+1时刻偏航角；Δt为k时刻到k+1时刻之间的时间间隔；w₁,w₂,w₃,w₄为相互独立的系统过程噪声，均服从高斯分布。

本发明还提供了一种基于视觉辅助的定位系统，上述的基于视觉辅助的定位方法的步骤流程可以视作基于视觉辅助的定位系统的优选实施方式，基于视觉辅助的定位系统的功能可以通过执行基于视觉辅助的定位方法中的步骤来实现。所述基于视觉辅助的定位系统包含以下模块：数据采集判断模块：获取来自惯性传感器的惯性传感器数据，根据惯性传感器数据判断步态动作是否发生；步长估算模块：当判定步态动作发生时，根据惯性传感器数据估算得到步长；偏航角计算模块：获取来自视觉陀螺仪的视觉数据，对视觉数据与惯性传感器数据进行卡尔曼滤波获得偏航角；位置计算模块：根据步长与偏航角，计算获得位置信息。

所述步长估算模块包含以下任一个或全部模块：离线步长估算模块：根据步长估计模型计算获取步长，所述步长估计模型通过统计多个相同或不同身高、体重、性别人员信息进行离线生成；在线步长估算模块：根据惯性传感器数据，按如下公式计算获得步长：

SL_k＝α·freq+β

式中：SL_k为k时刻行人的步长；k为采样时刻；α、β为比例因子；freq为步频，惯性传感器数据包含步频。

所述惯性传感器数据包含磁力航向角θ^m，所述视觉数据包含视觉航向角θ^v；在偏航角计算模块中，对θ^m与θ^v按如下公式进行卡尔曼滤波：

式中：

为k时刻偏航角变化率；θ_k为k时刻偏航角；Δt为k-1时刻到k时刻之间的时间间隔；

为k-1时刻偏航角变化率；θ_k-1为k-1时刻偏航角；ω_k-1为k-1时刻过程噪声，服从高斯分布；

为k时刻视觉航向角θ^v变化率；

为k时刻磁力航向角θ^m；v_k为k时刻观测噪声，服从高斯分布。

所述视觉航向角θ^v通过以下模块获取：高频滤波模块：将进入到视觉陀螺仪的图像帧进行Canny边缘检测，对所述图像帧进行高频滤波，获得滤波图像；霍夫变换模块：利用霍夫变换检测滤波图像中直线线条；线条筛选模块：筛选出滤波图像中垂直于图像平面的结构线条；消失点确认模块：根据结构线条，确定滤波图像中的消失点；视觉航向角估算模块：利用得到的消失点估算得到视觉航向角，公式如下：

式中：x为像素横坐标；u为像素中心点的横坐标；f_x为相机焦距。

线条筛选模块中，在所有直线线条中选取左侧线条l_l与右侧线条l_r，通过调整左侧线条l_l与右侧线条l_r的角度调整阈值，以获取所述结构线条；消失点确认模块中，消失点通过如下公式获取：

vp＝{dot|dot＝l_l∩l_r}

式中：vp为消失点；dot为线l_l和l_r的交点。

消失点确认模块还包含消失点误差消除模块，所述误差消除模块包含以下模块：模块M1：求出所有消失点的横纵坐标的中位数，将检测到的与中位数差别大于设定值的消失点剔除掉；模块M2：求剩下的所有消失点的方差。

所述位置计算模块包含以下任一个模块：直接位置更新模块：将步长与偏航角代入到如下公式进行位置更新：

卡尔曼滤波位置更新模块：将步长与偏航角代入到如下公式进行卡尔曼滤波位置更新：

式中：x_k+1为k+1时刻行人位置横坐标；x_k为k时刻行人位置横坐标；SL_k为k时刻行人步长；θ_k为k时刻偏航角；y_k+1为k+1时刻行人位置纵坐标；y_k为k时刻行人位置纵坐标；

为k+1时刻偏航角变化率；

为k时刻偏航角变化率；θ_k+1为k+1时刻偏航角；Δt为k时刻到k+1时刻之间的时间间隔；w₁,w₂,w₃,w₄为相互独立的系统过程噪声，均服从高斯分布。

本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的控制器及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的控制器及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的控制器及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变形或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。

Claims (6)

1.一种基于视觉辅助的定位方法，其特征在于，包含以下步骤：

数据采集判断步骤：获取来自惯性传感器的惯性传感器数据，根据惯性传感器数据判断步态动作是否发生；

步长估算步骤：当判定步态动作发生时，根据惯性传感器数据估算得到步长；

偏航角计算步骤：获取来自视觉陀螺仪的视觉数据，对视觉数据与惯性传感器数据进行卡尔曼滤波获得偏航角；

位置计算步骤：根据步长与偏航角，计算获得位置信息；

所述步长估算步骤包含以下任一个或全部步骤：

离线步长估算步骤：根据步长估计模型计算获取步长，所述步长估计模型通过统计多个相同或不同身高、体重、性别人员信息进行离线生成；

在线步长估算步骤：根据惯性传感器数据，按如下公式计算获得步长：

SL_k＝α·freq+β

式中：SL_k为k时刻行人的步长；

k为采样时刻；

α、β为比例因子；

freq为步频，惯性传感器数据包含步频；

所述惯性传感器数据

包含磁力航向角θ^m，所述视觉数据包含视觉航向角θ^v；

在偏航角计算步骤中，对θ^m与θ^v按如下公式进行卡尔曼滤波：

式中：

为k时刻偏航角变化率；

θ_k为k时刻偏航角；

Δt为k-1时刻到k时刻之间的时间间隔；

为k-1时刻偏航角变化率；

θ_k-1为k-1时刻偏航角；

ω_k-1为k-1时刻过程噪声，服从高斯分布；

为k时刻视觉航向角θ^v变化率；

为k时刻磁力航向角θ^m；

v_k为k时刻观测噪声，服从高斯分布。

2.根据权利要求1所述的基于视觉辅助的定位方法，其特征在于，所述视觉航向角θ^v通过以下步骤获取：

高频滤波步骤：将进入到视觉陀螺仪的图像帧进行Canny边缘检测，对所述图像帧进行高频滤波，获得滤波图像；

霍夫变换步骤：利用霍夫变换检测滤波图像中直线线条；

线条筛选步骤：筛选出滤波图像中垂直于图像平面的结构线条；

消失点确认步骤：根据结构线条，确定滤波图像中的消失点；

视觉航向角估算步骤：利用得到的消失点估算得到视觉航向角，公式如下：

式中：x为像素横坐标；

u为像素中心点的横坐标；

f_x为相机焦距；

线条筛选步骤中，在所有直线线条中选取左侧线条l_l与右侧线条l_r，通过调整左侧线条l_l与右侧线条l_r的角度调整阈值，以获取所述结构线条；

消失点确认步骤中，消失点通过如下公式获取：vp

＝{dot|dot＝l_l∩l_r}

式中：vp为消失点；

dot为线l_l和l_r的交点；

消失点确认步骤还包含消失点误差消除步骤，所述误差消除步骤包含以下步骤：

步骤S1：求出所有消失点的横纵坐标的中位数，将检测到的与中位数差别大于设定值的消失点剔除掉；

步骤S2：求剩下的所有消失点的方差。

3.根据权利要求1所述的基于视觉辅助的定位方法，其特征在于，所述位置计算步骤包含以下任一个步骤：

直接位置更新步骤：将步长与偏航角代入到如下公式进行位置更新：

卡尔曼滤波位置更新步骤：将步长与偏航角代入到如下公式进行卡尔曼滤波位置更新：

式中：x_k+1为k+1时刻行人位置横坐标；

x_k为k时刻行人位置横坐标；

SL_k为k时刻行人步长；

θ_k为k时刻偏航角；

y_k+1为k+1时刻行人位置纵坐标；

y_k为k时刻行人位置纵坐标；

为k+1时刻偏航角变化率；

为k时刻偏航角变化率；

θ_k+1为k+1时刻偏航角；

Δt为k时刻到k+1时刻之间的时间间隔；

w₁,w₂,w₃,w₄为相互独立的系统过程噪声，均服从高斯分布。

4.一种基于视觉辅助的定位系统，其特征在于，包含以下模块：

数据采集判断模块：获取来自惯性传感器的惯性传感器数据，根据惯性传感器数据判断步态动作是否发生；

步长估算模块：当判定步态动作发生时，根据惯性传感器数据估算得到步长；

偏航角计算模块：获取来自视觉陀螺仪的视觉数据，对视觉数据与惯性传感器数据进行卡尔曼滤波获得偏航角；

位置计算模块：根据步长与偏航角，计算获得位置信息；

所述步长估算模块包含以下任一个或全部模块：

离线步长估算模块：根据步长估计模型计算获取步长，所述步长估计模型通过统计多个相同或不同身高、体重、性别人员信息进行离线生成；

在线步长估算模块：根据惯性传感器数据，按如下公式计算获得步长：SL_k＝

α·freq+β

式中：SL_k为k时刻行人的步长；

k为采样时刻；

α、β为比例因子；

freq为步频，惯性传感器数据包含步频；

述惯性传感器数据包含磁力航向角θ^m，所述视觉数据包含视觉航向角θ^v；

在偏航角计算模块中，对θ^m与θ^v按如下公式进行卡尔曼滤波：

式中：

为k时刻偏航角变化率；

θ_k为k时刻偏航角；

Δt为k-1时刻到k时刻之间的时间间隔；

为k-1时刻偏航角变化率；

θ_k-1为k-1时刻偏航角；

ω_k-1为k-1时刻过程噪声，服从高斯分布；

为k时刻视觉航向角θ^v变化率；

为k时刻磁力航向角θ^m；

v_k为k时刻观测噪声，服从高斯分布。

5.根据权利要求4所述的基于视觉辅助的定位系统，其特征在于，所述视觉航向角θ^v通过以下模块获取：

高频滤波模块：将进入到视觉陀螺仪的图像帧进行Canny边缘检测，对所述图像帧进行高频滤波，获得滤波图像；

霍夫变换模块：利用霍夫变换检测滤波图像中直线线条；

线条筛选模块：筛选出滤波图像中垂直于图像平面的结构线条；

消失点确认模块：根据结构线条，确定滤波图像中的消失点；

视觉航向角估算模块：利用得到的消失点估算得到视觉航向角，公式如下：

式中：x为像素横坐标；

u为像素中心点的横坐标；

f_x为相机焦距；

线条筛选模块中，在所有直线线条中选取左侧线条l_l与右侧线条l_r，通过调整左侧线条l_l与右侧线条l_r的角度调整阈值，以获取所述结构线条；

消失点确认模块中，消失点通过如下公式获取：vp

＝{dot|dot＝l_l∩l_r}

式中：vp为消失点；

dot为线l_l和l_r的交点；

消失点确认模块还包含消失点误差消除模块，所述误差消除模块包含以下模块：

模块M1：求出所有消失点的横纵坐标的中位数，将检测到的与中位数差别大于设定值的消失点剔除掉；

模块M2：求剩下的所有消失点的方差。

6.根据权利要求4所述的基于视觉辅助的定位系统，其特征在于，所述位置计算模块包含以下任一个模块：

直接位置更新模块：将步长与偏航角代入到如下公式进行位置更新：

卡尔曼滤波位置更新模块：将步长与偏航角代入到如下公式进行卡尔曼滤波位置更新：

式中：x_k+1为k+1时刻行人位置横坐标；

x_k为k时刻行人位置横坐标；

SL_k为k时刻行人步长；

θ_k为k时刻偏航角；

y_k+1为k+1时刻行人位置纵坐标；

y_k为k时刻行人位置纵坐标；

为k+1时刻偏航角变化率；

为k时刻偏航角变化率；

θ_k+1为k+1时刻偏航角；

Δt为k时刻到k+1时刻之间的时间间隔；

w₁,w₂,w₃,w₄为相互独立的系统过程噪声，均服从高斯分布。

Images