基于蓝牙与SLAM的室内定位导航装置及其方法制造方法及图纸

本发明专利技术公开了一种室内定位导航装置，包括蓝牙模块，以获取室内定位导航装置的Mesh网络信息，获取Mesh网络中多个蓝牙节点的地址、属性、RSSI、IQ数据、角度、到达时间；视觉模块，用于采集机器人周围的图像。里程计模块，用于实时估计机器人的位置；数据存储与处理模块，用于接收蓝牙模块、视觉模块以及里程计模块所采集以及计算得到的数据信息；位置融合与估算模块，用于对数据存储与处理模块中的数据进行融合；地图构建模块，用于根据蓝牙节点、特征点的空间位置信息创建的三维环境地图以及路径规划与运动控制模块，用于驱动机器人并根据创建的三维环境地图进行路径规划与导航。本发明专利技术的定位导航装置可有效提高定位导航精度。

【技术实现步骤摘要】
基于蓝牙与SLAM的室内定位导航装置及其方法
本专利技术涉及机器人控制领域，尤其涉及一种基于蓝牙与SLAM的室内定位导航装置及其方法。
技术介绍
随着移动互联网技术、智能移动终端技术和物联网技术的发展,人们对室内定位服务的需求越来越多元化，使得室内定位导航技术成为智能定位服务领域的研究热点。SLAM(simultaneouslocalizationandmapping，同步定位与建图)技术是机器人领域比较经典的问题，通常SLAM问题可以描述为：机器人在未知环境中从一个未知位置开始移动，在移动过程中根据位置估计和地图自身定位，同时在自身定位的基础上建造增量式地图，实现机器人的自主定位和导航。目前室内定位导航技术是通过各种物理传感器来实现定位导航的，如摄像头、IMU等，在室内复杂的环境中易受环境因素的影响或传感器自身精度低而导致定位精度不高的问题。比如，每个传感器受影响的因素都不一样，摄像头是光照，黑暗下摄像头获取不到特征点，IMU是外界因素，如人为拖拽、地面打滑等，长时间工作会存在累计误差。为了解决室内定位服务的定位精度问题，将多个定位传感器进行数据融合，抗干扰能力强，实现更高精度的定位。本专利技术提供了一种基于蓝牙与SLAM室内定位导航装置及其方法，不仅可以有效地减少因环境因素造成的影响来提高定位精度，而且可以提供可靠性更高、更精确的三维环境地图，使其扩展应用更广泛。比如：蓝牙是不受光照影响和打滑影响，但会因墙、桌子等障碍物遮挡，导致定位精度不高，所以涉及的蓝牙、摄像头以及里程计都会存在自身的一些缺陷，需要多个定位传感器的优势相结合，取长补短。
技术实现思路
本专利技术所要解决的技术问题在于提供一种基于蓝牙与SLAM室内定位导航装置及其方法，通过蓝牙节点信息、视觉模块采集的特征点以及特征信息，里程计模块估算的实时位置信息构建场景地图，从而实现精确的定位和导航功能。本专利技术公开了一种室内定位导航装置，其中所述室内定位导航装置内置于可移动的智能平台或机器人中，包括：蓝牙模块，用于获取所述室内定位导航装置的Mesh网络信息，获取Mesh网络中多个蓝牙节点的地址、属性、RSSI(ReceivedSignalStrengthIndication，接收的信号强度指示)、IQ(In-phaseQuadrature,同向正交)数据、角度、到达时间。视觉模块，用于采集机器人周围的图像，并对所采集的图像灰度化处理、图像矫正以及对图像进行特征点提取和特征线段提取。里程计模块，用于通过相对定位计算出机器人每一时刻位置相对于上一时刻位置的变化，实时估计机器人的位置。数据存储与处理模块，用于接收蓝牙模块、视觉模块以及里程计模块所采集以及计算得到的数据信息。位置融合与估算模块，用于对数据存储与处理模块中的数据进行融合，获取室内定位导航装置的实时位置。地图构建模块，用于存储位置融合与估算模块估计的室内定位导航装置的实时位置，以及根据数据存储与处理模块中的蓝牙节点、特征点的空间位置信息创建的三维环境地图，以及路径规划与运动控制模块，用于驱动机器人，并根据地图构建模块创建的三维环境地图进行路径规划与导航。本专利技术开公开了一种室内定位导航方法，包括：标定视觉模块内的摄像头；获取蓝牙模块、视觉模块以及里程计模块采集的数据；对采集的数据进行卡尔曼滤波；根据计算得到室内定位导航装置的实时位置、蓝牙节点、特征点的空间位置创建三维环境地图，以及根据创建的三维环境地图进行定位和导航。有利地，本专利技术的室内定位导航装置及方法能够通过Mesh网络中蓝牙节点信息、图像的特征点、特征线段信息以及里程计估算的实时位置信息构建场景地图，用于机器人的精确定位及路径规划。附图说明图1是本专利技术实施例提供的基于蓝牙与SLAM室内定位导航装置的模块图。图2是本专利技术实施例提供的基于蓝牙与SLAM室内定位导航装置中视觉模块的工作流程图。图3是本专利技术实施例提供的基于蓝牙与SLAM室内定位导航装置中位置融合与估算模块卡尔曼滤波的流程图。图4是本专利技术实施例提供的基于蓝牙与SLAM室内定位导航装置中蓝牙模块的定位模型图。图5是本专利技术实施例提供的基于蓝牙与SLAM室内定位导航装置中里程计模块的运动模型图。图6是本专利技术实施例提供的基于蓝牙与SLAM室内定位导航装置的工作流程图。具体实施方式为了使本专利技术所要解决的技术问题、技术方案及有益效果更加清楚明白，以下结合附图及实施例，对本专利技术进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本专利技术，并不用于限定本专利技术。图1是本专利技术实施例提供的基于蓝牙与SLAM室内定位导航装置100的模块图。如图1所示，基于蓝牙与SLAM室内定位导航装置100包括：蓝牙模块111、视觉模块112、里程计模块113、数据存储与处理模块114、位置融合与估算模块115、地图构建模块116以及路径规划与运动控制117。该室内定位导航装置100可内置于扫地机器人、室内机器人中，用于实现机器人的地图构建以及精准定位和导航。如图1所示，蓝牙模块111为集成了蓝牙功能的芯片基本电路集合，可用于无线网络通讯，可实现数据传输、音频传输功能。蓝牙模块111位于室内定位导航装置100中，具有智能组网和定位功能，具体包括有实现蓝牙网络中蓝牙设备的组网以及定位。蓝牙模块111获取室内定位导航装置100所处的Mesh网络，蓝牙网络中蓝牙节点的地址、属性、RSSI(ReceivedSignalStrengthIndication，接收的信号强度指示)、IQ(In-phaseQuadrature,同向正交)数据、角度(包括方向角和仰角)、到达时间等信息。其中RSSI的单位为dbm，一般为负值，可以反映蓝牙网络中两个蓝牙节点之间距离的远近。蓝牙节点的地址信息是蓝牙节点的一个标识符，用字节表示；蓝牙节点的属性有，友好节点、低功耗节点、中继节点、标准节点以及代理节点这五种类型。进一步地，蓝牙模块111通过定位功能获取周围蓝牙节点到机器人(包含有室内定位导航装置100)的TOA(TimeOfArrive，到达时间)、RSSI、IQ数据和AOA(AngleOfArrive，到达角度)。蓝牙模块111与RSSI测距模型和相位差测距模型构建基于TOA、RSSI、相位以及AOA定位模型方程，如下：其中，(x,y,z)代表了室内定位导航装置100的空间位置，蓝牙节点Bi的空间位置为(Xbi,Ybi,Zbi)，θi是蓝牙节点Bi与室内定位导航装置100的蓝牙天线阵列平面之间的方位角，是蓝牙节点Bi与室内定位导航装置100的蓝牙天线阵列平面之间的仰角，D1i是由RSSI测距模型获取的室内定位导航装置100与蓝牙设备Bi之间的距离，D2i是由TOA测距模型获取的室内定位导航装置100与蓝牙设备Bi之间的距离，D3i是由相位差测距模型获取的室内定位导航装置100与蓝牙设备Bi之间的距离。通过最小二乘方法获取上述定位模型的最佳解，可以计算出蓝牙节点Bi相对于室内定位导航装置100的空间位置，即(x,本文档来自技高网...

【技术保护点】
1.一种基于蓝牙与SLAM的室内定位导航装置，其中所述室内定位导航装置内置于可移动的智能平台或机器人中，包括：/n蓝牙模块，用于获取所述室内定位导航装置的Mesh网络信息，获取Mesh网络中多个蓝牙节点的地址、属性、RSSI、IQ数据、角度、到达时间；/n视觉模块，用于采集机器人周围的图像，并对所采集的图像灰度化处理、图像矫正以及对图像进行特征点提取和特征线段提取；/n里程计模块，用于通过相对定位计算出机器人每一时刻位置相对于上一时刻位置的变化，实时估计机器人的位置；/n数据存储与处理模块，用于接收蓝牙模块、视觉模块以及里程计模块所采集以及计算得到的数据信息；/n位置融合与估算模块，用于对数据存储与处理模块中的数据进行融合，获取室内定位导航装置的实时位置；/n地图构建模块，用于存储位置融合与估算模块估计的室内定位导航装置的实时位置，以及根据数据存储与处理模块中的蓝牙节点、特征点的空间位置信息创建的三维环境地图，以及/n路径规划与运动控制模块，用于驱动机器人，并根据地图构建模块创建的三维环境地图进行路径规划与导航。/n

【技术特征摘要】
1.一种基于蓝牙与SLAM的室内定位导航装置，其中所述室内定位导航装置内置于可移动的智能平台或机器人中，包括：
蓝牙模块，用于获取所述室内定位导航装置的Mesh网络信息，获取Mesh网络中多个蓝牙节点的地址、属性、RSSI、IQ数据、角度、到达时间；
视觉模块，用于采集机器人周围的图像，并对所采集的图像灰度化处理、图像矫正以及对图像进行特征点提取和特征线段提取；
里程计模块，用于通过相对定位计算出机器人每一时刻位置相对于上一时刻位置的变化，实时估计机器人的位置；
数据存储与处理模块，用于接收蓝牙模块、视觉模块以及里程计模块所采集以及计算得到的数据信息；
位置融合与估算模块，用于对数据存储与处理模块中的数据进行融合，获取室内定位导航装置的实时位置；
地图构建模块，用于存储位置融合与估算模块估计的室内定位导航装置的实时位置，以及根据数据存储与处理模块中的蓝牙节点、特征点的空间位置信息创建的三维环境地图，以及
路径规划与运动控制模块，用于驱动机器人，并根据地图构建模块创建的三维环境地图进行路径规划与导航。


2.如权利要求1所述的室内定位导航装置，其特征在于，所述蓝牙模块与RSSI测距模型和相位差测距模型构建基于TOA、RSSI、相位以及AOA的定位模型方程，并通过如下方程计算室内定位导航装置的实时位置：



其中(x,y,z)代表了室内定位导航装置的空间位置，蓝牙节点Bi的空间位置为(Xbi,Ybi,Zbi)，θi是蓝牙节点Bi与室内定位导航装置的蓝牙天线阵列平面之间的方位角，是蓝牙节点Bi与室内定位导航装置的蓝牙天线阵列平面之间的仰角，D1i是由RSSI测距模型获取的室内定位导航装置与蓝牙设备Bi之间的距离，D2i是由TOA测距模型获取的室内定位导航装置与蓝牙设备Bi之间的距离，D3i是由相位差测距模型获取的室内定位导航装置与蓝牙设备Bi之间的距离。


3.如权利要求1所述的室内定位导航装置，其特征在于，所述视觉模块中的摄像头采用12*9的10mm*10mm的黑白棋盘格标定板标定。


4.如权利要求3所述的室内定位导航装置，其特征在于，所述视觉模块中的摄像头包含摄像头参数和畸变参数信息。


5.如权利要求1所述的室内定位导航装置，其特征在于，所述视觉模块还包括对图像畸变进行校正。


6.如权利要求1所述的室内定位导航装置，其特征在于，所述位置融合与估算模块对数据进行融合的算法有最小二乘方法、LM算法、BA算法、智能优化算法、卡尔曼滤波算法，其中卡尔曼滤波时还包括对各模块初始化、建立运动方程、观测方程、更新协方差矩阵、更新增益矩阵以及更新状态向量。


7.如权利要求6所述的室内定位导航装置，其特征在于，所述位置融合与估算模块通过下述公式估算室内定位导航装置在k+1时刻的的系统状态X(k+1|k)，
X(k+1|k)＝X(k)+Uk+1+Qk+1
其中，X(k)为k时刻的系统状态，Qk+1为k+1时刻的运动方程噪声，Uk+1为k+1时刻的姿态变化量。


8.如权利要求7所述的室内定位导航装置，其特征在于，所述位置融合与估算模块根据k+1时刻的的系统状态X(k+1|k)估算k+1时刻的系统观测量，公式如下：即其中，



其中，坐标(Xbi,Ybi,Zbi)为蓝牙网络中的蓝牙节点Bi的坐标，其相对于室内定位导航装置在k+1时刻的系统状态量X(k+1|k)的方位角θi和仰角Fi表示其中的一个特征点，该特征点的空间三维坐标为(Xfi,Yfi,Zfi)，f是摄像头的焦距，Wk+1为k+1时刻的观测方程噪声，H是观测方程对状态量的雅克比矩阵。


9.如权利要求6所述的室内定位导航装置，其特征在于，所述位置融合与估算模块更新协方差方程，计算公式如下：
P(k+1)＝(P(k+1|k)-1+HT*R-1*H)-1
其中P(k+1)为k+1时刻的协方差矩阵，R为观测噪声协方差矩阵。


10.如权利要求6所述的室内定位导航装置，其特征在于，所述位置融合与估算模块更新增益矩阵，计算公式如下：
K＝P(k+1)*HT*R-1
其中，K是增益矩阵。

<...

【专利技术属性】
技术研发人员：杜珣弤，雷鸣，
申请(专利权)人：三个机器人公司，
类型：发明
国别省市：美国;US