专利名称：一种采用智能手机导航的清扫机器人及其导航清扫方法
技术领域：
本发明涉及导航，特别是涉及一种采用智能手机导航的清扫机器人及其导航清扫方法。
背景技术：
清扫机器人在有障碍物的环境中迅速有效地全面覆盖清扫区域，面向目标自主运动，快速完成清扫工作，需要具备自主导航定位功能。中国专利CN201010251088. 6公开了《一种清扫机器人自主实现导航和全面清扫覆盖的方法》，但是需要在清扫机器人上额外加装激光发射接收装置，增加成本。此外，CN200710020820. 7公开了《一种基于蓝牙技术移动清扫机器人控制装置》，该装置需要额外设计数字信号处理芯片控制清扫机器人运动，不适合清扫机器人的自主导航。
发明内容
本发明所要解决的ー个技术问题是弥补上述现有技术的缺陷，提供一种采用智能手机导航的清扫机器人。本发明所要解决的另ー个技术问题是弥补上述现有技术的缺陷，提供ー种采用智能手机导航的清扫机器人的导航清扫方法。本发明的采用智能手机导航的清扫机器人技术问题通过以下技术方案予以解決。这种采用智能手机导航的清扫机器人，包括清扫机器人模块，所述清扫机器人模块包括第二蓝牙通信模块、机器人控制子模块、避障传感器和行走轮控制模块，所述避障传感器包括用于采集清扫机器人的周围障碍或墙的红外墙检传感器、用于采集清扫机器人的周围地面环境的红外地检传感器、用于采集清扫机器人的周围制约信号的虚拟墙检测传感器，以及用于采集清扫机器人的周围回充座信号的座充检测传感器。所述第二蓝牙通信模块用于对外发送和接收数据。所述机器人控制子模块包括嵌入了预定的机器人路径规划程序、机器人状态数据采集程序和机器人运动控制程序的控制芯片，控制清扫机器人按预定的路径进行工作，并根据接收到的外界信号改变清扫机器人的运动状态。所述行走轮控制模块包括两个驱动轮及ー个万向轮，用于清扫机器人在工作区域运动。这种采用智能手机导航的清扫机器人的特点是在清扫机器人机身上方设有智能手机模块，所述智能手机模块包括智能手机摄像头、图像处理子模块、摄像头标定子模块、扩展卡尔曼滤波器-同步定位与地图创建(Extended Kalman Filter-Simultaneous Localization And Map Building,缩略I司为EKF-SLAM)子模块、地图维护与路径规划子模块和第一蓝牙通信模块，以及传感器子模块、传感器数据采集子模块。所述图像处理子模块、摄像头标定子模块、EKF-SLAM子模块和地图维护与路径规划子模块分别通过各自的应用程序编程接ロ(Application ProgrammingInterface，缩略词为API)进行通信，以实现在清扫机器人行走过程中，利用智能手机摄像头和清扫机器人内部传感器的信息，通过基于EKF-SLAM子模块，进行导航定位运算，井根据导定位结果和所述清扫机器人模块的传感器子模块采集的数据或提供的信息，进行精确的定位，创建环境地图和建立室内环境障碍物覆盖地图，自动规划清扫机器人的清扫路线，控制清扫机器人行走轮的下一歩运动。所述智能手机摄像头用于从外界获取图像信息。所述传感器数据采集子模块包括用于将传感器采集的模拟信号转换为数字信号的模数转换(Analog To Digital Converter,缩略词为A/D)模块。所述图像处理子模块用于对从外界获取的图像进行特征提取并输出相关結果。所述摄像头标定子模块用于将采集到的图像与三维空间中的物体建立映射关系。所述EKF-SLAM子模块根据所述智能手机摄像头捕获的图像信息及所述惯性传感 器采集的清扫机器人运动參数信息进行定位，创建环境地图，还根据所述避障传感器采集的清扫机器人周围状况信息建立室内障碍物覆盖地图。所述地图维护与路径规划子模块根据所述EKF-SLAM子模块获得的地图信息规划清扫机器人的清扫路线，并通过第一蓝牙通信模块、第二蓝牙通信模块控制清扫机器人的动作。所述第二蓝牙通信模块，与所述智能手机模块中的所述第一蓝牙通信模块组成相同，通过蓝牙进行双エ通信，并利用蓝牙通信控制清扫机器人的动作。本发明的采用智能手机导航的清扫机器人技术问题通过以下进ー步的技术方案予以解決。所述传感器子模块包括惯性传感器陀螺仪和加速度计，所述陀螺仪用于采集清扫机器人运动參数角速度，所述加速度计用于采集清扫机器人运动參数线加速度。本发明的采用智能手机导航的清扫机器人的导航清扫方法技术问题通过以下技术方案予以解决。这种采用智能手机导航的清扫机器人的导航清扫方法的特点是采用智能手机导航的清扫机器人基于预设的标定模板工作于导航清扫模式，所述预设的标定模板是图案为黑白相间的棋盘格的标定模板，依次有以下步骤I)采用所述标定模板标定智能手机摄像头内參数；2)建立蓝牙通信连接；3)启动EKF-SLAM子模块，进行精确定位，创建三维环境地图；4)建立室内环境障碍物覆盖地图；5)自动规划清扫机器人的清扫路线；6)控制清扫机器人行走轮的下一歩运动。本发明的采用智能手机导航的清扫机器人的导航清扫方法技术问题通过以下进ー步的技术方案予以解決。所述步骤I)标定智能手机摄像头内參数，包括以下分步骤I I)将智能手机摄像头对准所述标定模板；I 2)启动摄像头标定子模块，在保证所述标定模板全部位于摄像头图像视野中的前提下，以至少两个方向对所述标定模板进行拍照，采集标定板图像；
I 3)摄像头标定子模块将采集到的标定板图像中检测到的点集与实际标定板的三维点集建立对应关系，采用平面标定算法自动计算出智能手机摄像头优化的内參数矩阵(fu, fv, uO, vO, kl, k2, pi, p2),并保存于智能手机中，所述智能手机摄像头优化的内參数矩阵(fu, fv, uO, vO, kl, k2, pi, p2)其中Fu :横焦距；Fv :纵焦距；uO :主点横坐标；vO :主点纵坐标；kl :镜头径向一次畸变參数；k2 :镜头径向二次畸变參数；pi :镜头切向一次畸变參数；
p2 :镜头切向二次畸变參数；系统运行过程中，同一智能手机不必再次标定。所述步骤2)建立蓝牙通信连接，包括以下分步骤2 I)启动智能手机模块中的所述第一蓝牙通信模块，与清扫机器人机身上的第ニ蓝牙通信模块进行蓝牙配对后建立蓝牙无线通信连接；2 2)由清扫机器人识别由智能手机发送的控制清扫机器人运动的指令；如果命令指令识别为查询指令，嵌入了预定的机器人状态数据采集程序的控制芯片的所述机器人控制子模块就将清扫机器人内部传感器每ー时刻的状态进行编码，并加入时间戳，返回数据状态标识符，组成传感器状态向量，再将传感器状态向量通过第一蓝牙通信模块、第二蓝牙通信模块发送到智能手机；如果指令识别为控制指令，嵌入了预定的机器人运动控制程序的控制芯片的所述机器人控制子模块对接收到的智能手机发送的控制指令进行解码，并转换为清扫机器人的运行控制命令，控制清扫机器人按预定的路径进行工作，井根据接收到的外界信号改变清扫机器人的运动状态，包括前进后退、旋转指定角度；如果控制成功，则通过第一蓝牙通信模块、第二蓝牙通信模块向智能手机发送控制返回指令，指令中包括是否操作成功标识以及控制返回指令标识；如果控制未成功，则由智能手机在延迟设定时间后重新发送控制指令，再重复分步骤2*2)，所述延迟设定时间为2秒 4秒。所述步骤3)启动EKF-SLAM子模块，进行精确定位，创建三维环境地图，包括将智能手机摄像头保持竖直向上，固定于清扫机器人顶端，以及以下分步骤3 I) EKF-SLAM子模块初始化判断是否存在已创建的地图；如果存在已创建地图，则转入EKF-SLAM算法重定位操作，采用重定位算法获得当前智能手机摄像头在已创建地图中的位置和姿态，包括以下子步骤3*1.1)对当前摄像头图像中提取的角点与已创建地图中的地图特征进行全局匹配，获得ニ维特征与三维点的初步对应关系；3*1 *2)利用基于直接线性变换(Direct Linear Transformation,缩略词为 DLT)的透视N点算法(Perspective N Points,缩略词为PNP),配合随机抽样一致性(RandomSample Consensus,缩略词为RANSAC)算法,根据已标定好的摄像头内參数(fu, fv,uO, vO,kl, k2，pi, p2)，获得最优的智能手机摄像头的位置和姿态；如果成功获得最优的智能手机摄像头的位置和姿态，则进行分步骤3 3)，一歩EKF-SLAM算法迭代；如果未成功获得最优的智能手机摄像头的位置和姿态，则进行分步骤3*2);如果不存在已创建地图，则直接进行分步骤3*2);3 2)地图初始化利用智能手机与清扫机器人建立的蓝牙连接，控制清扫机器人直线运动设定的距离，对运动前后摄像头在两个不同位置获得的两帧图像，进行全局的图像角点的提取和匹配，利用匹配结果以及天花板环境平面假设，计算出两帧图像与摄像头的相对位置和姿态R0Utl,其中Rtl是ー个3X3的旋转矩阵表示初始两帧图像与摄像头之间的三维旋转，b是ー 个3X I的列向量代表初始两帧图像与摄像头之间的三维平移，并利用三角化生成初始三维地图特征，利用捆绑调整(Bundle Adjustment,缩略词为BA)算法进行优化,得到最终初始三维地图的地图特征点和两帧图像中的清扫机器人的位置和姿态；所述清扫机器人直线运动设定的距离是25cm 35cm ；3 3)读取清扫机器人的传感器子模块中的惯性传感器采集清扫机器人的运动參数作为EKF-SLAM子模块的控制量，进行系统状态的一步预测，所述系统状态包括机器人位置、姿态，以及所有三维地图特征点的位置；3 *4)对于智能手机摄像头采集到的视频图像，利用分步骤3 *3)预测的清扫机器人状态，获得图像特征预测区域，进行角点特征提取和地图特征的匹配；如果成功进行角点特征提取和地图特征的匹配，则进行分步骤3*5);如果未成功进行角点特征提取和地图特征的匹配，即出现数据关联失败，再重复分步骤3 I)进行清扫机器人的重定位操作；所述地图特征的匹配,是采用基于主方向的灰度直方图最小均方误差(MinimumMean Squared Error,缩略词为MMSE)算法进行匹配；3 5)将分步骤3 4)进行角点特征提取和地图特征的匹配的结果结合分步骤I 3)自动计算出的智能手机摄像头优化的内參数矩阵(fu, fv, uO, vO, kl, k2, pi, p2),获得观测更新矩阵，进行EKF更新，更新分步骤3 3)预测的清扫机器人状态，同时更新地图特征点，获得清扫机器人当前的位置和姿态；3 6)判断是否存在新建地图特征需要进行特征转换；如果某ー摄像头坐标系下特征点深度的倒数P的标准差O p大于或等于设定的阈值T，则进入分步骤3 7)；如果某ー摄像头坐标系下特征点深度的倒数P的标准差0 p小于设定的阈值T，需要进行特征转换，将地图特征由反深度表示转换为三维世界坐标表示，所述阈值T的选择要保证P以99%的概率落在T与O p确定的等概曲面以内；转换公式如下
权利要求
1.一种采用智能手机导航的清扫机器人，包括清扫机器人模块，所述清扫机器人模块包括第二蓝牙通信模块、机器人控制子模块、避障传感器和行走轮控制模块，所述避障传感器包括用于采集清扫机器人的周围障碍或墙的红外墙检传感器、用于采集清扫机器人的周围地面环境的红外地检传感器、用于采集清扫机器人的周围制约信号的虚拟墙检测传感器，以及用于采集清扫机器人的周围回充座信号的座充检测传感器，所述第二蓝牙通信模块用于对外发送和接收数据，所述机器人控制子模块包括嵌入了预定的机器人路径规划程序、机器人状态数据采集程序和机器人运动控制程序的控制芯片，控制清扫机器人按预定的路径进行工作，井根据接收到的外界信号改变清扫机器人的运动状态，所述行走轮控制模块包括两个驱动轮及ー个万向轮，用于清扫机器人在工作区域运动，其特征在于 在清扫机器人机身上方设有智能手机模块，所述智能手机模块包括智能手机摄像头、图像处理子模块、摄像头标定子模块、扩展卡尔曼滤波器-同步定位与地图创建EKF-SLAM子模块、地图维护与路径规划子模块和第一蓝牙通信模块，以及传感器子模块、传感器数据采集子模块，所述图像处理子模块、摄像头标定子模块、EKF-SLAM子模块和地图维护与路径规划子模块分别通过各自的应用程序编程接ロ API进行通信，以实现在清扫机器人行走过程中，利用智能手机摄像头和清扫机器人内部传感器的信息，通过基于EKF-SLAM子模块，进行导航定位运算，井根据导定位结果和所述清扫机器人模块的传感器子模块采集的数据或提供的信息，进行精确的定位，创建环境地图和建立室内环境障碍物覆盖地图，自动规划清扫机器人的清扫路线，控制清扫机器人行走轮的下一步运动； 所述智能手机摄像头用于从外界获取图像信息，所述传感器数据采集子模块包括用于将传感器采集的模拟信号转换为数字信号的模数转换A/D模块，所述图像处理子模块用于对从外界获取的图像进行特征提取并输出相关结果，所述摄像头标定子模块用于将采集到的图像与三维空间中的物体建立映射关系，所述EKF-SLAM子模块根据所述智能手机摄像头捕获的图像信息及所述惯性传感器采集的清扫机器人运动參数信息进行定位，创建环境地图，还根据所述避障传感器采集的清扫机器人周围状况信息建立室内障碍物覆盖地图，所述地图维护与路径规划子模块根据所述EKF-SLAM子模块获得的地图信息规划清扫机器人的清扫路线，并通过第一蓝牙通信模块、第二蓝牙通信模块控制清扫机器人的动作，所述第二蓝牙通信模块，与所述智能手机模块中的所述第一蓝牙通信模块组成相同，通过蓝牙进行双エ通信，并利用蓝牙通信控制清扫机器人的动作。
2.如权利要求I所述的采用智能手机导航的清扫机器人，其特征在于 所述传感器子模块包括惯性传感器陀螺仪和加速度计，所述陀螺仪用于采集清扫机器人运动參数角速度，所述加速度计用于采集清扫机器人运动參数线加速度。
3.一种采用智能手机导航的清扫机器人的导航清扫方法，其特征在于 采用智能手机导航的清扫机器人基于预设的标定模板工作于导航清扫模式，所述预设的标定模板是图案为黑白相间的棋盘格的标定模板，依次有以下步骤 1)采用所述标定模板标定智能手机摄像头内參数； 2)建立蓝牙通信连接； 3)启动EKF-SLAM子模块，进行精确定位，创建三维环境地图； 4)建立室内环境障碍物覆盖地图； 5)自动规划清扫机器人的清扫路线；6)控制清扫机器人行走轮的下一歩运动。
4.如权利要求3所述的采用智能手机导航的清扫机器人的导航清扫方法，其特征在于 所述步骤I)标定智能手机摄像头内參数，包括以下分步骤 I D将智能手机摄像头对准所述标定模板； I 2)启动摄像头标定子模块，在保证所述标定模板全部位于摄像头图像视野中的前提下，以至少两个方向对所述标定模板进行拍照，采集标定板图像； 1 3)摄像头标定子模块将采集到的标定板图像中检测到的点集与实际标定板的三维点集建立对应关系，采用平面标定算法自动计算出智能手机摄像头优化的内參数矩阵(fu，fv, uO, vO, kl，k2，pi, p2)，并保存于智能手机中，所述智能手机摄像头优化的内參数矩阵(fu, fv, uO, vO, kl, k2, pi, p2)其中 Fu :横焦距； Fv :纵焦距； uO :主点横坐标； vO :主点纵坐标； kl :镜头径向一次畸变參数； k2 :镜头径向二次畸变參数； Pl :镜头切向一次畸变參数； P2 :镜头切向二次畸变參数； 系统运行过程中，同一智能手机不必再次标定。
5.如权利要求4所述的采用智能手机导航的清扫机器人的导航清扫方法，其特征在于 所述步骤2)建立蓝牙通信连接，包括以下分步骤 2 I)启动智能手机模块中的所述第一蓝牙通信模块，与清扫机器人机身上的第二蓝牙通信模块进行蓝牙配对后建立蓝牙无线通信连接； 2 2)由清扫机器人识别由智能手机发送的控制清扫机器人运动的指令， 如果命令指令识别为查询指令，嵌入了预定的机器人状态数据采集程序的控制芯片的所述机器人控制子模块就将清扫机器人内部传感器每ー时刻的状态进行编码，并加入时间戳，返回数据状态标识符，组成传感器状态向量，再将传感器状态向量通过第一蓝牙通信模块、第二蓝牙通信模块发送到智能手机； 如果指令识别为控制指令，嵌入了预定的机器人运动控制程序的控制芯片的所述机器人控制子模块对接收到的智能手机发送的控制指令进行解码，并转换为清扫机器人的运行控制命令，控制清扫机器人按预定的路径进行工作，井根据接收到的外界信号改变清扫机器人的运动状态，包括前进后退、旋转指定角度； 如果控制成功，则通过第一蓝牙通信模块、第二蓝牙通信模块向智能手机发送控制返回指令，指令中包括是否操作成功标识以及控制返回指令标以； 如果控制未成功，则由智能手机在延迟设定时间后重新发送控制指令，再重复分步骤.2 2)，所述延迟设定时间为2秒 4秒。
6.如权利要求5所述的采用智能手机导航的清扫机器人的导航清扫方法，其特征在于: 所述步骤3)启动EKF-SLAM子模块，进行精确定位，创建三维环境地图，包括将智能手机摄像头保持竖直向上，固定于清扫机器人顶端，以及以下分步骤 ·3 I) EKF-SLAM子模块初始化 判断是否存在已创建的地图； 如果存在已创建地图，则转入EKF-SLAM算法重定位操作，采用重定位算法获得当前智能手机摄像头在已创建地图中的位置和姿态，包括以下子步骤 ·3*1*1)对当前摄像头图像中提取的角点与已创建地图中的地图特征进行全局匹配，获得ニ维特征与三维点的初步对应关系； ·3*1*2)利用基于直接线性变换DLT的透视N点算法PNP，配合随机抽样一致性RANSAC算法，根据已标定好的摄像头内參数(fu, fv, uO, vO, kl, k2, pi, p2),获得最优的智能手机摄像头的位置和姿态； 如果成功获得最优的智能手机摄像头的位置和姿态，则进行分步骤3 3)，一歩EKF-SLAM算法迭代； 如果未成功获得最优的智能手机摄像头的位置和姿态，则进行分步骤3 2)； 如果不存在已创建地图，则直接进行分步骤3*2); ·3 2)地图初始化 利用智能手机与清扫机器人建立的蓝牙连接，控制清扫机器人直线运动设定的距离，对运动前后摄像头在两个不同位置获得的两帧图像，进行全局的图像角点的提取和匹配，利用匹配结果以及天花板环境平面假设，计算出两帧图像与摄像头的相对位置和姿态Re、tQ，其中Rtl是ー个3X3的旋转矩阵表示初始两帧图像与摄像头之间的三维旋转，t0是ー个3X1的列向量代表初始两帧图像与摄像头之间的三维平移，并利用三角化生成初始三维地图特征，利用捆绑调整BA算法进行优化，得到最終初始三维地图的地图特征点和两帧图像中的清扫机器人的位置和姿态； 所述清扫机器人直线运动设定的距离是25cm 35cm ； ·3 3)读取清扫机器人的传感器子模块中的惯性传感器采集清扫机器人的运动參数作为EKF-SLAM子模块的控制量，进行系统状态的一步预测，所述系统状态包括机器人位置、姿态，以及所有三维地图特征点的位置； ·3*4)对于智能手机摄像头采集到的视频图像，利用分步骤3 *3)预测的清扫机器人状态，获得图像特征预测区域，进行角点特征提取和地图特征的匹配； 如果成功进行角点特征提取和地图特征的匹配，则进行分步骤3 5)； 如果未成功进行角点特征提取和地图特征的匹配，即出现数据关联失败，再重复分步骤3 I)进行清扫机器人的重定位操作； 所述地图特征的匹配，是采用基于主方向的灰度直方图最小均方误差MMSE算法进行匹配； ·3 5)将分步骤3 4)进行角点特征提取和地图特征的匹配的结果结合分步骤I 3)自动计算出的智能手机摄像头优化的内參数矩阵(fu, fv, uO, vO, kl, k2, pi, p2),获得观测更新矩阵，进行EKF更新，更新分步骤3 *3)预测的清扫机器人状态，同时更新地图特征点，获得清扫机器人当前的位置和姿态；.3 6)判断是否存在新建地图特征需要进行特征转换； 如果某ー摄像头坐标系下特征点深度的倒数P的标准差0 p大于或等于设定的阈值T，则进入分步骤3 7)； 如果某ー摄像头坐标系下特征点深度的倒数P的标准差。p小于设定的阈值て，需要进行特征转换，将地图特征由反深度表示转换为三维世界坐标表示，所述阈值T的选择要保证P以99%的概率落在T与0 p确定的等概曲面以内， 转换公式如下
7.如权利要求6所述的采用智能手机导航的清扫机器人的导航清扫方法，其特征在于 所述步骤4)建立室内环境障碍物覆盖地图，是地图维护及路径规划子模块利用蓝牙通信模块读取当前清扫机器人的传感器信息，将当前清扫机器人定位结果，以及避障传感器采集的清扫机器人的周围状况信息传送至地图维护及路径规划子模块，更新地板平面障碍物覆盖地图。
8.如权利要求7所述的采用智能手机导航的清扫机器人的导航清扫方法，其特征在于 所述步骤5)自动规划清扫机器人的清扫路线，是利用步骤4)更新的地板平面障碍物覆盖地图，根据清扫机器人行进路线以及覆盖区域，规划清扫机器人下一步行进路线。
9.如权利要求8所述的采用智能手机导航的清扫机器人的导航清扫方法，其特征在于 所述步骤6)控制清扫机器人行走轮的下一步运动，是通过第一蓝牙通信模块、第二蓝牙通信模块向智能手机发送控制指令，控制清扫机器人向无障碍物且未清扫区域运动，以下重复分步骤3 3)及其后续步骤。
全文摘要
一种采用智能手机导航的清扫机器人，包括清扫机器人模块，还设有包括智能手机摄像头、图像处理子模块、摄像头标定子模块、EKF-SLAM子模块、地图维护与路径规划子模块和传感器子模块、传感器数据采集子模块的智能手机模块。其导航清扫方法依次有以下步骤1)采用标定模板标定智能手机摄像头内参数；2)建立蓝牙通信连接；3)启动EKF-SLAM子模块，进行精确定位，创建三维环境地图；4)建立室内环境障碍物覆盖地图；5)自动规划清扫机器人的清扫路线；6)控制清扫机器人行走轮的下一步运动。本发明避免了直接在清扫机器人上加装导航定位以及传感器等硬件设备，实施简单，硬件改动小，定位准确，清扫覆盖面积大，清扫效率高。
文档编号G05D1/02GK102866706SQ20121033763
公开日2013年1月9日 申请日期2012年9月13日 优先权日2012年9月13日
发明者顾照鹏 申请人:深圳市银星智能科技股份有限公司