一种基于机器视觉与吸盘钩爪协同的马桶开盖装置及系统的制作方法

本发明涉及机器视觉机器人作业控制，具体为一种基于机器视觉与吸盘钩爪协同的马桶开盖装置及系统。

背景技术：

1、家用清洁机器人配置机械臂执行马桶开盖，是实现马桶自动化清洁的前提，马桶盖表面存在平整度差异，盖沿可吸附位置有限，机器人需要依靠视觉识别合适接触点位，再通过末端执行器完成开盖动作，点位选取不准时易出现吸盘漏气、吸附失效的问题，导致开盖任务失败；

2、现有机器人马桶开盖方案大多依靠吸盘吸附完成抬升开盖，对吸盘负压依赖度高；缺少完整的吸附路点筛选排序，抬升、载荷转移时易造成吸盘吸附失效；容错能力不足；

3、因此亟需一种机器视觉引导下吸盘与钩爪协同的马桶开盖系统，通过视觉筛选吸附接触路点，搭配路点切换重试机制，降低对吸盘负压的依赖，提升清洁机器人马桶开盖作业的可靠性与容错性。

技术实现思路

1、本发明的目的在于提供一种基于机器视觉与吸盘钩爪协同的马桶开盖装置及系统，以解决现有技术中提出的问题。

2、为实现上述目的，本发明提供如下技术方案：一种基于机器视觉与吸盘钩爪协同的马桶开盖系统，所述马桶开盖系统包括视觉感知模块、路点与法向生成模块、末端位姿与工具补偿模块、机械臂运动模块、吸盘气路控制模块、钩爪舵机控制模块、状态机决策模块，适用于配备视觉传感器和机械臂的清洁机器人，目标对象为马桶盖；所述马桶开盖系统统一采用机器人基座坐标系b表示位置和方向，采用法兰目标坐标系tk表示第k个候选路点对应的末端姿态，所有向量在代入公式前均转换至机器人基座坐标系b，其中法兰目标坐标系是以机器人法兰参考点为原点、用于描述末端执行器目标姿态的坐标系；

3、所述视觉感知模块对马桶区域进行视觉检测，输出边缘掩膜、局部平面点集及候选路点集合；为后续吸附操作提供几何依据与候选位置；

4、所述视觉感知模块包括特征提取单元、路点排序单元；

5、所述特征提取单元对马桶区域进行视觉检测，拍摄马桶盖图像，输出边缘掩膜、局部平面点集、候选路点集合；其中边缘掩膜为马桶盖图像中标记马桶盖沿所在区域的二值像素区域，用于限定吸附点搜索范围；局部平面点集为马桶盖沿附近区域内、位于马桶盖面上的三维点的集合，用于拟合机器人吸盘吸附区域的盖面局部平面；候选路点集合为检测到的马桶盖沿间隔分布的不同三维位置点，每个三维位置点作为一个可供吸盘接触的候选路点；

6、所述路点排序单元按照局部平整度对候选路点进行排序，所述局部平整度表示候选路点邻域内马桶盖面点相对于拟合局部平面的偏离程度，其中邻域是以候选路点为中心，取周围预设半径范围内的局部平面点集，预设半径根据确保包含足够点用于平面拟合且不引入盖沿以外的点设定，马桶盖面点为局部平面点集中位于马桶盖面上的三维点，拟合局部平面由所述马桶盖面点通过平面拟合得到的、代表马桶盖面局部形状的平面；偏离程度为各马桶盖面点到所述拟合局部平面的距离的统计值，距离越小偏离程度越小、平整度越高，吸盘接触后密封越可靠；候选路点按局部平整度由高到低排序。

7、所述路点与法向生成模块从候选路点集合中按排序依次选取候选路点，由当前候选路点邻域的局部平面点集拟合得到盖面外法向；同时沿盖面外法向外侧生成预接近点，作为机械臂接近马桶盖沿的中间过渡位置，当前候选路点失败时自动切换至下一候选路点；

8、所述路点与法向生成模块包括路点选取单元、法向估计单元、预接近点生成单元；

9、所述路点选取单元从候选路点中按排序依次选取候选路点，当前选取的候选路点索引记为k，k从1开始依次递增，k=1,2,…,k，k为有效候选路点总数；当前候选路点在后续操作中失败时，标记为失败并选取索引为k+1的下一候选路点；

10、所述法向估计单元取当前候选路点邻域的局部平面点集，通过平面拟合得到盖面外法向nk，盖面外法向为垂直于马桶盖面、由马桶盖面指向自由空间的单位向量，自由空间指马桶盖面背离马桶座体的一侧、无障碍物遮挡且可供吸盘接近与抬升运动的开放空间；

11、所述预接近点生成单元沿盖面外法向外侧、与候选路点相距预接近距离dpre处确定为预接近点，预接近点为供机器人机械臂到达后再沿法向直线切入的中间过渡位置点，使抬升马桶盖时朝向一侧进行，其中预接近距离dpre是预设的固定距离值，根据吸盘工具轴向长度与机械臂安全余量综合设定，取安全避障距离。

12、所述末端位姿与工具补偿模块根据盖面外法向确定末端工具姿态，使工具轴与盖面外法向反向共线，构造法兰目标坐标系的三个坐标轴，通过工具中心点标定计算工具偏置向量，反推得到法兰接触位置；

13、所述末端位姿与工具补偿模块包括姿态确定单元、坐标系构造单元、工具补偿单元；

14、所述姿态确定单元根据马桶盖面外法向确定末端工具姿态，使吸盘接触面正对马桶盖面；工具轴zt,k的正方向固定为由法兰原点指向吸盘接触面，盖面外法向nk的正方向固定为由盖面指向自由空间，二者指向相反，因此吸盘朝向盖面时工具轴与盖面外法向满足关系式：zt,k=-nk；公式中，zt,k为第k个候选路点的法兰目标坐标系工具轴在b下的单位方向向量，nk为第k个候选路点处的盖面外法向单位向量；

15、所述坐标系构造单元选定不与工具轴平行的参考向量，通过向量叉积构造右手正交坐标系，得到xt,k、yt,k，与zt,k共同构成法兰目标坐标系的三个坐标轴，其中xt,k为法兰目标坐标系的x轴单位方向向量，公式为：xt,k=(ab×zt,k)/|(ab×zt,k)|；yt,k为法兰目标坐标系的y轴单位方向向量，公式为：yt,k=zt,k×xt,k；公式中ab是构造工具姿态所用的参考单位向量，在b下表达，不与zt,k平行；

16、所述工具补偿单元计算从法兰目标坐标系的原点指向吸盘接触中心点的工具偏置向量tfc,k：tfc,k=lzt,k+oxxt,k+oyyt,k；公式中，l为法兰目标坐标系的原点沿工具轴至吸盘接触面的轴向距离，l＞0，ox、oy为吸盘接触中心点在xt,k、yt,k平面内相对于法兰轴线的偏移量，其中法兰轴线是从法兰目标坐标系的原点沿工具轴方向延伸的中心线；接着计算得到第k个候选路点对应的法兰接触位置pf,kc：pf,kc=pk-tfc,k；公式中，pf,kc为吸盘接触pk时的法兰目标位置，pk为第k个视觉接触候选点在b下的三维位置向量。

17、所述机械臂运动模块依次完成预接近、法向接触、第一段法向抬升、第二段法向抬升、侧移开盖及回位，使吸盘平稳接触马桶盖沿并由钩爪牵引马桶盖圈构件绕铰链转动至预设打开角度；

18、所述机械臂运动模块包括预接近接触单元、法向抬升单元、侧移开盖单元；

19、所述预接近接触单元按目标位姿执行机械臂运动，机械臂先运动到预接近点，接着沿盖面外法向以受限速度直线移动至法兰接触位置pf,kc后，判定吸附确认，受限速度为低于机械臂正常运动速度的预设值，用于避免吸盘冲击盖沿，使吸盘平稳接触马桶盖沿区域；其中预接近点的位置公式为：pf,kpre=pf,kc+dprenk；公式中，pf,kpre为吸盘接触马桶盖沿之前的预接近点；

20、所述法向抬升单元在吸附确认后执行第一段法向抬升，沿盖面外法向朝远离马桶盖面的一侧抬升第一距离，使马桶盖沿与马桶座体之间形成可供钩爪进入的初始间隙；在钩爪承载并泄压后执行第二段法向抬升，沿相同法向继续抬升第二距离，两段抬升结束时的法兰位置为pf,k1、pf,k2；其中pf,k1=pf,kc+d1nk、pf,k2=pf,kc+(d1+d2)nk；公式中，pf,k1、pf,k2分别为第一段、第二段法向抬升结束时的法兰位置，d1、d2分别为第一、第二距离，其中d1根据钩爪厚度与盖沿间隙需求取定，d2吸盘脱离马桶盖面的安全余量取定，优选d1为20mm，d2为10mm；

21、所述侧移开盖单元在两段法向抬升后执行侧移开盖，沿机器人基座坐标系的预设方向平移，使钩爪牵引马桶盖圈构件绕铰链转动，达到开盖终止条件后机械臂回到默认关节位姿，侧移开盖结束时的法兰目标位置为pf,k3：pf,k3=pf,k2+dses；公式中，pf,k3是侧移开盖结束时的法兰目标位置，es为侧移开盖的预设单位方向，在b下表达，ds为沿es的侧移距离，ds＞0；优选es取基座坐标系负x轴方向，ds取为250mm，开盖终止条件为马桶盖圈构件转动至预设打开角度，打开角度根据马桶盖圈完全开启后不遮挡清洁机器人作业、且不与水箱或后方障碍物发生碰撞的要求设定。

22、所述吸盘气路控制模块管理吸气通道与排气通道的切换，吸盘接触后开启吸气并通过负压传感器进行真空确认以判定吸附是否有效，未形成有效吸附时回撤并切换候选路点，形成有效吸附时关闭吸气、开启排气进行主动泄压，使载荷由吸盘快速转移至钩爪；

23、所述吸盘气路控制模块包括气路切换单元、泄压控制单元；

24、所述气路切换单元管理吸气通道与排气通道的切换，吸气通道为产生负压、使吸盘吸附于马桶盖面的气路，排气通道为主动通入空气以破除负压的气路；第一段法向抬升开始时，打开吸气通道、关闭排气通道，读取负压传感器进行第一次真空确认，负压传感器为实时测量吸盘内气压的传感器，将测得的负压绝对值与预设第一负压阈值比较，若达到或超过第一负压阈值时，第一次真空确认通过，否则吸盘进行动作回撤，接着切换下一候选路点，其中第一负压阈值根据第一段法向抬升所需最大载荷所需的最小负压值确定；

25、第一段法向抬升后，读取负压传感器进行第二次真空确认，将测得的负压绝对值与预设第二负压阈值比较，若达到或超过第二负压阈值时，判定为有效吸附，第二次真空确认通过，否则判定为未形成有效吸附，其中第二负压阈值根据第二段法向抬升所需最大载荷所需的最小负压值确定；

26、所述泄压控制单元在判定为有效吸附时，执行钩爪钩取，钩爪到位后，关闭吸气通道，打开排气通道进行泄压，使载荷由吸盘快速转移至钩爪，完成后关闭吸气和排气通道，其中钩爪钩取指舵机从收纳位置运动至钩取位置、使钩爪进入已形成的马桶盖沿间隙并抵靠盖沿的动作；钩爪到位指钩爪到达钩取位置且进入盖沿下方；

27、所述泄压控制单元中，吸气通道状态记为usuc，usuc为0是关闭、usuc为1是打开，排气通道状态记为uvent，uvent为0是关闭、uvent为1是打开，吸附建立至钩爪到位前，气路状态为(usuc,uvent)=(1,0)；载荷由吸盘转移至钩爪后，气路状态切换为(usuc,uvent)=(0,1)；泄压完成结束马桶盖开盖操作后，气路状态为(usuc,uvent)=(0,0)。

28、所述钩爪舵机控制模块驱动钩爪在收纳位置与钩取位置之间切换，吸盘接近、接触及第一段法向抬升期间保持钩爪收纳，形成初始间隙后转入钩取位置使钩爪进入间隙并接替吸盘承担后续开盖载荷，回位阶段将钩爪收回至收纳位置；

29、所述钩爪舵机控制模块包括钩爪驱动单元、姿态切换单元；

30、所述钩爪驱动单元驱动钩爪在收纳位置与钩取位置之间切换，钩爪上沿钩爪纵向间隔设有至少两个连接孔，用于调节钩爪工作端相对于舵机轴的长度，其中钩爪工作端指钩爪上用于伸入马桶盖沿下方、直接与马桶盖沿接触并承担钩取载荷的弯折末端，收纳位置为钩爪收起、不遮挡吸盘接近马桶盖沿的舵机角度位置，钩取位置为钩爪展开、工作端进入盖沿下方的舵机角度位置；

31、所述姿态切换单元在吸盘第一段法向抬升期间，钩爪保持收纳位置，形成初始间隙后，舵机从收纳位置运动至钩取位置，使钩爪进入已形成的间隙并抵靠盖沿，接替吸盘承担后续开盖载荷，钩取位置对应舵机位置数值取决于舵机型号、零位，回位阶段舵机从钩取位置驱动至收纳位置，其中回位阶段指侧移开盖达到开盖终止条件后，机器人机械臂返回预设关节位姿的收尾阶段。

32、所述状态机决策模块通过钩爪钩取许可条件与侧移开盖许可条件进行安全联锁，许可条件满足要求后推进至下一阶段；未满足时执行失败处理，包括沿盖面外法向回撤并切换下一候选路点、将马桶盖原路径放回，当全部k个候选路点均失败时，系统退出任务并告警；

33、所述状态机决策模块包括执行许可单元、失败处理单元；

34、所述执行许可单元在第一次真空确认和第二次真空确认通过时，允许钩爪进入钩取位置，钩爪钩取许可条件为chook,k=akq1q2；公式中，公式中ak、q1、q2均为取值0或1的布尔量，将它们直接相乘时，只有当三者全部为1时乘积才为1，有一个为0乘积就为0，ak是第k个候选路点的可执行标志，当前候选路点对应的法兰目标位姿处于机器人机械臂工作空间内为1，否则为0；q1为第一次真空确认结果，负压达到阈值时为1，否则为0；q2为第二次真空确认结果，负压达到阈值时为1，否则为0，chook,k为0时不允许钩爪进入钩取位置，为1时允许钩爪进入钩取位置；

35、钩爪进入钩取位置后，确认吸气通道关闭且排气通道打开后，允许执行侧移开盖，侧移开盖许可条件为cside,k=chook,kh(1-usuc)uvent；公式中，chook,k、h、(1-usuc)、uvent四个因子的取值均为0或1，将它们直接相乘，只有当四个因子全部为1时乘积才为1，有一个为0乘积就为0，cside,k为允许第k个路点执行侧移开盖的条件，h为钩爪到达钩取位置的确认结果，舵机到位时为1，否则为0，cside,k为0时不允许侧移开盖，为1时允许侧移开盖；

36、所述失败处理单元在chook,k为0时判定当前候选路点失败，关闭吸气通道，开启排气通道并沿盖面外法向回撤至预接近点，再选取索引为k+1的下一候选路点；若cside,k为0时判定当前候选路点失败，禁止泄压和侧移开盖，将马桶盖原路径放回，当全部k个候选路点均失败时，系统退出任务并告警。

37、一种基于机器视觉与吸盘钩爪协同的马桶开盖装置，所述马桶开盖装置包括电动真空吸盘、舵机、舵机摆臂、钩爪、带磁金属块和安装结构件；所述电动真空吸盘、舵机、舵机摆臂、钩爪、带磁金属块共用安装结构件作为几何基准，使吸盘中心、舵机轴线和钩爪之间保持可标定的固定关系；

38、所述安装结构件为装置的承载基座，安装结构件设有吸盘安装面、舵机安装面和金属块安装面，分别用于固定电动真空吸盘、舵机和带磁金属块。

39、所述电动真空吸盘固定于安装结构件的吸盘安装面上，吸盘接触面朝向下方，用于与马桶盖沿建立柔性负压连接并完成初始抬升；电动真空吸盘与安装结构件之间通过螺钉紧固连接，使吸盘中心相对于安装结构件的位置固定；

40、所述带磁金属块固定于安装结构件的金属块安装面上，位于安装结构件的顶部，用于与机器人腕部适配器和移动载体工具位上的电磁吸持件配合，实现开盖末端在机械臂与工具停放位之间转移；带磁金属块与安装结构件之间通过螺钉紧固连接或嵌入式固定；

41、所述舵机固定于安装结构件的舵机安装面上，位于安装结构件的侧部，舵机输出轴朝向外侧，用于驱动舵机摆臂转动；舵机与安装结构件之间通过至少两个螺钉紧固连接，使舵机轴线相对于安装结构件的位置固定；

42、所述舵机摆臂的一端与舵机的输出轴连接，随舵机输出轴同步转动，另一端与钩爪连接，用于将舵机的旋转运动传递至钩爪；舵机摆臂与舵机输出轴之间通过轴孔配合加锁紧螺钉固定，使舵机摆臂与舵机输出轴之间无相对转动；

43、所述钩爪与舵机摆臂的另一端连接，钩爪上沿钩爪纵向间隔设有至少两个连接孔，通过选择不同的连接孔与舵机摆臂连接，可调节钩爪工作端相对于舵机轴的有效长度；钩爪与舵机摆臂之间通过螺栓穿过连接孔紧固连接，钩爪随舵机摆臂在收纳位置与钩取位置之间切换；

44、所述电动真空吸盘、舵机、带磁金属块均直接固定于安装结构件上，所述舵机摆臂通过舵机间接安装于安装结构件上，所述钩爪通过舵机摆臂间接安装于安装结构件上。

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

46、1、基于视觉局部平整度筛选、排序、切换吸附路点：利用马桶盖局部平面点集评估各候选路点的平面偏离程度，优先选用密封条件更好的吸附位置；单一路点吸附失败时自动回撤切换下一个候选路点，全部路点失效时告警退出，提升吸附环节容错能力。

47、2、实现吸盘、钩爪载荷分步转移：依靠吸盘第一段法向抬升制造马桶盖沿间隙，钩爪伸入间隙后通过气路主动泄压，将开盖载荷由吸盘转移至钩爪承担，后续开盖动作不完全依赖吸盘负压，降低吸盘漏气造成任务失败的风险。

48、3、设置安全联锁与对应失败处理逻辑：对钩爪钩取、侧移开盖分别设置多因子布尔判断条件，只有视觉位姿、真空负压、钩爪到位、气路状态全部满足才允许进入下一作业阶段；环节异常时执行回撤、放回、切换路点动作，避免机械臂发生误动作，保障整机作业安全性。