[发明专利]清洁机器人台盆清扫路径自动规划方法及系统

权利要求书

1.一种清洁机器人台盆清扫路径自动规划方法，其特征在于，包括：

步骤S1：移动底盘导航至预设位置，然后机械臂按预设轨迹运动一周，视觉系统开始扫图识别卫生间环境；

步骤S2：移动底盘在预设位置之后，视觉系统扫图确定台盆台面的位置以及整个台盆台面的宽度width；然后视觉系统计算决定擦拭台盆台面时移动底盘的位置，视觉系统计算确定擦拭台盆台面时的导航位置点；

步骤S3：按上述步骤计算确定台盆和台面的导航位置点之后，视觉系统将计算结果发送给导航系统，导航系统再结合二维平面地图信息，计算得到导航坐标系下的位置点，然后导航系统控制底盘移动至第一个擦拭位置点；

步骤S4：导航移动至擦拭位置点之后，机械臂运动并控制视觉系统拍照；视觉拍照识别得到台盆的形状，以及台盆的特征点位，台面的角点特征点在相机坐标系下的三维位置，并将台盆台面的相关特征信息发送给机械臂控制系统；

步骤S5：控制系统接收到视觉数据之后，采用路径自动规划方法，计算得到台盆或台面的擦拭轨迹；

步骤S6：台盆台面擦拭路径生成之后，机械臂控制系统内部再规划得到每个周期位置并下发给伺服驱动器，然后伺服驱动器控制机械臂运行；

步骤S7：机械臂在当前位置点擦拭完成之后，通知导航系统，导航系统控制底盘移动至第二个位置点，接着重复步骤S4～S6；当运动至最后一个导航位置点，机械臂擦拭任务完成之后，整个台盆台面擦拭结束。

2.根据权利要求1所述的清洁机器人台盆清扫路径自动规划方法，其特征在于，所述步骤S2中视觉系统计算导航位置包括：

步骤S2.1：计算导航位置点个数N，N的计算是根据台盆的台面宽度width和机械臂臂展L决定：

N＝width/L+1

情形1)：如果N等于1，则导航仅需要一个位置点，机械臂能够一次性将整个台盆和台面打扫完成；

情形2)：如果N大于1，则需要将台盆台面分为3个区域进行打扫，即中间台盆区域、左边台面区域和右边台面区域，则此时导航需要三个位置点；

步骤S2.2：计算并导航在二维平面的位置点。

3.根据权利要求2所述的清洁机器人台盆清扫路径自动规划方法，其特征在于，所述步骤S2.2具体包括：

对于情形1)，导航位置点横向的位置为导航中心点和台盆正中间对齐，对于众向距离，导航中心点距离台面前边沿面的距离为B，导航横向距离和众向距离确定之后，导航再结合二维地图信息，即能够确定导航位置点；

对于情形2)，打扫台盆区域时导航位置点横向的位置导航中心点和台盆正中间对齐，打扫左边台面区域时导航位置点横向的位置保证导航中心点和左边台面正中间对齐，打扫右边台面区域时导航位置点横向的位置保证导航中心点和右边台面正中间对齐；

对于众向距离，导航中心点距离台面前边沿面的距离为B，导航系统获取视觉给的台盆和台面的特征信息之后，再结合二维平面地图信息，即能够确定上所述的两种情况下的导航位置点。

4.根据权利要求1所述的清洁机器人台盆清扫路径自动规划方法，其特征在于，所述步骤S5中机械臂路径自动规划包括：

矩形台面情况下：

步骤S5.1.1：计算擦拭的位置区域，视觉系统识别计算得到矩形台面的四个角点位置右下角位置pos0，左下角位置pos1，左上角位置pos2，右上角位置pos3，并将这几个位置发送给机械臂，机械臂根据这4个位置点，构成的一个四边形区域；

步骤S5.1.2：计算擦拭左右台面的姿态信息；

步骤S5.1.3：计算则得到擦拭过程中四个角点的位置以及姿态参数，再考虑台面擦拭工具的宽度，中间区域的位置再根据四个角点值，采用线性插值，得到中间区域的位置姿态值；

步骤S5.1.4：插值计算得到整个台面擦拭区域内机械臂末端的位置与姿态，然后机械臂内部按点到点的位置进行速度规划，轨迹插补确定整个擦拭路径轨迹。