机械臂运动路径生成方法和系统与流程

本发明涉及自动化控制技术领域，特别是涉及一种机械臂运动路径生成方法和系统。

背景技术：

如图1所示，在机械臂运动空间中存在周边设备及机械臂自身机座等障碍物，当控制机械臂从A点运动到B点时，需要对机械臂的运动路径进行规划，使机械臂要在运动过程中不与障碍物发生碰撞。

传统的机械臂路径规划方式主要使用人工示教中间点法，如下图2所示，即在机器人运动轨迹中分别手工示教点T1、T2、T3等机械臂运行的中间点，以在从起点A到达终点B的过程中避开障碍物。此种方式需要手工示教各个轨迹中间点，更换机械臂后要重新示教每个中间点，路径生成效率低。

技术实现要素：

基于此，有必要针对路径生成效率低的问题，提供一种机械臂运动路径生成方法和系统。

一种机械臂运动路径生成方法，包括以下步骤：

获取机械臂周围的障碍物的分布位置，根据所述分布位置计算机械臂运动的安全半径，根据所述安全半径和机械臂的工具爪的尺寸计算机械臂的实际工作半径；

根据所述工具爪的起始位置和所述实际工作半径计算机械臂运动的第一中间点位置，根据所述工具爪的目标位置和所述实际工作半径计算机械臂运动的第二中间点位置；

若所述工具爪的目标位置在所述机械臂当前姿态的运动范围内，根据所述起始位置、第一中间点位置、第二中间点位置和目标位置生成机械臂的运动路径。

一种机械臂运动路径生成系统，包括：

第一计算模块，用于获取机械臂周围的障碍物的分布位置，根据所述分布位置计算机械臂运动的安全半径，根据所述安全半径和机械臂的工具爪的尺寸计算机械臂的实际工作半径；

第二计算模块，用于根据所述工具爪的起始位置和所述实际工作半径计算机械臂运动的第一中间点位置，根据所述工具爪的目标位置和所述实际工作半径计算机械臂运动的第二中间点位置；

第一生成模块，用于若所述工具爪的目标位置在所述机械臂当前姿态的运动范围内，根据所述起始位置、第一中间点位置、第二中间点位置和目标位置生成机械臂的运动路径。

上述机械臂运动路径生成方法和系统，根据障碍物的分布位置计算机械臂运动的安全半径，根据所述安全半径和机械臂的工具爪的尺寸计算机械臂的实际工作半径，根据所述工具爪的起始位置和所述实际工作半径计算机械臂运动的第一中间点位置，根据所述工具爪的目标位置和所述实际工作半径计算机械臂运动的第二中间点位置，并自动生成机械臂的运动路径，当障碍物分布改变时，只需输入更改后的障碍物的分布位置即可自动生成新的机械臂运动路径，无需人工示教各个轨迹中间点，规划效率高。

附图说明

图1为一个实施例的机械臂及障碍物分布示意图；

图2为一个实施例的人工示教运动路径中间点的示意图；

图3为一个实施例的机械臂运动路径生成方法流程图；

图4为一个实施例的安全半径与实际工作半径示意图；

图5为一个实施例的中间点和换手区域示意图；

图6为第一实施例的机械臂运动路径示意图；

图7为第二实施例的机械臂运动路径示意图；

图8为一个实施例的机械臂运动路径生成系统的结构示意图。

具体实施方式

下面结合附图对本发明的技术方案进行说明。

如图1所示，本发明提供一种机械臂运动路径生成方法，可包括以下步骤：

S1，获取机械臂周围的障碍物的分布位置，根据所述分布位置计算机械臂运动的安全半径，根据所述安全半径和机械臂的工具爪的尺寸计算机械臂的实际工作半径；

在一个实施例中，可以建立机械臂及障碍物分布的坐标系，优选地，所述坐标系为三维坐标系。所述三维坐标系可以将机械臂底座的中心点设为原点，将水平面上两个相互垂直的方向设为x轴和y轴，将竖直方向设为z轴。通过建立坐标系，可以更加方便地对机械臂位置及障碍物分布进行描述。在所述三维坐标系下，所述障碍物的分布位置可以用坐标的形式表示出来，例如，可以建立一个坐标集合，该坐标集合中可以包括多个坐标值，每个坐标值代表一个障碍物的位置分布，该坐标值可表示为(x，y，z)的形式。为了便于区分各个障碍物，还可以将各个障碍物进行编号，并将障碍物的坐标值与对应编号进行绑定。

所述安全半径与实际工作半径如图4所示。图中，R_max和R_min分别表示安全半径的最大值和最小值，WR_max和WR_min分别表示实际工作半径的最大值和最小值，A和B分别表示起始位置和目标位置。所述安全半径的最大值可以根据障碍物分布来计算，可根据障碍物的分布位置获取机械臂周围的障碍物与机械臂底座的中心点之间的距离，根据所述距离的最小值计算机械臂运动的安全半径。在一个实施例中，可以将所述距离的最小值r_min设为所述安全半径的最大值，在另一个实施例中，也可以将一个小于r_min的数值设为所述安全半径的最大值。所述安全半径的最小值可以根据机械臂底座的尺寸来计算。在一个实施例中，可以将所述机械臂底座各点与机械臂底座中心点的距离的最大值r_max设为所述安全半径的最小值，在另一个实施例中，也可以将一个大于r_max的数值设为所述安全半径的最小值。由于机械臂的工具爪有一定尺寸，因此，机械臂的实际工作半径可根据以下方式计算：

WR_max＝R_max-d；

WR_min＝R_min+d；

其中，d为所述机械臂的工具爪的尺寸。

S2，根据所述工具爪的起始位置和所述实际工作半径计算机械臂运动的第一中间点位置，根据所述工具爪的目标位置和所述实际工作半径计算机械臂运动的第二中间点位置；

在一个实施例中，计算机械臂运动的第一中间点位置时，可以获取第一直线与所述实际工作半径对应的圆形区域的第一交点；其中，所述第一直线是所述起始位置与所述机械臂的底座的中心点所连成的直线；将其中一个第一交点所在的位置设为所述中间点位置。优选地，所述第一交点可以是所述第一直线与所述实际工作半径对应的圆的交点，如图5所示。在图5中，A’为所述交点，也即所述中间点。计算机械臂运动的第二中间点位置时，可以获取第二直线与所述实际工作半径对应的圆形区域的第二交点；其中，所述第二直线是所述目标位置与所述机械臂的底座的中心点所连成的直线；将其中一个第二交点所在的位置设为所述中间点位置。优选地，所述第二交点可以是所述第二直线与所述实际工作半径对应的圆的交点，如图5所示。在图5中，B’为所述交点，也即所述中间点。如果建立了三维坐标系，可以获取所述第一中间点和第二中间点在所述三维坐标系中的坐标，并保存。

在一个实施例中，由于机械臂本身的机械参数对机械臂的限制，如果机械臂保持当前姿态可能无法运动到目标位置，此时，在机械臂从起始位置运动到目标位置的过程中需要更换左右手姿态。机械臂更换左右手姿态时，需要将整个机械臂完全伸直，需要的空间最大，换手点处理不好即会撞机。因此，除了所述第一中间点位置和第二中间点位置之外，可能还需要在机械臂的运动路径中设置换手点位置。所述换手点位置可以设置在换手区域内，换手区域即障碍物与机械臂底座的中心点之间的距离大于所述机械臂的最大长度的区域。即，若所述工具爪的目标位置不在所述机械臂当前姿态的运动范围内，可在换手区域中选取至少一点作为换手点，获取换手点位置；其中，所述换手区域是障碍物与机械臂底座的中心点之间的距离大于所述机械臂的最大长度的区域。所述最大长度即机械臂完全伸直时的长度。换手区域如图5所示，图中的C1和C2为换手点，在实际情况下，换手点的数量也可以是1或者其他数量。其中，在判断机械臂保持当前姿态是否可以到达目标位置时，可读取机械臂的机械参数，并根据所述机械参数计算机械臂当前姿态能够到达的区域，若目标位置不在该区域内，则判定需要切换左右手姿态。

S3，若所述工具爪的目标位置在所述机械臂当前姿态的运动范围内，根据所述起始位置、第一中间点位置、第二中间点位置和目标位置生成机械臂的运动路径。

假设所述工具爪的目标位置在所述机械臂当前姿态的运动范围内，即不需要切换左右手姿态，可直接根据所述起始位置、第一中间点位置、第二中间点位置和目标位置生成机械臂的运动路径。假设所述工具爪的目标位置不在所述机械臂当前姿态的运动范围内，即需要切换左右手姿态，可根据所述起始位置、第一中间点位置和步骤S2中计算出的换手点位置生成机械臂当前姿态的运动路径，并根据所述换手点位置、第二中间点位置和目标位置生成机械臂切换姿态后的运动路径。

在实际情况下，由于机械臂特殊结构，在起点时需要先将Z轴提升至安全Z轴的位置，在终点时再由安全Z轴降至终点Z轴位置，所有中间点都在安全Z轴上运行，即走一个“门”字型轨迹。安全Z轴的位置可以预先存储，在一般情况下，安全Z轴的位置无需经常变动。当需要更改机械臂的提升高度时，只需更改预存的安全Z轴的位置即可，无需人工调整机械臂高度。在这种情况下，需要在机械臂的运动路径中插入一个起始位置正上方的中间点。具体地，可以计算所述起始位置对应的第一安全高度位置；其中，所述第一安全高度位置是在所述起始位置正上方，且与所述起始位置的垂直高度大于或等于预设的高度值的位置；根据所述起始位置、第一安全高度位置、第一中间点位置、第二中间点位置和目标位置生成机械臂的运动路径。在这种情况下，还可以根据所述第一安全高度位置和所述障碍物分布计算机械臂运动的安全半径。此时，所述安全半径的最大值R_max可根据以下方式计算：

其中，h为安全Z轴的高度，l为障碍物与机械臂底座中心点的最大距离。

假设在起始位置时对机械臂进行了升高，如上所述，则在目标位置还需要将机械臂下降到原高度，此时，可以在机械臂运动的目标位置的正上方插入一个中间点。具体地，可以计算所述目标位置对应的第二安全高度位置；其中，所述第二安全高度位置是在所述目标位置正上方，且与所述目标位置的垂直高度大于或等于所述预设的高度值的位置；根据所述起始位置、第一安全高度位置、第一中间点位置、第二安全高度位置、第二中间点位置和目标位置生成机械臂的运动路径。

在生成机械臂的运动路径之后，可以根据所述运动路径控制机械臂运动，使机械臂自动沿着所述运动路径从所述起始点位置运动到所述目标点位置。当障碍物分布改变时，只需重新获取障碍物分布，即可自动重新规划出机械臂的运动路径。当机械臂更换时，只需在系统中重新输入机械臂的机械参数(如工具爪的尺寸)，即可自动重新规划出机械臂的运动路径。无需人工示教，操作简单，效率高。

机械臂的实际运动路径如图6和图7所示。图6为一个实施例的未加入竖直方向上的中间点时的路径示意图，图7为一个实施例的加入竖直方向上的中间点时的路径示意图。图中虚线表示机械臂的运动路径。

本发明具有以下优点：

(1)操作简单快速，不需要手动示教。

(2)智能插入需要的中间点，运行效率较高。

(3)自动管理Z轴规划，调用者只需要输入起点及终点信息。

(4)智能判断是否需要更换左右手姿态，运行效率较高。

(5)适应多种障碍物布局，环境变化后只需要更新环境信息即可立即使用。

如图8所示，本发明提供一种机械臂运动路径生成系统，可包括：

第一计算模块10，用于获取机械臂周围的障碍物的分布位置，根据所述分布位置计算机械臂运动的安全半径，根据所述安全半径和机械臂的工具爪的尺寸计算机械臂的实际工作半径；

在一个实施例中，可以建立机械臂及障碍物分布的坐标系，优选地，所述坐标系为三维坐标系。所述三维坐标系可以将机械臂底座的中心点设为原点，将水平面上两个相互垂直的方向设为x轴和y轴，将竖直方向设为z轴。通过建立坐标系，可以更加方便地对机械臂位置及障碍物分布进行描述。在所述三维坐标系下，所述障碍物的分布位置可以用坐标的形式表示出来，例如，可以建立一个坐标集合，该坐标集合中可以包括多个坐标值，每个坐标值代表一个障碍物的位置分布，该坐标值可表示为(x，y，z)的形式。为了便于区分各个障碍物，还可以将各个障碍物进行编号，并将障碍物的坐标值与对应编号进行绑定。

所述安全半径与实际工作半径如图4所示。图中，R_max和R_min分别表示安全半径的最大值和最小值，WR_max和WR_min分别表示实际工作半径的最大值和最小值。所述安全半径的最大值可以根据障碍物分布来计算，可根据障碍物的分布位置获取机械臂周围的障碍物与机械臂底座的中心点之间的距离，根据所述距离的最小值计算机械臂运动的安全半径。在一个实施例中，可以将所述距离的最小值r_min设为所述安全半径的最大值，在另一个实施例中，也可以将一个小于r_min的数值设为所述安全半径的最大值。所述安全半径的最小值可以根据机械臂底座的尺寸来计算。在一个实施例中，可以将所述机械臂底座各点与机械臂底座中心点的距离的最大值r_max设为所述安全半径的最小值，在另一个实施例中，也可以将一个大于r_max的数值设为所述安全半径的最小值。由于机械臂的工具爪有一定尺寸，因此，机械臂的实际工作半径可根据以下方式计算：

WR_max＝R_max-d；

WR_min＝R_min+d；

其中，d为所述机械臂的工具爪的尺寸。

第二计算模块20，用于根据所述工具爪的起始位置和所述实际工作半径计算机械臂运动的第一中间点位置，根据所述工具爪的目标位置和所述实际工作半径计算机械臂运动的第二中间点位置；

在一个实施例中，计算机械臂运动的第一中间点位置时，可以获取第一直线与所述实际工作半径对应的圆形区域的第一交点；其中，所述第一直线是所述起始位置与所述机械臂的底座的中心点所连成的直线；将其中一个第一交点所在的位置设为所述中间点位置。优选地，所述第一交点可以是所述第一直线与所述实际工作半径对应的圆的交点，如图5所示。在图5中，A’为所述交点，也即所述中间点。计算机械臂运动的第二中间点位置时，可以获取第二直线与所述实际工作半径对应的圆形区域的第二交点；其中，所述第二直线是所述目标位置与所述机械臂的底座的中心点所连成的直线；将其中一个第二交点所在的位置设为所述中间点位置。优选地，所述第二交点可以是所述第二直线与所述实际工作半径对应的圆的交点，如图5所示。在图5中，B’为所述交点，也即所述中间点。如果建立了三维坐标系，可以获取所述第一中间点和第二中间点在所述三维坐标系中的坐标，并保存。

在一个实施例中，由于机械臂本身的机械参数对机械臂的限制，如果机械臂保持当前姿态可能无法运动到目标位置，此时，在机械臂从起始位置运动到目标位置的过程中需要更换左右手姿态。机械臂更换左右手姿态时，需要将整个机械臂完全伸直，需要的空间最大，换手点处理不好即会撞机。因此，除了所述第一中间点位置和第二中间点位置之外，可能还需要在机械臂的运动路径中设置换手点位置。所述换手点位置可以设置在换手区域内，换手区域即障碍物与机械臂底座的中心点之间的距离大于所述机械臂的最大长度的区域。即，可设置一获取模块，若所述工具爪的目标位置不在所述机械臂当前姿态的运动范围内，该获取模块可在换手区域中选取至少一点作为换手点，获取换手点位置；其中，所述换手区域是障碍物与机械臂底座的中心点之间的距离大于所述机械臂的最大长度的区域。所述最大长度即机械臂完全伸直时的长度。换手区域如图5所示。其中，在判断机械臂保持当前姿态是否可以到达目标位置时，可读取机械臂的机械参数，并根据所述机械参数计算机械臂当前姿态能够到达的区域，若目标位置不在该区域内，则判定需要切换左右手姿态。

第一生成模块30，用于若所述工具爪的目标位置在所述机械臂当前姿态的运动范围内，根据所述起始位置、第一中间点位置、第二中间点位置和目标位置生成机械臂的运动路径。

假设所述工具爪的目标位置在所述机械臂当前姿态的运动范围内，即不需要切换左右手姿态，可直接根据所述起始位置、第一中间点位置、第二中间点位置和目标位置生成机械臂的运动路径。假设所述工具爪的目标位置不在所述机械臂当前姿态的运动范围内，即需要切换左右手姿态，可设置第二生成模块，用于根据所述起始位置、第一中间点位置和第二计算模块20计算出的换手点位置生成机械臂当前姿态的运动路径，并根据所述换手点位置、第二中间点位置和目标位置生成机械臂切换姿态后的运动路径。

在实际情况下，由于机械臂特殊结构，在起点时需要先将Z轴提升至安全Z轴的位置，在终点时再由安全Z轴降至终点Z轴位置，所有中间点都在安全Z轴上运行，即走一个“门”字型轨迹。安全Z轴的位置可以预先存储，在一般情况下，安全Z轴的位置无需经常变动。当需要更改机械臂的提升高度时，只需更改预存的安全Z轴的位置即可，无需人工调整机械臂高度。在这种情况下，需要在机械臂的运动路径中插入一个起始位置正上方的中间点。具体地，可以计算所述起始位置对应的第一安全高度位置；其中，所述第一安全高度位置是在所述起始位置正上方，且与所述起始位置的垂直高度大于或等于预设的高度值的位置；根据所述起始位置、第一安全高度位置、第一中间点位置、第二中间点位置和目标位置生成机械臂的运动路径。在这种情况下，还可以根据所述第一安全高度位置和所述障碍物分布计算机械臂运动的安全半径。此时，所述安全半径的最大值R_max可根据以下方式计算：

其中，h为安全Z轴的高度，l为障碍物与机械臂底座中心点的最大距离。

假设在起始位置时对机械臂进行了升高，如上所述，则在目标位置还需要将机械臂下降到原高度，此时，可以在机械臂运动的目标位置的正上方插入一个中间点。具体地，可以计算所述目标位置对应的第二安全高度位置；其中，所述第二安全高度位置是在所述目标位置正上方，且与所述目标位置的垂直高度大于或等于所述预设的高度值的位置；根据所述起始位置、第一安全高度位置、第一中间点位置、第二安全高度位置、第二中间点位置和目标位置生成机械臂的运动路径。

在生成机械臂的运动路径之后，可以根据所述运动路径控制机械臂运动，使机械臂自动沿着所述运动路径从所述起始点位置运动到所述目标点位置。当障碍物分布改变时，只需重新获取障碍物分布，即可自动重新规划出机械臂的运动路径。当机械臂更换时，只需在系统中重新输入机械臂的机械参数(如工具爪的尺寸)，即可自动重新规划出机械臂的运动路径。无需人工示教，操作简单，效率高。

机械臂的实际运动路径如图6和图7所示。图6为一个实施例的未加入竖直方向上的中间点时的路径示意图，图7为一个实施例的加入竖直方向上的中间点时的路径示意图。图中虚线表示机械臂的运动路径。

本发明具有以下优点：

(1)操作简单快速，不需要手动示教。

(2)智能插入需要的中间点，运行效率较高。

(3)自动管理Z轴规划，调用者只需要输入起点及终点信息。

(4)智能判断是否需要更换左右手姿态，运行效率较高。

(5)适应多种障碍物布局，环境变化后只需要更新环境信息即可立即使用。

本发明的机械臂运动路径生成系统与本发明的机械臂运动路径生成方法一一对应，在上述机械臂运动路径生成方法的实施例阐述的技术特征及其有益效果均适用于机械臂运动路径生成系统的实施例中，特此声明。

以上所述实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。

本领域普通技术人员可以理解实现上述实施例方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成。所述的程序可以存储于一计算机可读取存储介质中。该程序在执行时，包括上述方法所述的步骤。所述的存储介质，包括：ROM/RAM、磁碟、光盘等。

以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明专利的保护范围应以所附权利要求为准。