CN106370189B - 一种基于多传感器融合的室内导航装置及方法 - Google Patents

Abstract

一种基于多传感器融合的室内导航装置及方法，构建当前室内的磁地图；实时测量室内当前位置的基础信息数据，该基础信息数据包括：楼层当前位置相对于地面的气压差ΔP，当前时刻所在位置与前一时刻所在位置之间的行走路径上的磁感应强度时间序列和磁场梯度矩阵时间序列，人体或者AGV小车在行走路径上行进时的XYZ三方向加速度的时间序列值，人体或者AGV小车在行走路径上行进时的XYZ三方向角速度的时间序列值，人体或者AGV小车在行走路径上行进时的X轴方向前方的障碍物距离。将基础信息数据进行融合，定位实时位置并进行直线分段导航。本发明导航精准，定位准确。

Description

一种基于多传感器融合的室内导航装置及方法

技术领域

本发明属于基于地磁分布的室内导航领域，尤其涉及一种基于磁梯度等多个传感器融合的室内导航装置及方法。

背景技术

随着经济社会的快速发展，人们活动的室内空间变得日益庞大和复杂，在大型地下停车场、购物百货商场、大型物流仓库等室内场所确定自己或者AGV的位置，并沿着特定路径导航到室内某一目的地显得力不从心，尤其是盲人，缺乏视觉感知能力时室内导航更为困难，因此迫切需要一种室内定位导航技术。

尽管室内导航需求强烈，传统的导航技术却因各种技术限制，无法满足室内定位要求。比如，目前室外应用最广泛的GPS技术在室内使用时，卫星信号容易被建筑物阻隔，导致无法实现准确定位；尽管手机的无线通讯信号可以穿透多数建筑的墙壁，但移动通信基站的分布密度太低，利用手机无线通讯信号进行定位的精度不足；基于Wi-Fi的定位技术需要了解每一个Wi-Fi热点的准确位置，甚至对每一个Wi-Fi信号强度进行测量，测量成本太高，定位精度难以保证。

近年来出现了基于地磁分布的室内定位方法，其原理是基于大型建筑物墙壁中的钢筋和室内货架、桌脚等铁磁性物质对室内地球磁场的改变，利用磁场传感器检测室内不同位置的不同磁场大小来进行定位。现有室内磁场导航方法的不足之处主要在于：磁场是一个梯度场，而仅仅测量磁场三分量数据并没有充分利用磁场分布信息，很有可能限于传感器的测量精度和室内地磁场分布变化差异性太小导致测量“盲区”。

发明内容

本发明要解决的技术问题是提供一种导航精确，定位精准的室内导航装置及方法。

为了解决上述技术问题，本发明采取以下技术方案：

一种基于多传感器融合的室内导航装置，所述装置包括室内磁地图构建模块、测量模块、导航定位模块、中央处理器、数据存储模块、无线通讯模块和反馈交互模块，室内磁地图构建模块、测量模块、导航定位模块、数据存储模块、无线通讯模块和反馈交互模块分别与中央处理器进行数据互联通讯。

所述室内磁地图构建模块包括楼层高度测量模块、室内路径分段模块、磁场三分量测量模块和磁场梯度测量模块。

所述测量模块包括气压传感器、磁梯度传感器、加速度计传感器、陀螺仪传感器和超声波传感器，该气压传感器、磁梯度传感器、加速度计传感器、陀螺仪传感器和超声波传感器之间通过中央处理器进行数据通讯。

所述导航定位模块内置有相互之间进行数据通讯交换的气压传感器楼高测量单元、磁场地图匹配定位单元、加速度计短距离补偿单元、陀螺仪旋转角度定位单元和超声波传感器测距单元。

所述数据存储模块包括SD卡、硬盘和/或EEPROM。

所述无线通讯模块包括行走路径云记录单元和路径磁场更新单元。

所述反馈交互模块包括耳机、左手震动器、右手震动器、小键盘、LED和喇叭。

一种基于多传感器融合的室内导航方法，所述方法包括以下步骤：

S1,构建当前室内的磁地图；

S2，实时测量室内当前位置的基础信息数据，该基础信息数据包括：楼层当前位置相对于地面的气压差ΔP，当前时刻所在位置与前一时刻所在位置之间的行走路径上的磁感应强度时间序列和磁场梯度矩阵时间序列，人体或者AGV小车在行走路径上行进时的XYZ三方向加速度的时间序列值，人体或者AGV小车在行走路径上行进时的XYZ三方向角速度的时间序列值，人体或者AGV小车在行走路径上行进时的X轴方向前方的障碍物距离；

S3,将基础信息数据进行融合，定位实时位置并进行直线分段导航。

所述步骤S1具体包括：

S1.1，将行走路径上的分叉口为断点，对室内平面图中所有的行走路径进行直线段离散分段处理得到离散直线段分段路径，对每一离散直线段分段路径和分叉口进行唯一地址编号，该分叉口包括行走路径上的拐弯路口、十字形路口和T字形路口；

S1.2，在每一个离散直线段分段路径上匀速行走，采集该离散直线段分段路径中每一点上的XYZ三方向磁感应强度平均值

和磁场梯度矩阵g＝[gradient_xgradient_y gradient_z]，该磁感应强度平均值和磁场梯度矩阵构成磁场数据，所有离散直线段分段路径的磁场数据构成磁地图数据；

S1.3，将离散直线段分段路径唯一地址编号和相应的磁地图数据配对存储，从而生成用于室内导航的磁地图。

所述步骤S3具体包括：

S3.1，利用公式Δh＝ΔP/(ρg)获得楼层当前位置相对于地面的高度，其中ρ为空气密度，g为重力加速度；

S3.2,以前一时刻所在的已知位置为起点，确定当前时刻的位置为导航目标，确定前一时刻到当前时刻的该段路径为匹配对象，将该段路径中测得的磁感应强度时间序列

和磁场梯度矩阵时间序列G＝[gradient_X gradient_Y gradient_Z]，与磁地图中的从前一已知位置开始2倍于匹配对象行走时间的时间范围内的磁感应强度时间序列

和磁场梯度矩阵时间序列g＝[gradient_x gradienty_y gradient_z]进行匹配，确定当前时刻所在位置；

S3.3，在行走路径中，若某一段路径的磁感应强度和磁场梯度矩阵变化小于设定值，获取在该段路径上行走时的XYZ轴三方向加速度的时间序列值a_x,a_y,a_z，并且对该a_x,a_y,a_z进行时间的双积分，预测该三方向的行走距离s_x,s_y,s_z：s_x＝∫∫a_xdt,s_y＝∫∫a_ydt,s_z＝∫∫a_zdt；

S3.4，在行走路径上拐弯时，实时确定拐弯角度的大小，获取当前拐弯处XYZ轴三方向角速度的ω_x,ω_y,ω_z，并且对该ω_x,ω_y,ω_z进行时间的一次积分，预测三方向的拐弯角度为θ_x,θ_y,θ_z：θ_x＝∫ω_xdt,θ_y＝∫ω_ydt,θ_z＝∫ω_zdt；

S3.5，若检测到在行走路径上行进时前方有障碍物，则进行提示。

所述步骤S3.2中，对不同路径的磁感应强度时间序列和磁场梯度矩阵时间序列进行匹配具体为：

S3.2.1，计算匹配矩阵M，匹配矩阵M中的元素M_mn的计算方式为

其中，m、n分别代表匹配矩阵M的行数和列数，m、n的数值分别是磁地图磁场的时间序列长度和匹配对象路径磁场的时间序列长度，w_c、w_g分别是磁地图磁场与匹配对象路径磁场之间的磁场分量差异和磁场梯度差异的权重系数；

S3.2.2，得到匹配矩阵M后，利用动态时间规整算法计算累积矩阵A，累积矩阵A中的元素A_nm的计算方式为：A_nm＝DTW[map(1:m),MEASURE(1:n)]，其中：

其中

从匹配矩阵M的每一行中分别选取，从该匹配矩阵M中选择L个元素组成规整路径，累积矩阵A中的元素A_nm是磁地图磁场矩阵map前m行和匹配对象路径磁场矩阵MEASURE前n行之间实现最佳匹配时的最小累计值，根据该最小累计值得到最佳匹配点，该最佳匹配点对应的磁地图中的位置为当前时刻所在位置。

本发明构建的磁地图路径简洁，减少冗余重叠，定位准确，导航精准，通过多种传感器的结合使用，最大程度地识别不同地点的磁场三分量信息和磁梯度信息。

附图说明

附图1为本发明装置的结构示意图；

附图2为本发明方法的流程示意图。

具体实施方式

为了便于本领域技术人员的理解，下面结合附图对本发明作进一步的描述。

如附图1所示，一种基于多传感器融合的室内导航装置，所述装置包括室内磁地图构建模块、测量模块、导航定位模块、中央处理器、数据存储模块、无线通讯模块和反馈交互模块，室内磁地图构建模块、测量模块、导航定位模块、数据存储模块、无线通讯模块和反馈交互模块分别与中央处理器进行数据互联通讯。中央处理器用于采集、分析地磁数据，并实时进行地图匹配、无线网络传输、人机交互通讯等作用。

室内磁地图构建模块包括楼层高度测量模块、室内路径分段模块、磁场三分量测量模块和磁场梯度测量模块。楼层高度测量模块标定每一楼层的高度差，室内路径分段模块在获取室内每一楼层的平面图后自动进行所有可能的行走路径直线段离散分段，磁场三分量测量模块在绘制磁地图时测量分段路径的三分量磁场，磁场梯度测量模块在绘制磁地图时测量分段路径的三分量磁场梯度。所测的三分量磁场数据和三分量磁场梯度数据由中央处理器存储在数据存储模块。构建地图时按照楼层高度测量模块、室内路径分段模块、磁场三分量测量模块和磁场梯度测量模块的顺序由上到下构建。

测量模块包括气压传感器、磁梯度传感器、加速度计传感器、陀螺仪传感器和超声波传感器。测量楼层相对于地面的气压差ΔP，磁梯度传感器用于测量室内所在位置之前某一小段路径上的磁感应强度时间序列和磁场梯度矩阵时间序列，加速度计传感器用于测量人体或者AGV小车在行走路径上行进时的XYZ三方向加速度的时间序列值，陀螺仪传感器用于测量人体或者AGV小车在行走路径上行进时的XYZ三方向角速度的时间序列值，超声波传感器用于测量人体或者AGV小车在行走路径上行进时的X轴方向前方的障碍物距离。各传感器执行各自的功能任务，由中央处理器将所测得的数据进行深度融合，并和数据存储模块中的磁场地图数据进行匹配，完成室内定位导航。在行走路径上行进时是基于负载有本方案中的室内导航装置的情况。上述磁梯度传感器由6个磁传感器组成一个正方体形状得到。上述各传感器通过中央处理器进行数据的通讯交换。

所述导航定位模块内置有气压传感器楼高测量单元、磁场动态规整匹配单元、加速度计短距离补偿单元、陀螺仪旋转角度定位单元和超声波传感器测距单元。各功能模块的程序存储在数据存储模块中，使用时由中央处理器实时调用。

数据存储模块具体包括：SD卡、EEPROM和硬盘，可以存储地图数据和各模块程序。不同的应用场合可以使用不同的数据存储模块，比如应用在盲人导航时，为了便携性可以使用SD卡、EEPROM，如果应用在AGV小车等导航时，为了存储大量数据可以使用EEPROM和硬盘等，使用方式灵活。

所述无线通讯模块包括行走路径云记录单元和路径磁场更新单元。行走路径云记录单元记录每次用户导航时的导航路径和行走习惯，并将这种行走习惯记录在云端，供日后快速调用。路径磁场更新单元记录每次行走导航时的各离散直线段的磁场地图信息，并将最新的地图数据更新存储在数据存储模块。

所述反馈交互模块包括耳机、左手震动器、右手震动器、小键盘、LED和喇叭。各模块可选择性使用。耳机可以为盲人提供导航时的语音信息；左手震动器安装在盲人左手，以震动方式提示盲人左转弯；右手震动器安装在盲人右手，以震动方式提示盲人右转弯；小键盘是数字和字母组合键盘，为盲人提供导航前的导航目的地信息输入；LED和喇叭为盲人或者AGV小车提供安全警示，当超声波传感器检测到盲人或者AGV小车前方遇到突发危险或障碍物太近时LED便闪烁提示，同时喇叭也响起警报声。

通过上述装置的模块化设计，可以在手机、AGV、可穿戴式导航手环等智能硬件上利用现有或者添加新的传感器和相应处理单元实现室内导航功能。

本发明还揭示了一种基于多传感器融合的室内导航方法，如附图2所示，包括以下步骤：

S1,构建当前室内的磁地图。

S2，实时测量室内当前位置的基础信息数据，该基础信息数据包括：楼层当前位置相对于地面的气压差ΔP，当前时刻所在位置与前一时刻所在位置之间的行走路径上的磁感应强度时间序列和磁场梯度矩阵时间序列，人体或者AGV小车在行走路径上行进时的XYZ三方向加速度的时间序列值，人体或者AGV小车在行走路径上行进时的XYZ三方向角速度的时间序列值，人体或者AGV小车在行走路径上行进时的X轴方向前方的障碍物距离。

S3,将基础信息数据进行融合，定位实时位置并进行直线分段导航。

所述步骤S1具体包括：

S1.1，将行走路径上的分叉口为断点，对室内平面图中所有的行走路径进行直线段离散分段处理得到离散直线段分段路径，对每一离散直线段分段路径和分叉口进行唯一地址编号，该分叉口包括行走路径上的拐弯路口、十字形路口和T字形路口。通过唯一地址编号，减小了磁地图绘制时的测量数据，也减小了磁数据存储容量要求，使得各行走路径不易发生冗余和重叠。

S1.2，在每一个离散直线段分段路径上匀速行走，采集该离散直线段分段路径中每一点上的XYZ三方向磁感应强度平均值

和磁场梯度矩阵g＝[gradient_xgradient_y gradient_z]，该磁感应强度平均值和磁场梯度矩阵构成磁场数据，所有离散直线段分段路径的磁场数据构成磁地图数据。采集时，需要以同一手持或AGV等设备搭载室内导航装置，保持X轴向前、Z轴向上的状态，人体或者AGV匀速在分段直线段路径上从起点走至终点，采集后将磁地图数据存储在存储模块内，如EEPROM、硬盘、SD卡等数字化存储介质内。

S1.3，将离散直线段分段路径唯一地址编号和相应的磁地图数据配对存储，从而生成用于室内导航的磁地图。具体为将每一离散直线段分段路径上的磁场数据以行走时的时间为顺序存储为数据矩阵，并将数据矩阵与直线段分段路径唯一地址编号配对存储。

所述步骤S3具体包括：

S3.1，利用公式Δh＝ΔP/(ρg)获得楼层当前位置相对于地面的高度，其中ρ为空气密度，g为重力加速度。

S3.2,以前一时刻所在的已知位置为起点，确定当前时刻的位置为导航目标，确定前一时刻到当前时刻的该段路径为匹配对象，将该段路径中测得的磁感应强度时间序列

和磁场梯度矩阵时间序列G＝[gradient_X gradient_Y gradient_Z]，与磁地图中的从前一已知位置开始2倍于匹配对象行走时间的时间范围内的磁感应强度时间序列

和磁场梯度矩阵时间序列g＝[gradient_x gradienty_y gradient_z]进行匹配，确定当前时刻所在位置；

S3.3，在行走路径中，若某一段路径的磁感应强度和磁场梯度矩阵变化小于设定值，获取在该段路径上行走时的XYZ轴三方向加速度的时间序列值a_x,a_y,a_z，并且对该a_x,a_y,a_z进行时间的双积分，预测该XYZ轴三方向的行走距离s_x,s_y,s_z：s_x＝∫∫a_xdt,s_y＝∫∫a_ydt,s_z＝∫∫a_zdt。该段路径也相应地称为“盲区”，通过该预测处理，补充“盲区”数据信息，使导航更加精确。

S3.4，在行走路径上拐弯时，实时确定拐弯角度的大小，获取当前拐弯处XYZ轴三方向角速度的ω_x,ω_y,ω_z，并且对该ω_x,ω_y,ω_z进行时间的一次积分，预测XYZ轴三方向的拐弯角度为θ_x,θ_y,θ_z：θ_x＝∫ω_xdt,θ_y＝∫ω_ydt,θ_z＝∫ω_zdt。

S3.5，若检测到在行走路径上行进时前方有障碍物，则进行提示，提高安全性。

所述步骤S3.2中，对不同路径的磁感应强度时间序列和磁场梯度矩阵时间序列进行匹配具体为：

S3.2.1，计算匹配矩阵M，匹配矩阵M中的元素M_mn的计算方式为

其中，m、n分别代表匹配矩阵M的行数和列数，m、n的数值分别是磁地图磁场的时间序列长度和匹配对象路径磁场的时间序列长度，w_c、w_g分别是磁地图磁场与匹配对象路径磁场之间的磁场分量差异和磁场梯度差异的权重系数；

S3.2.2，得到匹配矩阵M后，利用动态时间规整算法计算累积矩阵A，累积矩阵A中的元素A_nm的计算方式为：A_nm＝DTW[map(1:m),MEASURE(1:n)]，其中：

其中

从匹配矩阵M的每一行中分别选取，从该匹配矩阵M中选择L个元素组成规整路径，累积矩阵A中的元素A_nm是磁地图磁场矩阵map前m行和匹配对象路径磁场矩阵MEASURE前n行之间实现最佳匹配时的最小累计值，根据该最小累计值得到最佳匹配点，该最佳匹配点对应的磁地图中的位置为当前时刻所在位置。

可以利用磁梯度传感器建立室内直线段分段路径的磁地图，利用气压传感器、磁梯度传感器、加速度计传感器、陀螺仪传感器、超声波传感器等多种传感器的深度融合数据，实现室内直线导航、盲区估计、拐弯、避障等功能，有效避免了单个磁传感器三分量磁场的不充分信息和磁导航“盲区”的失效。

需要说明的是，以上所述并非是对发明技术方案的限定，在不脱离本发明的创造构思的前提下，任何显而易见的替换均在本发明的保护范围之内。

Claims (3)

1.一种基于多传感器融合的室内导航装置的导航方法，所述装置包括室内磁地图构建模块、测量模块、导航定位模块、中央处理器、数据存储模块、无线通讯模块和反馈交互模块，室内磁地图构建模块、测量模块、导航定位模块、数据存储模块、无线通讯模块和反馈交互模块分别与中央处理器进行数据互联通讯，其特征在于，所述方法包括以下步骤：

S1,构建当前室内的磁地图；

S2，实时测量室内当前位置的基础信息数据，该基础信息数据包括：楼层当前位置相对于地面的气压差△P，当前时刻所在位置与前一时刻所在位置之间的行走路径上的磁感应强度时间序列和磁场梯度矩阵时间序列，人体或者AGV小车在行走路径上行进时的XYZ三方向加速度的时间序列值，人体或者AGV小车在行走路径上行进时的XYZ三方向角速度的时间序列值，人体或者AGV小车在行走路径上行进时的X轴方向前方的障碍物距离；

S3,将基础信息数据进行融合，定位实时位置并进行直线分段导航；

所述步骤S3具体包括：

S3.1，利用公式△h＝△P/(ρg)获得楼层当前位置相对于地面的高度，其中ρ为空气密度，g为重力加速度；

S3.2,以前一时刻所在的已知位置为起点，确定当前时刻的位置为导航目标，确定前一时刻到当前时刻的该段路径为匹配对象，将该段路径中测得的磁感应强度时间序列

和磁场梯度矩阵时间序列G＝[gradient_X gradient_Y gradient_Z]，与磁地图中的从前一已知位置开始2倍于匹配对象行走时间的时间范围内的磁感应强度时间序列

和磁场梯度矩阵时间序列g＝[gradient_x gradienty_y gradient_z]进行匹配，确定当前时刻所在位置；

S3.3，在行走路径中，若某一段路径的磁感应强度和磁场梯度矩阵变化小于设定值，获取在该段路径上行走时的XYZ轴三方向加速度的时间序列值a_x,a_y,a_z，并且对该a_x,a_y,a_z进行时间的双积分，预测该三方向的行走距离s_x,s_y,s_z：s_x＝∫∫a_xdt,s_y＝∫∫a_ydt,s_z＝∫∫a_zdt；

S3.4，在行走路径上拐弯时，实时确定拐弯角度的大小，获取当前拐弯处XYZ轴三方向角速度的ω_x,ω_y,ω_z，并且对该ω_x,ω_y,ω_z进行时间的一次积分，预测三方向的拐弯角度为θ_x,θ_y,θ_z：θ_x＝∫ω_xdt,θ_y＝∫ω_ydt,θ_z＝∫ω_zdt；

S3.5，若检测到在行走路径上行进时前方有障碍物，则进行提示。

2.根据权利要求1所述的基于多传感器融合的室内导航装置的导航方法，其特征在于，所述步骤S1具体包括：

S1.1，将行走路径上的分叉口为断点，对室内平面图中所有的行走路径进行直线段离散分段处理得到离散直线段分段路径，对每一离散直线段分段路径和分叉口进行唯一地址编号，该分叉口包括行走路径上的拐弯路口、十字形路口和T字形路口；

S1.2，在每一个离散直线段分段路径上匀速行走，采集该离散直线段分段路径中每一点上的XYZ三方向磁感应强度平均值

和磁场梯度矩阵g＝[gradient_xgradient_y gradient_z]，该磁感应强度平均值和磁场梯度矩阵构成磁场数据，所有离散直线段分段路径的磁场数据构成磁地图数据；

S1.3，将离散直线段分段路径唯一地址编号和相应的磁地图数据配对存储，从而生成用于室内导航的磁地图。

3.根据权利要求2所述的基于多传感器融合的室内导航装置的导航方法，其特征在于，所述步骤S3.2中，对不同路径的磁感应强度时间序列和磁场梯度矩阵时间序列进行匹配具体为：

S3.2.1，计算匹配矩阵M，匹配矩阵M中的元素M_mn的计算方式为

其中，m、n分别代表匹配矩阵M的行数和列数，m、n的数值分别是磁地图磁场的时间序列长度和匹配对象路径磁场的时间序列长度，w_c、w_g分别是磁地图磁场与匹配对象路径磁场之间的磁场分量差异和磁场梯度差异的权重系数；

S3.2.2，得到匹配矩阵M后，利用动态时间规整算法计算累积矩阵A，累积矩阵A中的元素A_nm的计算方式为：A_nm＝DTW[map(1:m),MEASURE(1:n)]，其中：

其中

从匹配矩阵M的每一行中分别选取，从该匹配矩阵M中选择L个元素组成规整路径，累积矩阵A中的元素A_nm是磁地图磁场矩阵map前m行和匹配对象路径磁场矩阵MEASURE前n行之间实现最佳匹配时的最小累计值，根据该最小累计值得到最佳匹配点，该最佳匹配点对应的磁地图中的位置为当前时刻所在位置。

Images