清洁机器人镜面清扫路径自动规划方法及系统与流程

1.本发明涉及机器人清洁技术领域，具体地，涉及清洁机器人在酒店卫生间场景下镜面清扫系统以及机械臂镜面清扫的路径自动规划方法，尤其涉及一种清洁机器人镜面清扫路径自动规划方法及系统。


背景技术：

2.现在在清洁领域，使用智能机器人对酒店卫生间清扫属于空白领域，酒店卫生间清扫基本是人力清扫。使用智能机器人取代人力，是可行的也是前景广阔的一个市场。在清扫机器人进驻酒店之前，一般需要调试人员先针对酒店环境进行部署调试，但是酒店清扫的流程复杂，也给部署带来了很大的挑战，如何快速的部署是当前需要解决的技术问题。
3.公开号为cn206623100u的实用新型专利，公开了一种镜面清扫机器人控制结构，包括控制模块，控制模块连接编码器并通过电机驱动模块连接直流驱动电机，控制模块通过压力采集电路连接压力传感器、通过限位采集电路连接限位传感器、通过障碍采集电路连接障碍识别传感器、通过电磁阀驱动电路连接水用电磁阀以及通过水位采集电路连接水位传感器；控制模块连接电动伸缩杆控制器，电动伸缩杆控制器连接伸缩杆驱动电机。


技术实现要素：

4.针对现有技术中的缺陷，本发明提供一种清洁机器人镜面清扫路径自动规划方法及系统。
5.根据本发明提供的一种清洁机器人镜面清扫路径自动规划方法及系统，所述方案如下：
6.第一方面，提供了一种清洁机器人镜面清扫路径自动规划方法，所述方法包括：
7.步骤s1：移动底盘导航至预设位置，然后机械臂按预设轨迹运动一周，视觉系统开始扫图识别卫生间环境；
8.步骤s2：移动底盘在预设位置之后，视觉系统扫图确定镜面的位置以及镜面宽度width；然后视觉系统计算决定擦拭镜面时移动底盘的位置，视觉系统计算确定n个擦拭镜面时的导航位置点；
9.步骤s3：按上述步骤计算确定n个擦拭的导航位置点之后，视觉系统将计算结果发送给导航系统，导航系统再结合二维平面地图信息，计算得到导航坐标系下的位置点，然后导航系统控制底盘移动至第一个擦拭位置点；
10.步骤s4：导航移动至擦拭位置点之后，机械臂会运动并控制视觉系统拍照；视觉拍照识别得到镜子的形状，以及镜子角点特征点在相机坐标系下的三维位置，并将镜子相关特征信息发送给控制系统；
11.步骤s5：控制系统接收到视觉数据之后，采用路径自动规划方法，计算得到镜面的擦拭轨迹；
12.步骤s6：镜面擦拭路径生成之后，机械臂控制系统内部再规划得到每个周期位置
并下发给伺服驱动器，然后伺服驱动器控制机械臂运行；
13.步骤s7：机械臂在当前位置点擦拭完成之后，会通知导航系统，导航系统会控制底盘移动至第二个位置点，接着重复步骤s4～s6；当运动至最后一个导航位置点，机械臂擦拭任务完成之后，整个镜面擦拭结束。
14.优选地，所述步骤s2中视觉系统计算导航位置包括：
15.计算导航位置点个数n，根据镜面宽度width和机械臂臂展l决定：
16.n＝width/l 1
17.计算导航在二维平面的位置点，第n个导航点沿镜子下边沿方向导航中心点离左边镜面的距离为：
18.a＝n*width/(n 1)
19.垂直镜面方向，导航中心点距离镜面的距离为b，保证b大于等于根据臂末端长度和清洁镜面用的刮刀长度之和。
20.优选地，所述步骤s5中机械臂路径自动规划包括：
21.步骤s5.1：计算擦拭的位置区域，由视觉系统识别计算得到矩形的四个角点位置；
22.步骤s5.2：计算擦拭镜面的姿态信息；
23.步骤s5.3：计算得到擦拭过程中四个角点的位置以及姿态信息，再考虑刮刀宽度，即能够通过线性插值得到整个擦拭区域内机械臂路径的终点的位置与姿态，计算得到中间过程所有的终点位置值，确定整个擦拭路径；
24.步骤s5.4：机械臂按照通俗的t型或s型速度规划，轨迹插补，即能够计算得到每个周期下发给伺服驱动器的位置。
25.优选地，所述步骤s5.1中计算擦拭的位置区域具体包括：
26.视觉系统会识别计算得到矩形的四个角点位置右下角位置pos0，左下角位置pos1，左上角位置pos2，右上角位置pos3；并将这几个位置发送给机械臂；
27.机械臂根据上述四个位置点，然后再结合机械臂的可运动空间，计算得到机械臂能够擦拭的镜面高度h，得到新的左上角位置pos2_1和右上角的位置pos3_1，则确定该导航位置点能够擦拭的镜面区域为pos0，pos1，pos2_1，pos3_1四个点构成的一个四边形区域。
28.优选地，所述步骤s5.2中计算擦拭镜面的姿态信息具体包括：
29.在镜面上建立一个用户坐标系user_ref，用pos0，pos1，pos2_1，pos3_1拟合得到镜面平面方程，确定则user_ref的z方向user_ref_rz为垂直镜面向里的方向向量；取user_ref的x方向user_ref_rx为pos0指向pos1的方向向量，确定user_ref_rx和user_ref_rz的方向之后，根据叉乘即可确定user_ref的y方向user_ref_ry，此时确定user_ref为一个3
×
3的旋转矩阵rot_plane＝[user_ref_rx，user_ref_ry，user_ref_rz]。
[0030]
优选地，所述步骤s6包括：
[0031]
在运动过程中，导航系统激光雷达会检测周围是否有人靠近机器人，当距离在0.5～1米范围时，为1级预警；当距离小于0.5米时为2级预警；
[0032]
当条件触发时，导航系统会将信号发送给机械臂，机械臂收到1级预警信号时，内部会降速至现在速度的50％；
[0033]
机械臂收到2级预警信号时，机械臂会暂停任务，直至预警解除，机械臂会重新再开始运动任务。
[0034]
第二方面，提供了一种清洁机器人镜面清扫路径自动规划系统，所述系统包括：
[0035]
模块m1：移动底盘导航至预设位置，然后机械臂按预设轨迹运动一周，视觉系统开始扫图识别卫生间环境；
[0036]
模块m2：移动底盘在预设位置之后，视觉系统扫图确定镜面的位置以及镜面宽度width；然后视觉系统计算决定擦拭镜面时移动底盘的位置，视觉系统计算确定n个擦拭镜面时的导航位置点；
[0037]
模块m3：按上述步骤计算确定n个擦拭的导航位置点之后，视觉系统将计算结果发送给导航系统，导航系统再结合二维平面地图信息，计算得到导航坐标系下的位置点，然后导航系统控制底盘移动至第一个擦拭位置点；
[0038]
模块m4：导航移动至擦拭位置点之后，机械臂会运动并控制视觉系统拍照；视觉拍照识别得到镜子的形状，以及镜子角点特征点在相机坐标系下的三维位置，并将镜子相关特征信息发送给控制系统；
[0039]
模块m5：控制系统接收到视觉数据之后，采用路径自动规划方法，计算得到镜面的擦拭轨迹；
[0040]
模块m6：镜面擦拭路径生成之后，机械臂控制系统内部再规划得到每个周期位置并下发给伺服驱动器，然后伺服驱动器控制机械臂运行；
[0041]
模块m7：机械臂在当前位置点擦拭完成之后，会通知导航系统，导航系统会控制底盘移动至第二个位置点，接着重复模块m4～m6；当运动至最后一个导航位置点，机械臂擦拭任务完成之后，整个镜面擦拭结束。
[0042]
优选地，所述模块m2中视觉系统计算导航位置包括：
[0043]
计算导航位置点个数n，根据镜面宽度width和机械臂臂展l决定：
[0044]
n＝width/l 1
[0045]
计算导航在二维平面的位置点，第n个导航点沿镜子下边沿方向导航中心点离左边镜面的距离为：
[0046]
a＝n*width/(n 1)
[0047]
垂直镜面方向，导航中心点距离镜面的距离为b，保证b大于等于根据臂末端长度和清洁镜面用的刮刀长度之和。
[0048]
优选地，所述模块m5中机械臂路径自动规划包括：
[0049]
模块m5.1：计算擦拭的位置区域，由视觉系统识别计算得到矩形的四个角点位置；
[0050]
模块m5.2：计算擦拭镜面的姿态信息；
[0051]
模块m5.3：计算得到擦拭过程中四个角点的位置以及姿态信息，再考虑刮刀宽度，即能够通过线性插值得到整个擦拭区域内机械臂路径的终点的位置与姿态，计算得到中间过程所有的终点位置值，确定整个擦拭路径；
[0052]
模块m5.4：机械臂按照通俗的t型或s型速度规划，轨迹插补，即能够计算得到每个周期下发给伺服驱动器的位置。
[0053]
优选地，所述模块m5.1中计算擦拭的位置区域具体包括：
[0054]
视觉系统会识别计算得到矩形的四个角点位置右下角位置pos0，左下角位置pos1，左上角位置pos2，右上角位置pos3；并将这几个位置发送给机械臂；
[0055]
机械臂根据上述四个位置点，然后再结合机械臂的可运动空间，计算得到机械臂
能够擦拭的镜面高度h，得到新的左上角位置pos2_1和右上角的位置pos3_1，则确定该导航位置点能够擦拭的镜面区域为pos0，pos1，pos2_1，pos3_1四个点构成的一个四边形区域；
[0056]
所述步骤s5.2中计算擦拭镜面的姿态信息具体包括：
[0057]
在镜面上建立一个用户坐标系user_ref，用pos0，pos1，pos2_1，pos3_1拟合得到镜面平面方程，确定则user_ref的z方向user_ref_rz为垂直镜面向里的方向向量；取user_ref的x方向user_ref_rx为pos0指向pos1的方向向量，确定user_ref_rx和user_ref_rz的方向之后，根据叉乘即可确定user_ref的y方向user_ref_ry，此时确定user_ref为一个3
×
3的旋转矩阵rot_plane＝[user_ref_rx，user_ref_ry，user_ref_rz]；
[0058]
所述步骤s6包括：
[0059]
在运动过程中，导航系统激光雷达会检测周围是否有人靠近机器人，当距离在0.5～1米范围时，为1级预警；当距离小于0.5米时为2级预警；
[0060]
当条件触发时，导航系统会将信号发送给机械臂，机械臂收到1级预警信号时，内部会降速至现在速度的50％；
[0061]
机械臂收到2级预警信号时，机械臂会暂停任务，直至预警解除，机械臂会重新再开始运动任务。
[0062]
与现有技术相比，本发明具有如下的有益效果：
[0063]
本发明能够解决扫图过程繁琐，操作复杂的痛点，机械臂自动路径规划，简化了部署流程，具有智能化，大大提升部署时的效率。
附图说明
[0064]
通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：
[0065]
图1为系统结构示意图；
[0066]
图2为机械臂自动路径规划示意图；
[0067]
图3为系统流程图。
具体实施方式
[0068]
下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。
[0069]
本发明实施例提供了一种清洁机器人镜面清扫路径自动规划方法，参照图1和图2所示，清洁机器人的构成包括：机械臂、移动底盘、激光雷达传感器、伺服驱动器、视觉系统以及控制系统。
[0070]
机械臂安装在移动底盘上，移动底盘前后均安装有激光雷达传感器，雷达有两方面的作用，第一个是用来导航；第二个作用即是当移动底盘导航至镜面擦拭位置点之后，机械臂运动过程中移动底盘保持不动，此时激光雷达用来检测确保机械臂工作区间内没人，当有人靠近机器人时，激光雷达能够识别，导航系统会给机械臂控制系统发送预警信号；然后机械臂控制系统会做出快速响应。
[0071]
视觉系统安装在机械臂末端位置，视觉系统用来计算确定镜面擦拭时导航位置点；同时视觉系统还要用来识别镜面相关特征，并将镜面特征发送给机械臂控制系统，本实施例中的视觉系统采用相机。
[0072]
控制系统根据视觉反馈的镜面特征信息，采用路径自动规划方法生成对应的擦拭轨迹；控制系统接收到导航给的激光雷达预警信号，根据警告级别采取对应的响应措施。
[0073]
伺服驱动器则负责控制机械臂运行。
[0074]
具体地，本发明实施例提供的一种清洁机器人镜面清扫路径自动规划方法，参照图2和图3所示，该方法具体包括：
[0075]
步骤s1：移动底盘导航至预设位置，然后机械臂按预设轨迹运动一周，视觉系统开始扫图识别卫生间环境(移动底盘带着机械臂移动到酒店洗手间的预设位置区域，该区域能够保证机械臂小范围的移动不会刮擦其他物品，机械臂移动过程中视觉系统能够扫描构建整个洗手间的三维点云模型)。
[0076]
步骤s2：移动底盘在预设位置之后，视觉系统扫图确定镜面的位置以及镜面宽度width；然后视觉系统计算决定擦拭镜面时移动底盘的位置，视觉系统计算确定n个擦拭镜面时的导航位置点。
[0077]
该步骤s2中视觉系统计算导航位置具体包括：
[0078]
计算导航位置点个数n，根据镜面宽度width和机械臂臂展l决定：
[0079]
n＝width/l 1
[0080]
计算导航在二维平面的位置点，第n个导航点沿镜子下边沿方向导航中心点离左边镜面的距离为：
[0081]
a＝n*width/(n 1)
[0082]
垂直镜面方向，导航中心点距离镜面的距离为b，其中b的值是根据臂末端长度以及清洁镜面用的刮刀长度决定的，b不能太大也不能太小，保证b大于等于根据臂末端长度和清洁镜面用的刮刀长度之和，这样保证机械臂腕关节不会发生干涉；本实施例中b取为极小值，即b等于根据臂末端长度和清洁镜面用的刮刀长度之和。
[0083]
步骤s3：按上述步骤计算确定n个擦拭的导航位置点之后，视觉系统会将计算结果发送给导航系统，导航系统再结合二维平面地图信息，计算得到导航坐标系下的位置点，然后导航系统会控制底盘移动至第一个擦拭位置点。
[0084]
步骤s4：导航移动至擦拭位置点之后，机械臂会运动并控制视觉系统拍照；视觉拍照识别得到镜子的形状，以及镜子角点特征点在相机坐标系下的三维位置，并将镜子相关特征信息发送给控制系统。
[0085]
步骤s5：控制系统接收到视觉数据之后，采用路径自动规划方法，计算得到镜面的擦拭轨迹。
[0086]
该步骤s5中机械臂路径自动规划具体包括：
[0087]
步骤s5.1：以矩形形状镜子为例进行讲解，首先计算擦拭的位置区域，由视觉系统识别计算得到矩形的四个角点位置。
[0088]
具体地，参照图3所示，视觉系统会识别计算得到矩形的四个角点位置右下角位置pos0，左下角位置pos1，左上角位置pos2，右上角位置pos3；并将这几个位置发送给机械臂；机械臂根据上述四个位置点，然后再结合机械臂的可运动空间，计算得到机械臂能够擦拭
的镜面高度h，得到新的左上角位置pos2_1和右上角的位置pos3_1，则确定该导航位置点能够擦拭的镜面区域为pos0，pos1，pos2_1，pos3_1四个点构成的一个四边形区域。
[0089]
步骤s5.2：然后计算擦拭镜面的姿态信息。
[0090]
具体地，首先在镜面上建立一个用户坐标系user_ref，用pos0，pos1，pos2_1，pos3_1拟合得到镜面平面方程，确定则user_ref的z方向user_ref_rz为垂直镜面向里的方向向量；取user_ref的x方向user_ref_rx为pos0指向pos1的方向向量，确定user_ref_rx和user_ref_rz的方向之后，根据叉乘即可确定user_ref的y方向user_ref_ry，此时确定user_ref为一个3
×
3的旋转矩阵rot_plane＝[user_ref_rx，user_ref_ry，user_ref_rz]；由于擦拭镜面过程中刮刀必须垂直镜面完全贴合镜面，因此计算擦拭轨迹的姿态时只需要计算四个擦拭角点的姿态，该姿态只需要取为user_ref沿user_ref_rx方向旋转一定角度即为机械臂末端位置姿态，首先确定pos2_1和pos3_1的姿态，根据刮刀的设计尺寸，计算得到沿x方向的旋转角度rx；此方案中取旋转角度为30
°
，即rx＝30*3.1425/180，则pos2_1和pos3_1的姿态为rot_mat＝rot_plane*rotx(rx)；然后确定pos0和pos1的姿态，根据pos0和pos1的z值和x值，计算得到沿x方向的旋转角度rx0；如果z小于等于0，取旋转角度为90
°
，即rx0＝90*3.1425/180，如果z大于0则rx0＝arctan(x/z)，则pos0和pos1的姿态为rot_mat0＝rot_plane*rotx(rx)；其中rotx()该函数是机械臂通用理解的，此处不在描述其具体推导。
[0091]
步骤s5.3：基于上述计算得到擦拭过程中四个角点的位置以及姿态信息，再考虑刮刀宽度，即能够通过线性插值得到整个擦拭区域内机械臂路径的终点的位置与姿态，计算得到中间过程所有的终点位置值，确定整个擦拭路径。
[0092]
步骤s5.4：然后机械臂按照通俗的t型或s型速度规划，轨迹插补，即能够计算得到每个周期下发给伺服驱动器的位置。
[0093]
步骤s6：镜面擦拭路径生成之后，机械臂控制系统内部再规划得到每个周期位置并下发给伺服驱动器，然后伺服驱动器控制机械臂运行。
[0094]
在运动过程中，导航系统激光雷达会检测周围是否有人靠近机器人，当距离在0.5～1米范围时，为1级预警；当距离小于0.5米时为2级预警；
[0095]
当条件触发时，导航系统会将信号发送给机械臂，机械臂收到1级预警信号时，内部会降速至现在速度的50％；
[0096]
机械臂收到2级预警信号时，机械臂会暂停任务，直至预警解除，机械臂会重新再开始运动任务。
[0097]
步骤s7：机械臂在当前位置点擦拭完成之后，会通知导航系统，导航系统会控制底盘移动至第二个位置点，接着重复步骤s4～s6；当运动至最后一个导航位置点，机械臂擦拭任务完成之后，整个镜面擦拭结束。
[0098]
根据上述描述的智能清扫系统与机械臂路径自动规划方法，即可完成镜面的路径规划；然后机械臂控制器在每个插补周期，生成轨迹插补末端笛卡尔位置，然后通过逆解计算得到每个关节的角度，然后将关节角度位置下发给伺服，最终实现镜面的清扫动作。
[0099]
本发明实施例提供了一种清洁机器人镜面清扫路径自动规划方法及系统，导航系统与视觉系统协同，根据酒店卫生间现场环境和机械臂臂展信息，计算得到机械臂清扫镜面时导航的位置点；导航位置点确定之后，机械臂末端能够控制视觉系统拍照，视觉系统识
别镜面形状以及镜面的特征点；机械臂和视觉通讯，机械臂获取视觉系统给的镜面形状以及当前导航位置下镜面角点在机械臂基坐标系下的位置，然后机械臂控制器采用路径自动规划方法计算生成镜面的清扫轨迹动作，大大提升部署时的效率。
[0100]
本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统及其各个装置、模块、单元以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统及其各个装置、模块、单元以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同功能。所以，本发明提供的系统及其各项装置、模块、单元可以被认为是一种硬件部件，而对其内包括的用于实现各种功能的装置、模块、单元也可以视为硬件部件内的结构；也可以将用于实现各种功能的装置、模块、单元视为既可以是实现方法的软件模块又可以是硬件部件内的结构。
[0101]
在本技术的描述中，需要理解的是，术语“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本技术和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本技术的限制。
[0102]
以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本技术的实施例和实施例中的特征可以任意相互组合。