清洁用机械臂规划控制系统、方法及清洁机器人与流程

1.本发明涉及清洁机器人的技术领域，具体地，涉及一种清洁用机械臂规划控制系统、方法及清洁机器人。


背景技术：

2.移动机器人和通用机械臂结合在一起，构成一种新型机器人：复合型机器人。复合型机器人既带有移动机器人的行走、巡逻和报警等功能，又带有通用机械臂的抓取、码垛、喷涂等功能，因此复合型机器人可以应用在复杂的场景中，在不同的场景之间来回操作，能够完成多项任务。将复合型机器人应用到室内空间清洁，能够弥补扫地机器人不能清洁的位置和空间。将相机和复合型机器人搭配使用，可以使机械臂具有可视性，更加智能化。现有机械臂运动规划控制中，均是线对机械臂进行离线运动路径规划，通过相机获取目标位置与周围物体位置，通过概率搜索得到路径，进而使机械臂行走在此路径中，让机械臂不会触碰到周围物体到达目标位置。
3.由于在进行清洁任务时，机械臂末端的清洁工具通常需要在笛卡尔空间中运动复杂的轨迹，这些轨迹可以使清洁工具全面覆盖被清洁的位置，因此清洁任务仅约束三个自由度，使机械臂末端的清洁工具按照指定路径运动。
4.公开号为cn114700937a的中国专利文献公开了一种机械臂及其运动路径规划方法、控制系统、介质及机器人，该机械臂运动路径规划方法包括以下步骤：设置起始点及目标点；分别以起始点及目标点作为根节点建立随机树进行双向搜索，以获得连接起始点与目标点的初始路径；对初始路径进行路径优化处理，以得到连接起始点与目标点的最短路径。
5.针对上述中的现有技术，发明人认为上述“点对点”式的离线运动路径规划并不适用于机械臂规划清洁路径，清洁效果较差，不能满足机械臂清洁任务的要求，且在复杂的环境中避障较难。


技术实现要素：

6.针对现有技术中的缺陷，本发明的目的是提供一种清洁用机械臂规划控制系统、方法及清洁机器人。
7.根据本发明提供的一种清洁用机械臂规划控制系统，包括机械臂模块；
8.所述机械臂模块包括机械臂、夹爪以及机械臂控制器；
9.所述夹爪安装在机械臂的端部；
10.所述机械臂控制器控制机械臂和夹爪；
11.所述夹爪用于抓取、移动或放下清洁工具；
12.所述机械臂控制器根据被清洁物体表面制定清洁工具的清洁路径，并通过机械臂和夹爪控制清洁工具按照清洁路径运动；
13.所述机械臂控制器根据清洁路径、运动在清洁路径中的清洁工具、机械臂当前状
态以及机械臂周围环境为机械臂实时制定避障路径，并在清洁工具运动时控制机械臂实时按照避障路径运动。
14.优选的，所述机械臂上设置有视觉模块；
15.所述视觉模块获取被清洁物体表面以及机械臂周围环境的点云数据，进而获取目标特征点；
16.所述视觉模块将获取的目标特征点传递到机械臂控制器中；
17.所述目标特征点包括被清洁物体表面特征点和机械臂周围环境特征点。
18.优选的，在所述机械臂控制器中，规划与算法控制如下：
19.根据机械臂目标位置和避障位置计算得到关节避障速度；
20.根据机械臂实际位置和目标位置计算得到关节参考速度；
21.根据关节参考速度和关节避障速度叠加得到综合速度；
22.机械臂根据综合速度达到目标位置。
23.优选的，在所述机械臂控制器中，
24.根据清洁目标位置x
goal
和机械臂当前状态实际位置x
fdb
计算位置偏差补偿速度v
err
：
[0025][0026]
其中，k表示位置偏差补偿速度的增益矩阵；x
goal
表示清洁路径上的目标位置；d
goal
表示目标位置与机械臂末端当前位置的距离；δ表示阈值；
[0027]
根据清洁路径生成的目标线速度v
goal
和位置偏差补偿速度v
err
计算机械臂笛卡尔空间内的期望线速度v
ref
：
[0028]vref
＝v
goal
v
err
[0029]
其中，v
err
表示由位置偏差引起的位置偏差补偿速度；
[0030]
根据避障补偿速度v
obstacle
和目标线速度v
goal
计算关节避障速度
[0031][0032]
其中，v
obstacle
表示根据周围环境生成的避障补偿速度；α0表示避障补偿速度增益；j0表示机械臂离障碍物预定距离处的线速度雅克比矩阵；je表示机械臂清洁工具末端线速度雅克比矩阵；αn表示避障补偿速度与末端线速度混合增益；i表示单位矩阵；表示机械臂清洁工具末端线速度雅克比矩阵的伪逆矩阵；表示机械臂清洁工具末端线速度雅克比矩阵的逆矩阵；
[0033]
根据机械臂笛卡尔空间内的期望线速度vref计算关节参考速度
[0034][0035]
关节参考速度和关节避障速度叠加得到综合速度
[0036][0037]
优选的，在所述机械臂控制器中，对于α0：
[0038]
[0039]
其中，d为机械臂连杆模型各端点与障碍物的预定距离，d
ug
表示产生单位避障补偿速度的距离阈值。
[0040]
优选的，在所述机械臂控制器中，对于αn：
[0041][0042]
其中，d
soi
为障碍物不影响机械臂运动的临界距离。
[0043]
根据本发明提供的一种清洁用机械臂规划控制方法，应用清洁用机械臂规划控制系统，包括如下步骤：
[0044]
清洁工具运动步骤：所述机械臂控制器根据被清洁物体表面制定清洁工具的清洁路径，并通过机械臂和夹爪控制清洁工具按照清洁路径运动；
[0045]
机械臂运动步骤：所述机械臂控制器根据清洁路径、运动在清洁路径中的清洁工具、机械臂当前状态以及机械臂周围环境为机械臂实时制定避障路径，并在清洁工具运动时控制机械臂实时按照避障路径运动。
[0046]
优选的，该方法还包括视觉信息获取步骤：被清洁物体表面以及机械臂周围环境的点云数据，进而获取目标特征点。
[0047]
优选的，在所述机械臂运动步骤中，根据机械臂目标位置和避障位置计算得到关节避障速度；
[0048]
根据机械臂实际位置和目标位置计算得到关节参考速度；
[0049]
根据关节参考速度和关节避障速度叠加得到综合速度；
[0050]
机械臂根据综合速度达到目标位置。
[0051]
根据本发明提供的一种清洁机器人，应用清洁用机械臂规划控制系统，包括移动机器人模块、机械臂模块和视觉模块；
[0052]
所述机械臂的背离夹爪的一端安装在移动机器人模块上；
[0053]
所述移动机器人模块根据接收的指令进行路径规划和运动控制移动到预定位置；
[0054]
所述视觉模块获取被清洁物体表面以及机械臂周围环境信息，并判断是否完成清洁；
[0055]
所述机械臂控制器根据被清洁物体表面制定清洁工具的清洁路径，并通过机械臂和夹爪控制清洁工具按照清洁路径运动；
[0056]
所述机械臂控制器根据清洁路径、运动在清洁路径中的清洁工具、机械臂当前状态以及机械臂周围环境为机械臂实时制定避障路径，并在清洁工具运动时控制机械臂实时按照避障路径运动。
[0057]
与现有技术相比，本发明具有如下的有益效果：
[0058]
1、本发明能够自动规划移动路径和清洁路径，并解决机器人在复杂环境中自动避障问题，保证机器人清洁功能和避障功能；
[0059]
2、本发明根据被清洁面尺寸制定相应位置约束，并通过算法指定机械臂的避障路径，提高了清洁机器人的部署效率。
附图说明
[0060]
通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：
[0061]
图1为清洁机器人组成图；
[0062]
图2为清洁机器人工作流程框架图；
[0063]
图3为机械臂末端清洁路径示意图。
具体实施方式
[0064]
下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。
[0065]
本发明实施例公开了一种清洁机器人，如图1所示，由三大模块组成：移动机器人模块、机械臂模块和视觉模块。如图1所示，机械臂模块设置在移动机器人模块上，视觉模块设置在机械臂模块上。
[0066]
所述机械臂模块包括机械臂、夹爪以及机械臂控制器。夹爪安装在机械臂的端部；所述机械臂控制器控制机械臂和夹爪；夹爪用于抓取、移动或放下清洁工具。机械臂的背离夹爪的一端安装在移动机器人模块上。机械臂实时规划与运动控制程序均运行于机械臂控制器中。
[0067]
移动机器人模块能够进行导航、slam以及运动规划。slam的英文全称为simultaneous localization and mapping，中文译文为即时定位与地图构建或并发建图与定位。移动机器人在移动时进行避障。
[0068]
视觉模块设置在机械臂上，视觉模块获取被清洁物体表面以及机械臂周围环境的点云数据，进而获取目标特征点；视觉模块将获取的目标特征点传递到机械臂控制器中；目标特征点包括被清洁物体表面特征点和机械臂周围环境特征点。视觉模块包括深度相机，提供目标平面与立体的点云数据，并根据点云数据获取目标特征点。
[0069]
移动机器人模块根据接收的指令进行路径规划和运动控制移动到预定位置。
[0070]
视觉模块获取被清洁物体表面以及机械臂周围环境信息，并判断是否完成清洁。
[0071]
机械臂控制器根据被清洁物体表面制定清洁工具的清洁路径，并通过机械臂和夹爪控制清洁工具按照清洁路径运动。
[0072]
机械臂控制器根据清洁路径、运动在清洁路径中的清洁工具、机械臂当前状态以及机械臂周围环境为机械臂实时制定避障路径，并在清洁工具运动时控制机械臂实时按照避障路径运动。
[0073]
如图2所示，工作人员下发任务指令，移动机器人模块移动到第i个指定点，机械臂运动至拍照构型，视觉模块获取被清洁物体位置信息，并判断是否完成所有物体清洁，识别是否存在污渍，若存在污渍，控制机械臂抓取清洁工具并清洁被清洁物体，当所有待清洁物体均被清洁后，该室内清洁流程完成。若不存在污渍，直接结束操作。机械臂基于视觉模块的三维点云匹配技术与图像分割技术进行实时规划与控制。
[0074]
机械臂的清洁任务在进行时，对机械臂进行约束，约束清洁工具笛卡尔位置，清洁
工具笛卡尔位置位于机械臂末端，并约束机械臂姿态，机械臂进行避障。
[0075]
对清洁工具笛卡尔位置进行约束，对于被清洁物体表面，规划出路径保证该路径能够全覆盖被清洁物体表面。如图3所示，针对圆形清洁区域，根据清洁工具尺寸确定清洁区域点位，根据清洁区域尺寸与清洁工具尺寸选取合适的点位顺序。
[0076]
保证清洁工具处于预设路径上，利用机械臂的冗余自由度进行避障规划。机械臂模块接收到任务指令开始工作，视觉模块将被清洁物体信息与周围环境信息传递给机械臂模块，若机械臂完成了预设清洁路径，工作任务完成；若机械臂未完成预设清洁路径，则在预设路径下继续运动：根据机械臂周围信息与机械臂状态规划出下一时刻机械臂位姿，并控制机械臂到达下一时刻位姿。
[0077]
机械臂在运动过程中进行避障：已知机械臂末端期望位置和实际位置，通过控制算法使机械臂末端平稳、准确并快速的到达目标位置，并在此过程中进行避障。
[0078]
智能规划与控制算法如下：根据机械臂目标位置和避障位置计算得到关节避障速度。根据机械臂实际位置和目标位置计算得到关节参考速度；根据关节参考速度和关节避障速度叠加得到综合速度；机械臂根据综合速度达到目标位置。
[0079]
具体为，机械臂各关节位置q通过机械臂正向运动学fkine(q)得到机械臂当前状态实际位置x
fdb
。
[0080]
机械臂当前状态实际位置x
fdb
和目标位置x
goal
得到目标位置与实际位置的位置偏差x
err
。目标位置与实际位置的位置偏差x
err
根据位置偏差补偿速度的增益矩阵k得到位置偏差补偿速度v
err
。
[0081]
根据清洁目标位置x
goal
和机械臂当前状态实际位置x
fdb
计算位置偏差补偿速度v
err
：
[0082][0083]
其中，k表示位置偏差补偿速度的增益矩阵；x
goal
表示清洁路径上的目标位置；d
goal
表示目标位置与机械臂末端当前位置的距离；δ表示阈值。
[0084]
x
fdb
为机械臂当前状态实际位置；目标位置x
goal
可由视觉模块通过三维点云匹配与图像分割技术获得，机械臂当前位置可由正向运动学根据机械臂模块反馈的关节位置计算得到。并且为保证机械臂运动过程中的平顺与精确性，在计算位置偏差补偿速度时也考虑到了目标位置与机械臂末端当前位置的距离d
goal
与其阈值δ之间的关系，当距离过大时，位置偏差补偿速度为零，当距离较近，即快接近目标位姿时，根据上述公式得到补偿速度。
[0085]
根据清洁路径生成的目标线速度v
goal
和位置偏差补偿速度v
err
计算机械臂笛卡尔空间内的期望线速度v
ref
：
[0086]vref
＝v
goal
v
err
[0087]
其中，v
err
表示由位置偏差引起的位置偏差补偿速度。
[0088]
根据避障补偿速度v
obstacle
和目标线速度v
goal
计算关节避障速度
[0089][0090]
其中，v
obstacle
表示根据周围环境生成的避障补偿速度；α0表示避障补偿速度增益；j0表示机械臂离障碍物预定距离处的线速度雅克比矩阵(j0为机械臂离障碍物最近处的线速度雅克比矩阵)；je表示机械臂清洁工具末端线速度雅克比矩阵；αn表示避障补偿速度与
末端线速度混合增益；i表示单位矩阵；表示机械臂清洁工具末端线速度雅克比矩阵的伪逆矩阵；表示机械臂清洁工具末端线速度雅克比矩阵的逆矩阵。
[0091]
根据机械臂笛卡尔空间内的期望线速度vref计算关节参考速度
[0092][0093]
根据关节参考速度和关节避障速度叠加得到机械臂各关节速度
[0094]
关节参考速度和关节避障速度叠加得到综合速度
[0095][0096]
机械臂各关节速度速度经过积分功能∫。得到机械臂各关节位置q。
[0097]
对于α0：
[0098][0099]
其中，d为机械臂连杆模型各端点与障碍物的预定距离，d
ug
表示产生单位避障补偿速度的距离阈值。增益α0与机械臂连杆模型各端点与障碍物的最近距离d负相关，当d越小时，增益α0越大，当d大于阈值d
ug
时，增益α0为零。
[0100]
对于αn：
[0101][0102]
其中，d
soi
为障碍物不影响机械臂运动的临界距离。当机械臂连杆模型各端点与障碍物的最近距离d超过该临界距离时d
soi
，增益αn为零，当d小于阈值d
ug
时，增益αn为1，当d在两者区间范围内时，增益αn与距离d负相关。
[0103]
实时求解期望线速度v
ref
，清洁工具末端线速度雅克比矩阵je，机械臂离障碍物最近处的线速度雅克比矩阵j0，再设置合适的增益k、α0、αn，经过矩阵运算与积分，得到机械臂各关节速度与位置。
[0104]
清洁路径都是基于如多个直线段，圆弧，椭圆弧，样条曲线组合在一起形成清洁路径(笛卡尔空间坐标轨迹规划)，通过笛卡尔空间坐标轨迹规划方法实时求得目标线速度v
goal
。根据机械臂的结构将机械臂简化为连杆模型，通过机械臂正向运动学实时求解得到各连杆两端点的实际位置，进一步求解得到改点处的线速度雅克比矩阵。通过对增益α0、αn进行设置，可以保证满足避障要求的关节速度的平滑。
[0105]
根据智能规划与控制算法框架计算出满足位置约束的关节速度与满足避障要求的关节速度，再根据机械臂实际位姿与期望位姿，不断迭代求解，规划出运动轨迹并控制机械臂到达目标位姿。
[0106]
根据目标位置和避障位置计算得到关节避障速度，根据实际位置和目标位置计算得到机械臂笛卡尔空间内的期望线速度v
ref
＝v
goal
v
err
；关节参考速度和关节避障速度叠加得到综合速度机械臂根据综合速度安全达到目标位置。
[0107]
本发明实施例还公开了一种清洁用机械臂规划控制方法，应用机械臂模块，包括如下步骤：
[0108]
清洁工具运动步骤：所述机械臂控制器根据被清洁物体表面制定清洁工具的清洁路径，并通过机械臂和夹爪控制清洁工具按照清洁路径运动。
[0109]
机械臂运动步骤：所述机械臂控制器根据清洁路径、运动在清洁路径中的清洁工具、机械臂当前状态以及机械臂周围环境为机械臂实时制定避障路径，并在清洁工具运动时控制机械臂实时按照避障路径运动。
[0110]
根据机械臂目标位置和避障位置计算得到关节避障速度；根据机械臂实际位置和目标位置计算得到关节参考速度；根据关节参考速度和关节避障速度叠加得到综合速度；机械臂根据综合速度达到目标位置。
[0111]
视觉信息获取步骤：被清洁物体表面以及机械臂周围环境的点云数据，进而获取目标特征点。
[0112]
本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统、装置及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的系统、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。
[0113]
以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本技术的实施例和实施例中的特征可以任意相互组合。