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

权利要求书

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

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

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

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

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

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

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

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

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

计算导航位置点个数N，根据镜面宽度width和机械臂臂展L决定：

N＝width/L+1

计算导航在二维平面的位置点，第N个导航点沿镜子下边沿方向导航中心点离左边镜面的距离为：

A＝N*width/(N+1)

垂直镜面方向，导航中心点距离镜面的距离为B，保证B大于等于根据臂末端长度和清洁镜面用的刮刀长度之和。

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

步骤S5.1：计算擦拭的位置区域，由视觉系统识别计算得到矩形的四个角点位置；

步骤S5.2：计算擦拭镜面的姿态信息；

步骤S5.3：计算得到擦拭过程中四个角点的位置以及姿态信息，再考虑刮刀宽度，即能够通过线性插值得到整个擦拭区域内机械臂路径的终点的位置与姿态，计算得到中间过程所有的终点位置值，确定整个擦拭路径；

步骤S5.4：机械臂按照通俗的T型或S型速度规划，轨迹插补，即能够计算得到每个周期下发给伺服驱动器的位置。

4.根据权利要求3所述的清洁机器人镜面清扫路径自动规划方法，其特征在于，所述步骤S5.1中计算擦拭的位置区域具体包括：

视觉系统会识别计算得到矩形的四个角点位置右下角位置pos0，左下角位置pos1，左上角位置pos2，右上角位置pos3；并将这几个位置发送给机械臂；

机械臂根据上述四个位置点，然后再结合机械臂的可运动空间，计算得到机械臂能够擦拭的镜面高度h，得到新的左上角位置pos2_1和右上角的位置pos3_1，则确定该导航位置点能够擦拭的镜面区域为pos0，pos1，pos2_1，pos3_1四个点构成的一个四边形区域。

5.根据权利要求4所述的清洁机器人镜面清扫路径自动规划方法，其特征在于，所述步骤S5.2中计算擦拭镜面的姿态信息具体包括：

在镜面上建立一个用户坐标系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]。