CN102866706A - 一种采用智能手机导航的清扫机器人及其导航清扫方法 - Google Patents

Abstract

一种采用智能手机导航的清扫机器人，包括清扫机器人模块，还设有包括智能手机摄像头、图像处理子模块、摄像头标定子模块、EKF-SLAM子模块、地图维护与路径规划子模块和传感器子模块、传感器数据采集子模块的智能手机模块。其导航清扫方法依次有以下步骤：1）采用标定模板标定智能手机摄像头内参数；2）建立蓝牙通信连接；3）启动EKF-SLAM子模块，进行精确定位，创建三维环境地图；4）建立室内环境障碍物覆盖地图；5）自动规划清扫机器人的清扫路线；6）控制清扫机器人行走轮的下一步运动。本发明避免了直接在清扫机器人上加装导航定位以及传感器等硬件设备，实施简单，硬件改动小，定位准确，清扫覆盖面积大，清扫效率高。

Description

一种采用智能手机导航的清扫机器人及其导航清扫方法

技术领域

本发明涉及导航，特别是涉及一种采用智能手机导航的清扫机器人及其导航清扫方法。

背景技术

清扫机器人在有障碍物的环境中迅速有效地全面覆盖清扫区域，面向目标自主运动，快速完成清扫工作，需要具备自主导航定位功能。中国专利CN201010251088.6公开了《一种清扫机器人自主实现导航和全面清扫覆盖的方法》，但是需要在清扫机器人上额外加装激光发射接收装置，增加成本。此外，CN200710020820.7公开了《一种基于蓝牙技术移动清扫机器人控制装置》，该装置需要额外设计数字信号处理芯片控制清扫机器人运动，不适合清扫机器人的自主导航。

发明内容

本发明所要解决的一个技术问题是弥补上述现有技术的缺陷，提供一种采用智能手机导航的清扫机器人。

本发明所要解决的另一个技术问题是弥补上述现有技术的缺陷，提供一种采用智能手机导航的清扫机器人的导航清扫方法。

本发明的采用智能手机导航的清扫机器人技术问题通过以下技术方案予以解决。

这种采用智能手机导航的清扫机器人，包括清扫机器人模块，所述清扫机器人模块包括第二蓝牙通信模块、机器人控制子模块、避障传感器和行走轮控制模块，所述避障传感器包括用于采集清扫机器人的周围障碍或墙的红外墙检传感器、用于采集清扫机器人的周围地面环境的红外地检传感器、用于采集清扫机器人的周围制约信号的虚拟墙检测传感器，以及用于采集清扫机器人的周围回充座信号的座充检测传感器。

所述第二蓝牙通信模块用于对外发送和接收数据。

所述机器人控制子模块包括嵌入了预定的机器人路径规划程序、机器人状态数据采集程序和机器人运动控制程序的控制芯片，控制清扫机器人按预定的路径进行工作，并根据接收到的外界信号改变清扫机器人的运动状态。

所述行走轮控制模块包括两个驱动轮及一个万向轮，用于清扫机器人在工作区域运动。

这种采用智能手机导航的清扫机器人的特点是：

在清扫机器人机身上方设有智能手机模块，所述智能手机模块包括智能手机摄像头、图像处理子模块、摄像头标定子模块、扩展卡尔曼滤波器-同步定位与地图创建(Extended Kalman Filter-Simultaneous LocalizationAnd Map Building，缩略词为EKF-SLAM)子模块、地图维护与路径规划子模块和第一蓝牙通信模块，以及传感器子模块、传感器数据采集子模块。所述图像处理子模块、摄像头标定子模块、EKF-SLAM子模块和地图维护与路径规划子模块分别通过各自的应用程序编程接口(ApplicationProgramming Interface，缩略词为API)进行通信，以实现在清扫机器人行走过程中，利用智能手机摄像头和清扫机器人内部传感器的信息，通过基于EKF-SLAM子模块，进行导航定位运算，并根据导定位结果和所述清扫机器人模块的传感器子模块采集的数据或提供的信息，进行精确的定位，创建环境地图和建立室内环境障碍物覆盖地图，自动规划清扫机器人的清扫路线，控制清扫机器人行走轮的下一步运动。

所述智能手机摄像头用于从外界获取图像信息。

所述传感器数据采集子模块包括用于将传感器采集的模拟信号转换为数字信号的模数转换(Analog To Digital Converter，缩略词为A/D)模块。

所述图像处理子模块用于对从外界获取的图像进行特征提取并输出相关结果。

所述摄像头标定子模块用于将采集到的图像与三维空间中的物体建立映射关系。

所述EKF-SLAM子模块根据所述智能手机摄像头捕获的图像信息及所述惯性传感器采集的清扫机器人运动参数信息进行定位，创建环境地图，还根据所述避障传感器采集的清扫机器人周围状况信息建立室内障碍物覆盖地图。

所述地图维护与路径规划子模块根据所述EKF-SLAM子模块获得的地图信息规划清扫机器人的清扫路线，并通过第一蓝牙通信模块、第二蓝牙通信模块控制清扫机器人的动作。

所述第二蓝牙通信模块，与所述智能手机模块中的所述第一蓝牙通信模块组成相同，通过蓝牙进行双工通信，并利用蓝牙通信控制清扫机器人的动作。

本发明的采用智能手机导航的清扫机器人技术问题通过以下进一步的技术方案予以解决。

所述传感器子模块包括惯性传感器：陀螺仪和加速度计，所述陀螺仪用于采集清扫机器人运动参数：角速度，所述加速度计用于采集清扫机器人运动参数：线加速度。

本发明的采用智能手机导航的清扫机器人的导航清扫方法技术问题通过以下技术方案予以解决。

这种采用智能手机导航的清扫机器人的导航清扫方法的特点是：

采用智能手机导航的清扫机器人基于预设的标定模板工作于导航清扫模式，所述预设的标定模板是图案为黑白相间的棋盘格的标定模板，依次有以下步骤：

1)采用所述标定模板标定智能手机摄像头内参数；

2)建立蓝牙通信连接；

3)启动EKF-SLAM子模块，进行精确定位，创建三维环境地图；

4)建立室内环境障碍物覆盖地图；

5)自动规划清扫机器人的清扫路线；

6)控制清扫机器人行走轮的下一步运动。

本发明的采用智能手机导航的清扫机器人的导航清扫方法技术问题通过以下进一步的技术方案予以解决。

所述步骤1)标定智能手机摄像头内参数，包括以下分步骤：

1·1)将智能手机摄像头对准所述标定模板；

1·2)启动摄像头标定子模块，在保证所述标定模板全部位于摄像头图像视野中的前提下，以至少两个方向对所述标定模板进行拍照，采集标定板图像；

1·3)摄像头标定子模块将采集到的标定板图像中检测到的点集与实际标定板的三维点集建立对应关系，采用平面标定算法自动计算出智能手机摄像头优化的内参数矩阵(fu，fv，u0，v0，k1，k2，p1，p2)，并保存于智能手机中，所述智能手机摄像头优化的内参数矩阵(fu，fv，u0，v0，k1，k2，p1，p2)其中：

Fu：横焦距；

Fv：纵焦距；

u0：主点横坐标；

v0：主点纵坐标；

k1：镜头径向一次畸变参数；

k2：镜头径向二次畸变参数；

p1：镜头切向一次畸变参数；

p2：镜头切向二次畸变参数；

***运行过程中，同一智能手机不必再次标定。

所述步骤2)建立蓝牙通信连接，包括以下分步骤：

2·1)启动智能手机模块中的所述第一蓝牙通信模块，与清扫机器人机身上的第二蓝牙通信模块进行蓝牙配对后建立蓝牙无线通信连接；

2·2)由清扫机器人识别由智能手机发送的控制清扫机器人运动的指令；

如果命令指令识别为查询指令，嵌入了预定的机器人状态数据采集程序的控制芯片的所述机器人控制子模块就将清扫机器人内部传感器每一时刻的状态进行编码，并加入时间戳，返回数据状态标识符，组成传感器状态向量，再将传感器状态向量通过第一蓝牙通信模块、第二蓝牙通信模块发送到智能手机；

如果指令识别为控制指令，嵌入了预定的机器人运动控制程序的控制芯片的所述机器人控制子模块对接收到的智能手机发送的控制指令进行解码，并转换为清扫机器人的运行控制命令，控制清扫机器人按预定的路径进行工作，并根据接收到的外界信号改变清扫机器人的运动状态，包括前进后退、旋转指定角度；

如果控制成功，则通过第一蓝牙通信模块、第二蓝牙通信模块向智能手机发送控制返回指令，指令中包括是否操作成功标识以及控制返回指令标识；

如果控制未成功，则由智能手机在延迟设定时间后重新发送控制指令，再重复分步骤2·2)，所述延迟设定时间为2秒～4秒。

所述步骤3)启动EKF-SLAM子模块，进行精确定位，创建三维环境地图，包括将智能手机摄像头保持竖直向上，固定于清扫机器人顶端，以及以下分步骤：

3·1)EKF-SLAM子模块初始化

判断是否存在已创建的地图；

如果存在已创建地图，则转入EKF-SLAM算法重定位操作，采用重定位算法获得当前智能手机摄像头在已创建地图中的位置和姿态，包括以下子步骤：

3·1·1)对当前摄像头图像中提取的角点与已创建地图中的地图特征进行全局匹配，获得二维特征与三维点的初步对应关系；

3·1·2)利用基于直接线性变换(Direct Linear Transformation，缩略词为DLT)的透视N点算法(Perspective N Points，缩略词为PNP)，配合随机抽样一致性(Random Sample Consensus，缩略词为RANSAC)算法，根据已标定好的摄像头内参数(fu，fv，u0，v0，k1，k2，p1，p2)，获得最优的智能手机摄像头的位置和姿态；

如果成功获得最优的智能手机摄像头的位置和姿态，则进行分步骤3·3)，一步EKF-SLAM算法迭代；

如果未成功获得最优的智能手机摄像头的位置和姿态，则进行分步骤3·2)；

如果不存在已创建地图，则直接进行分步骤3·2)；

3·2)地图初始化

利用智能手机与清扫机器人建立的蓝牙连接，控制清扫机器人直线运动设定的距离，对运动前后摄像头在两个不同位置获得的两帧图像，进行全局的图像角点的提取和匹配，利用匹配结果以及天花板环境平面假设，计算出两帧图像与摄像头的相对位置和姿态R₀、t₀，其中R₀是一个3×3的旋转矩阵表示初始两帧图像与摄像头之间的三维旋转，t₀是一个3×1的列向量代表初始两帧图像与摄像头之间的三维平移，并利用三角化生成初始三维地图特征，利用捆绑调整(Bundle Adjustment，缩略词为BA)算法进行优化，得到最终初始三维地图的地图特征点和两帧图像中的清扫机器人的位置和姿态；

所述清扫机器人直线运动设定的距离是25cm～35cm；

3·3)读取清扫机器人的传感器子模块中的惯性传感器采集清扫机器人的运动参数作为EKF-SLAM子模块的控制量，进行***状态的一步预测，所述***状态包括机器人位置、姿态，以及所有三维地图特征点的位置；

3·4)对于智能手机摄像头采集到的视频图像，利用分步骤3·3)预测的清扫机器人状态，获得图像特征预测区域，进行角点特征提取和地图特征的匹配；

如果成功进行角点特征提取和地图特征的匹配，则进行分步骤3·5)；

如果未成功进行角点特征提取和地图特征的匹配，即出现数据关联失败，再重复分步骤3·1)进行清扫机器人的重定位操作；

所述地图特征的匹配，是采用基于主方向的灰度直方图最小均方误差(Minimum Mean Squared Error，缩略词为MMSE)算法进行匹配；

3·5)将分步骤3·4)进行角点特征提取和地图特征的匹配的结果结合分步骤1·3)自动计算出的智能手机摄像头优化的内参数矩阵(fu，fv，u0，v0，k1，k2，p1，p2)，获得观测更新矩阵，进行EKF更新，更新分步骤3·3)预测的清扫机器人状态，同时更新地图特征点，获得清扫机器人当前的位置和姿态；

3·6)判断是否存在新建地图特征需要进行特征转换；

如果某一摄像头坐标系下特征点深度的倒数ρ的标准差σ_ρ大于或等于设定的阈值τ，则进入分步骤3·7)；

如果某一摄像头坐标系下特征点深度的倒数ρ的标准差σ_ρ小于设定的阈值τ，需要进行特征转换，将地图特征由反深度表示转换为三维世界坐标表示，所述阈值τ的选择要保证ρ以99％的概率落在τ与σ_ρ确定的等概曲面以内；

转换公式如下：

其中：

$\left[\begin{array}{c}{x}^w\\ y^w\\ z^w\end{array}\right]:$ 摄像头位置的三维向量；

x^W：摄像头位置的三维向量中的x轴坐标参数；

y^W：摄像头位置的三维向量中的y轴坐标参数；

z^W：摄像头位置的三维向量中的z轴坐标参数；

$\left[\begin{array}{c}{r}_x\\ r_y\\ r_z\end{array}\right]:$ 地图特征初始化时摄像头位置的三维向量；

r_x：地图特征初始化时摄像头位置的三维向量中的x轴坐标参数；

r_y：地图特征初始化时摄像头位置的三维向量中的y轴坐标参数；

r_z：地图特征初始化时摄像头位置的三维向量中的z轴坐标参数；

θ：地图特征点的初始深度方向的方向角；

地图特征点的初始深度方向的俯仰角；

ρ：摄像头坐标系下特征点深度的倒数；

3·7)根据PNP算法判断当前可见特征点是否小于设定范围，所述设定范围是6～15个可见点；

如果存在已知三维位置和对应图像位置的6～15个特征点，则智能手机摄像头的位置和姿态有唯一解；

如果当前可见特征点小于设定范围，则新增特征点，进行分步骤3·8)；

如果当前可见特征点大于或等于设定范围，则完成一步EKF-SLAM算法迭代，进行分步骤3·9)：

3·8)新建特征点

对于当前摄像头图像，在无正确匹配地图特征的图像区域，检测图像角点，并进行反深度地图特征初始化；

所述检测图像角点采用快速角点检测(Features from AcceleratedSegment Test，简称FAST)算法进行，角点特征的描述采用半径10像素带有主方向的极坐标灰度均值直方图描述，地图特征反深度初始化参数为6维向量：

其中：

[r_x，r_y，r_z]^T：摄像头当前位置的三维向量；

r_x：摄像头当前位置的三维向量中的x轴坐标参数；

r_y：摄像头当前位置的三维向量中的y轴坐标参数；

r_z：摄像头当前位置的三维向量中的z轴坐标参数；

θ：地图特征点的初始深度方向的方向角；

地图特征点的初始深度方向的俯仰角；

ρ为新建地图特征深度估计值的倒数；

3·9)一步EKF-SLAM算法迭代完成，获得机器人的精确定位结果以及三维环境地图，判断是否终止清扫操作；

如果终止清扫操作，则结束导航操作；

如果不终止清扫操作，则进入步骤4)。

所述步骤4)建立室内环境障碍物覆盖地图，是地图维护及路径规划子模块利用蓝牙通信模块读取当前清扫机器人的传感器信息，将当前清扫机器人定位结果，以及避障传感器采集的清扫机器人的周围状况信息传送至地图维护及路径规划子模块，更新地板平面障碍物覆盖地图。

所述步骤5)自动规划清扫机器人的清扫路线，是利用步骤4)更新的地板平面障碍物覆盖地图，根据清扫机器人行进路线以及覆盖区域，规划清扫机器人下一步行进路线。

所述步骤6)控制清扫机器人行走轮的下一步运动，是通过第一蓝牙通信模块、第二蓝牙通信模块向智能手机发送控制指令，控制清扫机器人向无障碍物且未清扫区域运动，以下重复分步骤3·3)及其后续步骤。

本发明与现有技术相比的有益效果是：

本发明避免了直接在清扫机器人上加装导航定位以及传感器等硬件设备，利用智能手机内部的摄像头、包括加速度计与陀螺仪的惯性传感器进行清扫机器人的导航定位，获得清扫机器人实时的精确位置信息和周围环境的三维地图，以及地板平面障碍物覆盖地图，并通过蓝牙通信控制清扫机器人的每一步动作，具有实施简单，硬件改动小，定位准确，清扫覆盖面积大，清扫效率高的优点。

附图说明

图1是本发明具体实施方式的外形图；

图2是图1智能手机的电路组成方框图；

图3是图1清扫机器人的电路组成方框图；

图4是图1清扫机器人的智能壁障式清扫路线图；

图5是图1清扫机器人的螺旋外扩式清扫路线图；

图6是图1清扫机器人的沿墙精扫式清扫路线图；

图7是图1清扫机器人的折线精扫式清扫路线图；

图8是图1清扫机器人的线障清扫式清扫路线图。

具体实施方式

下面结合具体实施方式并对照附图对本发明进行说明。

一种如图1～8所示的采用智能手机导航的清扫机器人，包括清扫机器人模块2，清扫机器人模块2包括第二蓝牙通信模块11、机器人控制子模块12、避障传感器和行走轮控制模块17，其中避障传感器包括用于采集清扫机器人的周围障碍或墙的红外墙检传感器13、用于采集清扫机器人的周围地面环境的红外地检传感器14、用于采集清扫机器人的周围制约信号的虚拟墙检测传感器15，以及用于采集清扫机器人的周围回充座信号的座充检测传感器16。

第二蓝牙通信模块11用于对外发送和接收数据。

机器人控制子模块12包括嵌入了预定的机器人路径规划程序、机器人状态数据采集程序和机器人运动控制程序的控制芯片，控制清扫机器人按预定的路径进行工作，并根据接收到的外界信号改变清扫机器人的运动状态。

行走轮控制模块17包括两个驱动轮及一个万向轮，用于清扫机器人在工作区域运动。

在清扫机器人机身上方设有智能手机模块1，智能手机模块1包括智能手机摄像头4、图像处理子模块7、摄像头标定子模块3、扩展卡尔曼滤波器-同步定位与地图创建(Extended Kalman Filter-SimultaneousLocalization And Map Building，缩略词为EKF-SLAM)子模块8、地图维护与路径规划子模块9、第一蓝牙通信模块10，以及传感器子模块、传感器数据采集子模块。其中传感器子模块包括惯性传感器：陀螺仪5和加速度计6。图像处理子模块7、摄像头标定子模块3、EKF-SLAM子模块8和地图维护与路径规划子模块9分别通过各自的API进行通信，以实现在清扫机器人行走过程中，利用智能手机摄像头4和清扫机器人内部传感器的信息，通过基于EKF-SLAM子模块8，进行导航定位运算，并根据导定位结果和清扫机器人模块的传感器子模块采集的数据或提供的信息，进行精确的定位，创建环境地图和建立室内环境障碍物覆盖地图，自动规划清扫机器人的清扫路线，控制清扫机器人行走轮的下一步运动。

智能手机摄像头4用于从外界获取图像信息。

陀螺仪5用于采集清扫机器人运动参数：角速度。

加速度计6用于采集清扫机器人运动参数：线加速度。

传感器数据采集子模块包括用于将传感器采集的模拟信号转换为数字信号的A/D模块。

图像处理子模块7用于对从外界获取的图像进行特征提取并输出相关结果。

摄像头标定子模块3用于将采集到的图像与三维空间中的物体建立映射关系。

EKF-SLAM子模块8根据智能手机摄像头4捕获的图像信息及惯性传感器采集的清扫机器人运动参数信息进行定位，创建环境地图，还根据避障传感器采集的清扫机器人周围状况信息建立室内障碍物覆盖地图。

地图维护与路径规划子模块9根据EKF-SLAM子模块8获得的地图信息规划清扫机器人的清扫路线，并通过第一蓝牙通信模块10、第二蓝牙通信模块11控制清扫机器人的动作。

第二蓝牙通信模块10与第一蓝牙通信模块11组成相同，通过蓝牙进行双工通信，并利用蓝牙通信控制清扫机器人的动作。

本具体实施方式的清扫路线包括如图4～图8所示的智能壁障式、螺旋外扩式、沿墙精扫式、折线精扫式和线障清扫式。

导航清扫方法是基于预设的图案为黑白相间的棋盘格的标定模板，依次有以下步骤：

1)采用标定模板标定智能手机摄像头内参数；

包括以下分步骤：

1·1)将智能手机摄像头对准标定模板；

1·2)启动摄像头标定子模块，在保证标定模板全部位于摄像头图像视野中的前提下，以至少两个方向对标定模板进行拍照，采集标定板图像；

1·3)摄像头标定子模块将采集到的标定板图像中检测到的点集与实际标定板的三维点集建立对应关系，采用平面标定算法自动计算出智能手机摄像头优化的内参数矩阵(fu，fv，u0，v0，k1，k2，p1，p2)，并保存于智能手机中，智能手机摄像头优化的内参数矩阵(fu，fv，u0，v0，k1，k2，p1，p2)其中：

Fu：横焦距；

Fv：纵焦距；

u0：主点横坐标；

v0：主点纵坐标；

k1：镜头径向一次畸变参数；

k2：镜头径向二次畸变参数；

p1：镜头切向一次畸变参数；

p2：镜头切向二次畸变参数；

***运行过程中，同一智能手机不必再次标定。

2)建立蓝牙通信连接；

包括以下分步骤：

2·1)启动智能手机模块中的第一蓝牙通信模块，与清扫机器人机身上的第二蓝牙通信模块进行蓝牙配对后建立蓝牙无线通信连接；

2·2)由清扫机器人识别由智能手机发送的控制清扫机器人运动的指令；

如果命令指令识别为查询指令，嵌入了预定的机器人状态数据采集程序的控制芯片的机器人控制子模块就将清扫机器人内部传感器每一时刻的状态进行编码，并加入时间戳，返回数据状态标识符，组成传感器状态向量，再将传感器状态向量通过第一蓝牙通信模块、第二蓝牙通信模块发送到智能手机；

如果指令识别为控制指令，嵌入了预定的机器人运动控制程序的控制芯片的机器人控制子模块对接收到的智能手机发送的控制指令进行解码，并转换为清扫机器人的运行控制命令，控制清扫机器人按预定的路径进行工作，并根据接收到的外界信号改变清扫机器人的运动状态，包括前进后退、旋转指定角度；

如果控制成功，则通过第一蓝牙通信模块、第二蓝牙通信模块向智能手机发送控制返回指令，指令中包括是否操作成功标识以及控制返回指令标识；

如果控制未成功，则由智能手机在延迟设定时间2秒～4秒后重新发送控制指令，再重复分步骤2·2)；

3)启动EKF-SLAM子模块，进行精确定位，创建三维环境地图；

包括将智能手机摄像头保持竖直向上，固定于清扫机器人顶端，以及以下分步骤：

3·1)EKF-SLAM子模块初始化

判断是否存在已创建的地图；

如果存在已创建地图，则转入EKF-SLAM算法重定位操作，采用重定位算法获得当前智能手机摄像头在已创建地图中的位置和姿态，包括以下子步骤：

3·1·1)对当前摄像头图像中提取的角点与已创建地图中的地图特征进行全局匹配，获得二维特征与三维点的初步对应关系；

3·1·2)利用基于DLT的PNP算法配合RANSAC算法，根据已标定好的摄像头内参数(fu，fv，u0，v0，k1，k2，p1，p2)，获得最优的智能手机摄像头的位置和姿态；

如果成功获得最优的智能手机摄像头的位置和姿态，则进行分步骤3·3)，一步EKF-SLAM算法迭代；

如果未成功获得最优的智能手机摄像头的位置和姿态，则进行分步骤3·2)；

如果不存在已创建地图，则直接进行分步骤3·2)；

3·2)地图初始化

利用智能手机与清扫机器人建立的蓝牙连接，控制清扫机器人直线运动设定的距离25cm～35cm，对运动前后摄像头在两个不同位置获得的两帧图像，进行全局的图像角点的提取和匹配，利用匹配结果以及天花板环境平面假设，计算出两帧图像与摄像头的相对位置和姿态R₀、t₀，其中R₀是一个3×3的旋转矩阵表示初始两帧图像与摄像头之间的三维旋转，t₀是一个3×1的列向量代表初始两帧图像与摄像头之间的三维平移，并利用三角化生成初始三维地图特征，利用BA算法进行优化，得到最终初始三维地图的地图特征点和两帧图像中的清扫机器人的位置和姿态；

3·3)读取清扫机器人的传感器子模块中的惯性传感器采集清扫机器人的运动参数作为EKF-SLAM子模块的控制量，进行***状态的一步预测，所述***状态包括机器人位置、姿态，以及所有三维地图特征点的位置；

3·4)对于智能手机摄像头采集到的视频图像，利用分步骤3·3)预测的清扫机器人状态，获得图像特征预测区域，进行角点特征提取和采用基于主方向的灰度直方图MMSE算法进行地图特征的匹配；

如果成功进行角点特征提取和地图特征的匹配，则进行分步骤3·5)；

如果未成功进行角点特征提取和地图特征的匹配，即出现数据关联失败，再重复分步骤3·1)进行清扫机器人的重定位操作；

3·5)将分步骤3·4)进行角点特征提取和地图特征的匹配的结果结合分步骤1·3)自动计算出的智能手机摄像头优化的内参数矩阵(fu，fv，u0，v0，k1，k2，p1，p2)，获得观测更新矩阵，进行EKF更新，更新分步骤3·3)预测的清扫机器人状态，同时更新地图特征点，获得清扫机器人当前的位置和姿态；

3·6)判断是否存在新建地图特征需要进行特征转换；

如果某一摄像头坐标系下特征点深度的倒数ρ的标准差σ_ρ大于或等于设定的阈值τ，则进入分步骤3·7)；

如果某一摄像头坐标系下特征点深度的倒数ρ的标准差σ_ρ小于设定的阈值τ，需要进行特征转换，将地图特征由反深度表示转换为三维世界坐标表示，所述阈值τ的选择要保证ρ以99％的概率落在τ与σ_ρ确定的等概曲面以内，

转换公式如下：

其中：

$\left[\begin{array}{c}{x}^w\\ y^w\\ z^w\end{array}\right]:$ 摄像头位置的三维向量；

x^W：摄像头位置的三维向量中的x轴坐标参数；

y^W：摄像头位置的三维向量中的y轴坐标参数；

z^W：摄像头位置的三维向量中的z轴坐标参数；

$\left[\begin{array}{c}{r}_x\\ r_y\\ r_z\end{array}\right]:$ 地图特征初始化时摄像头位置的三维向量；

r_x：地图特征初始化时摄像头位置的三维向量中的x轴坐标参数；

r_y：地图特征初始化时摄像头位置的三维向量中的y轴坐标参数；

r_z：地图特征初始化时摄像头位置的三维向量中的z轴坐标参数；

θ：地图特征点的初始深度方向的方向角；

地图特征点的初始深度方向的俯仰角；

ρ：摄像头坐标系下特征点深度的倒数；

3·7)根据PNP算法判断当前可见特征点是否小于设定范围6～15个可见点；

如果存在已知三维位置和对应图像位置的6～15个特征点，则智能手机摄像头的位置和姿态有唯一解；

如果当前可见特征点小于设定范围，则新增特征点，进行分步骤3·8)；

如果当前可见特征点大于或等于设定范围，则完成一步EKF-SLAM算法迭代，进行分步骤3·9)：

3·8)新建特征点

对于当前摄像头图像，在无正确匹配地图特征的图像区域，采用快速角点检测(Features from Accelerated Segment Test，简称FAST)算法检测图像角点，并进行反深度地图特征初始化；

角点特征的描述采用半径10像素带有主方向的极坐标灰度均值直方图描述，地图特征反深度初始化参数为6维向量：

其中：

[r_x，r_y，r_z]^T：摄像头当前位置的三维向量；

r_x：摄像头当前位置的三维向量中的x轴坐标参数；

r_y：摄像头当前位置的三维向量中的y轴坐标参数；

r_z：摄像头当前位置的三维向量中的z轴坐标参数；

θ：地图特征点的初始深度方向的方向角；

地图特征点的初始深度方向的俯仰角；

ρ为新建地图特征深度估计值的倒数；

3·9)一步EKF-SLAM算法迭代完成，获得机器人的精确定位结果以及三维环境地图，判断是否终止清扫操作；

如果终止清扫操作，则结束导航操作；

如果不终止清扫操作，则进入步骤4)；

4)建立室内环境障碍物覆盖地图；

地图维护及路径规划子模块利用蓝牙通信模块读取当前清扫机器人的传感器信息，将当前清扫机器人定位结果，以及避障传感器采集的清扫机器人的周围状况信息传送至地图维护及路径规划子模块，更新地板平面障碍物覆盖地图；

5)自动规划清扫机器人的清扫路线；

利用步骤4)更新的地板平面障碍物覆盖地图，根据清扫机器人行进路线以及覆盖区域，规划清扫机器人下一步行进路线；

6)控制清扫机器人行走轮的下一步运动。

通过第一蓝牙通信模块、第二蓝牙通信模块向智能手机发送控制指令，控制清扫机器人向无障碍物且未清扫区域运动，以下重复分步骤3·3)及其后续步骤。

以上内容是结合具体的优选实施方式对本发明所作的进一步详细说明，不能认定本发明的具体实施只局限于这些说明。对于本发明所属技术领域的普通技术人员来说，在不脱离本发明构思的前提下做出如果干等同替代或明显变型，而且性能或用途相同，都应当视为属于本发明由所提交的权利要求书确定的专利保护范围。

Claims (9)

1.一种采用智能手机导航的清扫机器人，包括清扫机器人模块，所述清扫机器人模块包括第二蓝牙通信模块、机器人控制子模块、避障传感器和行走轮控制模块，所述避障传感器包括用于采集清扫机器人的周围障碍或墙的红外墙检传感器、用于采集清扫机器人的周围地面环境的红外地检传感器、用于采集清扫机器人的周围制约信号的虚拟墙检测传感器，以及用于采集清扫机器人的周围回充座信号的座充检测传感器，所述第二蓝牙通信模块用于对外发送和接收数据，所述机器人控制子模块包括嵌入了预定的机器人路径规划程序、机器人状态数据采集程序和机器人运动控制程序的控制芯片，控制清扫机器人按预定的路径进行工作，并根据接收到的外界信号改变清扫机器人的运动状态，所述行走轮控制模块包括两个驱动轮及一个万向轮，用于清扫机器人在工作区域运动，其特征在于：

在清扫机器人机身上方设有智能手机模块，所述智能手机模块包括智能手机摄像头、图像处理子模块、摄像头标定子模块、扩展卡尔曼滤波器-同步定位与地图创建EKF-SLAM子模块、地图维护与路径规划子模块和第一蓝牙通信模块，以及传感器子模块、传感器数据采集子模块，所述图像处理子模块、摄像头标定子模块、EKF-SLAM子模块和地图维护与路径规划子模块分别通过各自的应用程序编程接口API进行通信，以实现在清扫机器人行走过程中，利用智能手机摄像头和清扫机器人内部传感器的信息，通过基于EKF-SLAM子模块，进行导航定位运算，并根据导定位结果和所述清扫机器人模块的传感器子模块采集的数据或提供的信息，进行精确的定位，创建环境地图和建立室内环境障碍物覆盖地图，自动规划清扫机器人的清扫路线，控制清扫机器人行走轮的下一步运动；

所述智能手机摄像头用于从外界获取图像信息，所述传感器数据采集子模块包括用于将传感器采集的模拟信号转换为数字信号的模数转换A/D模块，所述图像处理子模块用于对从外界获取的图像进行特征提取并输出相关结果，所述摄像头标定子模块用于将采集到的图像与三维空间中的物体建立映射关系，所述EKF-SLAM子模块根据所述智能手机摄像头捕获的图像信息及所述惯性传感器采集的清扫机器人运动参数信息进行定位，创建环境地图，还根据所述避障传感器采集的清扫机器人周围状况信息建立室内障碍物覆盖地图，所述地图维护与路径规划子模块根据所述EKF-SLAM子模块获得的地图信息规划清扫机器人的清扫路线，并通过第一蓝牙通信模块、第二蓝牙通信模块控制清扫机器人的动作，所述第二蓝牙通信模块，与所述智能手机模块中的所述第一蓝牙通信模块组成相同，通过蓝牙进行双工通信，并利用蓝牙通信控制清扫机器人的动作。

2.如权利要求1所述的采用智能手机导航的清扫机器人，其特征在于：

所述传感器子模块包括惯性传感器：陀螺仪和加速度计，所述陀螺仪用于采集清扫机器人运动参数：角速度，所述加速度计用于采集清扫机器人运动参数：线加速度。

3.一种采用智能手机导航的清扫机器人的导航清扫方法，其特征在于：

采用智能手机导航的清扫机器人基于预设的标定模板工作于导航清扫模式，所述预设的标定模板是图案为黑白相间的棋盘格的标定模板，依次有以下步骤：

1)采用所述标定模板标定智能手机摄像头内参数；

2)建立蓝牙通信连接；

3)启动EKF-SLAM子模块，进行精确定位，创建三维环境地图；

4)建立室内环境障碍物覆盖地图；

5)自动规划清扫机器人的清扫路线；

6)控制清扫机器人行走轮的下一步运动。

4.如权利要求3所述的采用智能手机导航的清扫机器人的导航清扫方法，其特征在于：

所述步骤1)标定智能手机摄像头内参数，包括以下分步骤：

1·1)将智能手机摄像头对准所述标定模板；

1·2)启动摄像头标定子模块，在保证所述标定模板全部位于摄像头图像视野中的前提下，以至少两个方向对所述标定模板进行拍照，采集标定板图像；

1·3)摄像头标定子模块将采集到的标定板图像中检测到的点集与实际标定板的三维点集建立对应关系，采用平面标定算法自动计算出智能手机摄像头优化的内参数矩阵(fu，fv，u0，v0，k1，k2，p1，p2)，并保存于智能手机中，所述智能手机摄像头优化的内参数矩阵(fu，fv，u0，v0，k1，k2，p1，p2)其中：

Fu：横焦距；

Fv：纵焦距；

u0：主点横坐标；

v0：主点纵坐标；

k1：镜头径向一次畸变参数；

k2：镜头径向二次畸变参数；

p1：镜头切向一次畸变参数；

p2：镜头切向二次畸变参数；

***运行过程中，同一智能手机不必再次标定。

5.如权利要求4所述的采用智能手机导航的清扫机器人的导航清扫方法，其特征在于：

所述步骤2)建立蓝牙通信连接，包括以下分步骤：

2·1)启动智能手机模块中的所述第一蓝牙通信模块，与清扫机器人机身上的第二蓝牙通信模块进行蓝牙配对后建立蓝牙无线通信连接；

2·2)由清扫机器人识别由智能手机发送的控制清扫机器人运动的指令，

如果命令指令识别为查询指令，嵌入了预定的机器人状态数据采集程序的控制芯片的所述机器人控制子模块就将清扫机器人内部传感器每一时刻的状态进行编码，并加入时间戳，返回数据状态标识符，组成传感器状态向量，再将传感器状态向量通过第一蓝牙通信模块、第二蓝牙通信模块发送到智能手机；

如果指令识别为控制指令，嵌入了预定的机器人运动控制程序的控制芯片的所述机器人控制子模块对接收到的智能手机发送的控制指令进行解码，并转换为清扫机器人的运行控制命令，控制清扫机器人按预定的路径进行工作，并根据接收到的外界信号改变清扫机器人的运动状态，包括前进后退、旋转指定角度；

如果控制成功，则通过第一蓝牙通信模块、第二蓝牙通信模块向智能手机发送控制返回指令，指令中包括是否操作成功标识以及控制返回指令标以；

如果控制未成功，则由智能手机在延迟设定时间后重新发送控制指令，再重复分步骤2·2)，所述延迟设定时间为2秒～4秒。

6.如权利要求5所述的采用智能手机导航的清扫机器人的导航清扫方法，其特征在于：

所述步骤3)启动EKF-SLAM子模块，进行精确定位，创建三维环境地图，包括将智能手机摄像头保持竖直向上，固定于清扫机器人顶端，以及以下分步骤：

3·1)EKF-SLAM子模块初始化

判断是否存在已创建的地图；

如果存在已创建地图，则转入EKF-SLAM算法重定位操作，采用重定位算法获得当前智能手机摄像头在已创建地图中的位置和姿态，包括以下子步骤：

3·1·1)对当前摄像头图像中提取的角点与已创建地图中的地图特征进行全局匹配，获得二维特征与三维点的初步对应关系；

3·1·2)利用基于直接线性变换DLT的透视N点算法PNP，配合随机抽样一致性RANSAC算法，根据已标定好的摄像头内参数(fu，fv，u0，v0，k1，k2，p1，p2)，获得最优的智能手机摄像头的位置和姿态；

如果成功获得最优的智能手机摄像头的位置和姿态，则进行分步骤3·3)，一步EKF-SLAM算法迭代；

如果未成功获得最优的智能手机摄像头的位置和姿态，则进行分步骤3·2)；

如果不存在已创建地图，则直接进行分步骤3·2)；

3·2)地图初始化

利用智能手机与清扫机器人建立的蓝牙连接，控制清扫机器人直线运动设定的距离，对运动前后摄像头在两个不同位置获得的两帧图像，进行全局的图像角点的提取和匹配，利用匹配结果以及天花板环境平面假设，计算出两帧图像与摄像头的相对位置和姿态R₀、t₀，其中R₀是一个3×3的旋转矩阵表示初始两帧图像与摄像头之间的三维旋转，t₀是一个3×1的列向量代表初始两帧图像与摄像头之间的三维平移，并利用三角化生成初始三维地图特征，利用捆绑调整BA算法进行优化，得到最终初始三维地图的地图特征点和两帧图像中的清扫机器人的位置和姿态；

所述清扫机器人直线运动设定的距离是25cm～35cm；

3·3)读取清扫机器人的传感器子模块中的惯性传感器采集清扫机器人的运动参数作为EKF-SLAM子模块的控制量，进行***状态的一步预测，所述***状态包括机器人位置、姿态，以及所有三维地图特征点的位置；

3·4)对于智能手机摄像头采集到的视频图像，利用分步骤3·3)预测的清扫机器人状态，获得图像特征预测区域，进行角点特征提取和地图特征的匹配；

如果成功进行角点特征提取和地图特征的匹配，则进行分步骤3·5)；

如果未成功进行角点特征提取和地图特征的匹配，即出现数据关联失败，再重复分步骤3·1)进行清扫机器人的重定位操作；

所述地图特征的匹配，是采用基于主方向的灰度直方图最小均方误差MMSE算法进行匹配；

3·5)将分步骤3·4)进行角点特征提取和地图特征的匹配的结果结合分步骤1·3)自动计算出的智能手机摄像头优化的内参数矩阵(fu，fv，u0，v0，k1，k2，p1，p2)，获得观测更新矩阵，进行EKF更新，更新分步骤3·3)预测的清扫机器人状态，同时更新地图特征点，获得清扫机器人当前的位置和姿态；

3·6)判断是否存在新建地图特征需要进行特征转换；

如果某一摄像头坐标系下特征点深度的倒数ρ的标准差σ_ρ大于或等于设定的阈值τ，则进入分步骤3·7)；

如果某一摄像头坐标系下特征点深度的倒数ρ的标准差σ_ρ小于设定的阈值τ，需要进行特征转换，将地图特征由反深度表示转换为三维世界坐标表示，所述阈值τ的选择要保证ρ以99％的概率落在τ与σ_ρ确定的等概曲面以内，

转换公式如下：

其中：

$\left[\begin{array}{c}{x}^w\\ y^w\\ z^w\end{array}\right]:$ 摄像头位置的三维向量；

x^W：摄像头位置的三维向量中的x轴坐标参数；

y^W：摄像头位置的三维向量中的y轴坐标参数；

z^W：摄像头位置的三维向量中的z轴坐标参数；

$\left[\begin{array}{c}{r}_x\\ r_y\\ r_z\end{array}\right]:$ 地图特征初始化时摄像头位置的三维向量；

r_x：地图特征初始化时摄像头位置的三维向量中的x轴坐标参数；

r_y：地图特征初始化时摄像头位置的三维向量中的y轴坐标参数；

r_z：地图特征初始化时摄像头位置的三维向量中的z轴坐标参数；

θ：地图特征点的初始深度方向的方向角；

地图特征点的初始深度方向的俯仰角；

ρ：摄像头坐标系下特征点深度的倒数；

3·7)根据PNP算法判断当前可见特征点是否小于设定范围，所述设定范围是6～15个可见点；

如果存在已知三维位置和对应图像位置的6～15个特征点，则智能手机摄像头的位置和姿态有唯一解；

如果当前可见特征点小于设定范围，则新增特征点，进行分步骤3·8)；

如果当前可见特征点大于或等于设定范围，则完成一步EKF-SLAM算法迭代，进行分步骤3·9)：

3·8)新建特征点

对于当前摄像头图像，在无正确匹配地图特征的图像区域，检测图像角点，并进行反深度地图特征初始化；

所述检测图像角点采用快速角点检测FAST算法进行，角点特征的描述采用半径10像素带有主方向的极坐标灰度均值直方图描述，地图特征反深度初始化参数为6维向量：

其中：

[r_x，r_y，r_z]^T：摄像头当前位置的三维向量；

r_x：摄像头当前位置的三维向量中的x轴坐标参数；

r_y：摄像头当前位置的三维向量中的y轴坐标参数；

r_z：摄像头当前位置的三维向量中的z轴坐标参数；

θ：地图特征点的初始深度方向的方向角；

地图特征点的初始深度方向的俯仰角；

ρ为新建地图特征深度估计值的倒数；

3·9)一步EKF-SLAM算法迭代完成，获得机器人的精确定位结果以及三维环境地图，判断是否终止清扫操作；

如果终止清扫操作，则结束导航操作；

如果不终止清扫操作，则进入步骤4)。

7.如权利要求6所述的采用智能手机导航的清扫机器人的导航清扫方法，其特征在于：

所述步骤4)建立室内环境障碍物覆盖地图，是地图维护及路径规划子模块利用蓝牙通信模块读取当前清扫机器人的传感器信息，将当前清扫机器人定位结果，以及避障传感器采集的清扫机器人的周围状况信息传送至地图维护及路径规划子模块，更新地板平面障碍物覆盖地图。

8.如权利要求7所述的采用智能手机导航的清扫机器人的导航清扫方法，其特征在于：

所述步骤5)自动规划清扫机器人的清扫路线，是利用步骤4)更新的地板平面障碍物覆盖地图，根据清扫机器人行进路线以及覆盖区域，规划清扫机器人下一步行进路线。

9.如权利要求8所述的采用智能手机导航的清扫机器人的导航清扫方法，其特征在于：

所述步骤6)控制清扫机器人行走轮的下一步运动，是通过第一蓝牙通信模块、第二蓝牙通信模块向智能手机发送控制指令，控制清扫机器人向无障碍物且未清扫区域运动，以下重复分步骤3·3)及其后续步骤。

Images