CN113951761A - 清洁空间矩形区域的机械臂运动规划方法及*** - Google Patents

Abstract

本发明提供了一种清洁空间矩形区域的机械臂运动规划方法及***，涉及机器人技术领域，该方法包括：移动机器人模块、机械臂模块和视觉模块，各模块均具有独立工作性能，且各模块中分配有相对应的工装；移动机器人模块：移动机器人模块中配备激光雷达和驱动电机在内的通用性设备，保证移动机器人单独完成常见的SLAM、导航和运动规划在内的相关功能；机械臂模块：包括机械臂本体和夹爪，保证机械臂能够完成抓取和转移物体在内的相关操作；视觉模块：包括深度相机，能够提供目标平面与立体的点云数据，并根据这些数据获取目标特征点。本发明能够自适应的制定清洁区域，确保机械臂与外部结构不发生碰撞。

Description

清洁空间矩形区域的机械臂运动规划方法及***

技术领域

本发明涉及机器人技术领域，具体地，涉及一种清洁空间矩形区域的机械臂运动规划方法及***。

背景技术

目前市场上的清洁机器人主要由各种扫地机器人构成，但扫地机器人有着天生的缺陷，它们仅能够清洁地面，无法像人类一样清洁其他区域，因此集成移动机器人和通用机械臂两项功能的复合型机器人成为了后续清洁机器人的发展趋势。

公开号为CN113143123A的发明专利，公开了一种清洁机器人***及清洁控制方法，所述清洁机器人***包括清洁机器人和基站；所述清洁机器人包括机身、吸尘组件和旋转清洁组件；所述基站包括清洁装置；当所述清洁机器人处于拖地模式时，所述旋转清洁组件处于工作状态，所述旋转清洁组件用于拖地工作；当所述清洁机器人处于清扫模式时，所述吸尘组件和所述旋转清洁组件均处于工作状态，所述吸尘组件和所述旋转清洁组件共同用于吸尘工作；当所述清洁机器人返回所述基站时，所述清洁装置用于对所述旋转清洁组件进行清洁。

在复合型机器人进行清洁任务时，其面对的工作场景是复杂多变的，尤其是在清洁某些区域角落这些地方时，机械臂的避障问题就变得尤为重要了，不仅仅是机械臂末端与外部结构发生碰撞，也需要考虑机械臂连杆是否与外部结构发生碰撞。因此，在机械臂清洁空间矩形区域过程中，首先需要保证机械臂末端轨迹能够覆盖该矩形区域，其次需要确保在矩形区域边缘时机械臂不与外部结构发生碰撞。

发明内容

针对现有技术中的缺陷，本发明提供一种清洁空间矩形区域的机械臂运动规划方法及***。

根据本发明提供的一种清洁空间矩形区域的机械臂运动规划方法及***，所述方案如下：

第一方面，提供了一种清洁空间矩形区域的机械臂运动规划方法，所述方法包括：

步骤S1：移动机器人通过SLAM进行建图；

步骤S2：移动机器人运动至工作地点；

步骤S3：视觉模块识别矩形清洁区域顶点；

步骤S4：根据所述顶点进行机械臂运动规划；

步骤S5：根据运动规划结构进行机械臂轨迹插补。

优选的，所述步骤S1包括：手动控制复合型机器人在全地图中运动并遍历完全地图中的所有空间，根据激光雷达返回的数据能够建立移动机器人工作空间的平面地图，并在地图上的工作地点附近进行简单标记。

优选的，所述步骤S2包括：根据导航控制算法控制复合型机器人运动至工作点附近，当移动机器人运动至工作点后，发送相关指令至机械臂模块。

优选的，所述步骤S3包括：复合型机器人运动至工作点后视觉模块便能够获取矩形清洁区域的图像信息，利用三维点云匹配技术获取该矩形清洁区域的四个顶点，并将上述顶点发送至机械臂模块。

优选的，所述步骤S4包括：矩形清洁区域的运动规划内容分为两大部分，首选需根据清洁工具尺寸重新确定清洁区域点位，再根据清洁区域尺寸与清洁工具尺寸选取合适的点位顺序，并根据点位顺序确定当前位置的姿态。

优选的，所述步骤S4具体包括：

步骤S4.1：定位清洁区域顶点位置；

通常情况下近似将清洁工具与清洁区域的接触面认为为矩形，先预设清洁接触面矩形的长和宽分别为l和w，新确认的清洁区域顶点位置均满足其离清洁区域长边距离为l/2，离清洁区域短边距离为w/2；

其中矩形区域的坐标系原点位于点A，其x轴由A点指向D点，y轴由A点指向B点，故新清洁区域的四个顶点E、F、G、H在世界坐标系中的坐标由下式计算得到：

点E：

点F：

点G：

点H：

其中，O表示世界坐标系原点；

表示清洁区域坐标系X轴方向；

表示清洁区域坐标系Y轴方向；

所求得的新矩形清洁区域的长宽分别为lh和wh。

优选的，所述步骤S4还包括：

步骤S4.2：确定清洁路径点顺序；

机械臂末端沿着新矩形清洁区域的长边运动，清洁路径的中间过渡点在新矩形清洁区域的短边内选取；首先确定单条边上的中间点数量：

n＝ceil(wh/w)+1

其中，ceil函数为向上取整的函数；

进一步求得EH和FG边上的所有中间点：

EH边：

FG边：

再依次连接所有中间点；

其中，i表示EH或FG中间点的顺序；

表示EH中间点或FG中间点在世界坐标系下的向量。

优选的，所述步骤S4还包括：

步骤S4.3：确定各清洁路径中间点的姿态；

建立EFGH矩形为底，高为h的方形盒子，方形盒子上面的矩形分别对应E’F’G’H’，通常情况下，选取E’F’G’H’矩形上距离机械臂基座最近的一条边上的中点为基准点M’，机械臂末端z轴指向为基准点M’指向各路径中间点，连接基准点M’与EF的中点M，定义机械臂末端x轴的指向垂直于M M’H三点所组成的平面；

再次利用叉乘得到机械臂末端y轴的指向。

优选的，所述步骤S5包括：获取清洁路径中间点的笛卡尔位姿采取直线轨迹插补，在经过清洁路径中间位置时，机械臂末端速度为零。

第二方面，提供了一种清洁空间矩形区域的机械臂运动规划***，所述***包括：

移动机器人模块、机械臂模块和视觉模块，各模块均具有独立工作性能，且各模块中分配有相对应的工装；

移动机器人模块：移动机器人模块中配备激光雷达和驱动电机在内的通用性设备，保证移动机器人单独完成常见的SLAM、导航和运动规划在内的相关功能；

机械臂模块：包括机械臂本体和夹爪，保证机械臂能够完成抓取和转移物体在内的相关操作；

视觉模块：包括深度相机，能够提供目标平面与立体的点云数据，并根据这些数据获取目标特征点。

与现有技术相比，本发明具有如下的有益效果：

1、提高机器人工作适应性，能够根据不同的清洁工具尺寸，自适应的制定清洁区域；

2、提高工作效率，能够根据清洁区域的特征自适应制定清洁路径，提升机械臂运动过程中的平均速度；

3、提高机器人的智能性，能够避免机械臂在清洁区域边缘时与外部结构发生碰撞。

附图说明

通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：

图1为复合型机器人组成图；

图2为复合型机器人SLAM建图；

图3为新清洁区域顶点确定示意图；

图4为机械臂末端清洁路径示意图；

图5为机械臂末端姿态确定示意图。

具体实施方式

下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。

本发明实施例提供了一种清洁空间矩形区域的机械臂运动规划***，参照图1所示，该***包括：移动机器人模块、机械臂模块和视觉模块，各模块均具有独立工作性能，且各模块中分配有相对应的工装。

移动机器人模块：移动机器人模块中配备激光雷达和驱动电机在内的通用性设备，能够保证移动机器人单独完成常见的SLAM，导航，运动规划等功能。

机械臂模块：除了机械臂本体外，还包含有夹爪，保证机械臂能够完成抓取、转移物体等操作。

视觉模块：主要由深度相机构成，能够提供目标平面与立体的点云数据，并能够根据这些数据获取目标特征点。

本发明还提供了一种清洁空间矩形区域的机械臂运动规划方法，该方法具体包括如下步骤：

步骤S1：移动机器人通过SLAM进行建图；

参照图2所示，手动控制复合型机器人在全地图中运动并遍历完全地图中的所有空间，根据激光雷达返回的数据能够建立移动机器人工作空间的平面地图，并在地图上的工作地点附近进行简单标记。

步骤S2：移动机器人运动至工作地点；

根据导航控制算法控制复合型机器人运动至工作点附近，本发明对导航控制算法不做特别说明，行业类常用的A*算法，D*算法以及人工势场法等均可以用于控制本发明中涉及到的复合型机器人。当移动机器人运动至工作点后，发送相关指令至机械臂模块。

步骤S3：视觉模块识别矩形清洁区域顶点；

复合型机器人运动至工作点后视觉模块便能够获取矩形清洁区域的图像信息，利用三维点云匹配技术获取该矩形清洁区域的四个顶点，并将上述顶点发送至机械臂模块。

步骤S4：根据所述顶点进行机械臂运动规划；

矩形清洁区域的运动规划内容分为两大部分，首选需根据清洁工具尺寸重新确定清洁区域点位，再根据清洁区域尺寸与清洁工具尺寸选取合适的点位顺序，并根据点位顺序确定当前位置的姿态。

1)定位清洁区域顶点位置：

通常情况下可近似将清洁工具与清洁区域的接触面认为为矩形，因此为保证清洁面积覆盖至清洁区域的角落里面，先预设清洁接触面矩形的长和宽分别为l和w，新确认的清洁区域顶点位置均满足其离清洁区域长边距离为l/2，离清洁区域短边距离为w/2，其示意图参照图3所示。其中矩形区域的坐标系原点位于点A，其x轴由A点指向D点，y轴由A点指向B点，故新清洁区域的四个顶点E、F、G、H在世界坐标系中的坐标由下式计算得到：

点E：

点F：

点G：

点H：

其中，O表示世界坐标系原点；

表示表示清洁区域坐标系X轴方向；

表示表示清洁区域坐标系Y轴方向；

所求得的新矩形清洁区域的长宽分别为lh和wh。

2)确定清洁路径点顺序：

为提高机械臂平均工作速度，一般而言，需让机械臂尽可能保持在较高的匀速阶段运动，减少加减速所耗费的时间。因此在本发明中，针对新的清洁区域，机械臂的运动规划应满足的条件是尽可能减少直线轨迹数量，实现单次直线路径尽可能长，因此机械臂末端沿着新矩形清洁区域的长边运动，清洁路径的中间过渡点在新矩形清洁区域的短边内选取。首先确定单条边上的中间点数量：

n＝ceil(wh/w)+1

其中，ceil函数为向上取整的函数；

进一步可求得EH和FG边上的所有中间点：

EH边：

FG边：

其中，i表示EH或FG中间点的顺序；

表示EH中间点或FG中间点在世界坐标系下的向量。

再依次连接所有中间点即可，例如，以H点位起点，F点为终点依次连接所有中间点，其清洁路径如图4所示。

3)确定各清洁路径中间点的姿态：

为避免在清洁角落时发生机械臂连杆与外部结构发生碰撞，重点在于设定机械臂末端z轴的指向，本发明采用如下方法来确定机械臂末端姿态。

建立EFGH矩形为底，高为h的方形盒子，方形盒子上面的矩形分别对应E’F’G’H’，通常情况下，选取E’F’G’H’矩形上距离机械臂基座最近的一条边上的中点为基准点M’，机械臂末端z轴指向为基准点M’指向各路径中间点，如图5所示。连接基准点M’与EF的中点M，定义机械臂末端x轴的指向垂直于M M’H三点所组成的平面；

进一步的可再次利用叉乘得到机械臂末端y轴的指向，如图5所示。

步骤S5：根据运动规划结构进行机械臂轨迹插补。

根据上述步骤S4中获取的清洁路径中间点的笛卡尔位姿采取直线轨迹插补，在经过清洁路径中间位置时，机械臂末端速度为零。

本发明实施例提供了一种清洁空间矩形区域的机械臂运动规划方法及***，能够适应空间中的任意矩形区域，并且根据清洁工具的尺寸，自适应运动路径并且保证运动路径能够覆盖全部矩形区域，针对矩形边缘可能发生的机械臂碰撞问题，本发明能够自适应计算得到机械臂末端位姿，确保机械臂与外部结构不发生碰撞。

本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的***及其各个装置、模块、单元以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的***及其各个装置、模块、单元以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同功能。所以，本发明提供的***及其各项装置、模块、单元可以被认为是一种硬件部件，而对其内包括的用于实现各种功能的装置、模块、单元也可以视为硬件部件内的结构；也可以将用于实现各种功能的装置、模块、单元视为既可以是实现方法的软件模块又可以是硬件部件内的结构。

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。

Claims (10)

1.一种清洁空间矩形区域的机械臂运动规划方法，其特征在于，包括：

步骤S1：移动机器人通过SLAM进行建图；

步骤S2：移动机器人运动至工作地点；

步骤S3：视觉模块识别矩形清洁区域顶点；

步骤S4：根据所述顶点进行机械臂运动规划；

步骤S5：根据运动规划结构进行机械臂轨迹插补。

2.根据权利要求1所述的清洁空间矩形区域的机械臂运动规划方法，其特征在于，所述步骤S1包括：手动控制复合型机器人在全地图中运动并遍历完全地图中的所有空间，根据激光雷达返回的数据能够建立移动机器人工作空间的平面地图，并在地图上的工作地点附近进行简单标记。

3.根据权利要求1所述的清洁空间矩形区域的机械臂运动规划方法，其特征在于，所述步骤S2包括：根据导航控制算法控制复合型机器人运动至工作点附近，当移动机器人运动至工作点后，发送相关指令至机械臂模块。

4.根据权利要求1所述的清洁空间矩形区域的机械臂运动规划方法，其特征在于，所述步骤S3包括：复合型机器人运动至工作点后视觉模块便能够获取矩形清洁区域的图像信息，利用三维点云匹配技术获取该矩形清洁区域的四个顶点，并将上述顶点发送至机械臂模块。

5.根据权利要求1所述的清洁空间矩形区域的机械臂运动规划方法，其特征在于，所述步骤S4包括：矩形清洁区域的运动规划内容分为两大部分，首选需根据清洁工具尺寸重新确定清洁区域点位，再根据清洁区域尺寸与清洁工具尺寸选取合适的点位顺序，并根据点位顺序确定当前位置的姿态。

6.根据权利要求5所述的清洁空间矩形区域的机械臂运动规划方法，其特征在于，所述步骤S4具体包括：

步骤S4.1：定位清洁区域顶点位置；

通常情况下近似将清洁工具与清洁区域的接触面认为为矩形，先预设清洁接触面矩形的长和宽分别为l和w，新确认的清洁区域顶点位置均满足其离清洁区域长边距离为l/2，离清洁区域短边距离为w/2；

其中矩形区域的坐标系原点位于点A，其x轴由A点指向D点，y轴由A点指向B点，故新清洁区域的四个顶点E、F、G、H在世界坐标系中的坐标由下式计算得到：

点E：

点F：

点G：

点H：

其中，O表示世界坐标系原点；

表示清洁区域坐标系X轴方向；

表示清洁区域坐标系Y轴方向；

所求得的新矩形清洁区域的长宽分别为lh和wh。

7.根据权利要求6所述的清洁空间矩形区域的机械臂运动规划方法，其特征在于，所述步骤S4还包括：

步骤S4.2：确定清洁路径点顺序；

机械臂末端沿着新矩形清洁区域的长边运动，清洁路径的中间过渡点在新矩形清洁区域的短边内选取；首先确定单条边上的中间点数量：

n＝ceil(wh/w)+1

其中，ceil函数为向上取整的函数；

进一步求得EH和FG边上的所有中间点：

EH边：

FG边：

再依次连接所有中间点；

其中，i表示EH或FG中间点的顺序；

表示EH中间点或FG中间点在世界坐标系下的向量。

8.根据权利要求7所述的清洁空间矩形区域的机械臂运动规划方法，其特征在于，所述步骤S4还包括：

步骤S4.3：确定各清洁路径中间点的姿态；

建立EFGH矩形为底，高为h的方形盒子，方形盒子上面的矩形分别对应E’F’G’H’，通常情况下，选取E’F’G’H’矩形上距离机械臂基座最近的一条边上的中点为基准点M’，机械臂末端z轴指向为基准点M’指向各路径中间点，连接基准点M’与EF的中点M，定义机械臂末端x轴的指向垂直于M M’H三点所组成的平面；

再次利用叉乘得到机械臂末端y轴的指向。

9.根据权利要求1所述的清洁空间矩形区域的机械臂运动规划方法，其特征在于，所述步骤S5包括：获取清洁路径中间点的笛卡尔位姿采取直线轨迹插补，在经过清洁路径中间位置时，机械臂末端速度为零。

10.一种清洁空间矩形区域的机械臂运动规划***，其特征在于，包括：移动机器人模块、机械臂模块和视觉模块，各模块均具有独立工作性能，且各模块中分配有相对应的工装；

移动机器人模块：移动机器人模块中配备激光雷达和驱动电机在内的通用性设备，保证移动机器人单独完成常见的SLAM、导航和运动规划在内的相关功能；

机械臂模块：包括机械臂本体和夹爪，保证机械臂能够完成抓取和转移物体在内的相关操作；

视觉模块：包括深度相机，能够提供目标平面与立体的点云数据，并根据这些数据获取目标特征点。

Images