CN117340890A - 一种机器人运动轨迹控制方法 - Google Patents

Abstract

本发明公开了一种机器人运动轨迹控制方法，包括：步骤1：对工作环境进行三维建模；步骤2：为每个机器人生成可行的轨迹，以确保可以协同工作而不发生碰撞；步骤3：将每个机器人的路径转化为轨迹，以便实际控制机器人的运动；步骤4：控制机器人按照生成的轨迹移动；步骤5：在每个时刻，需要进行碰撞检测，以确保多轴机器人之间不会发生碰撞；步骤6：实时环境中执行轨迹控制，不断监测机器人的状态和环境变化，并根据需要对机器人的轨迹和控制进行调整，以应对不断变化的工作环境。本方法充分考虑多轴机器人之间的碰撞和协调问题，通过实时的碰撞检测和避免机制，确保机器人在协同工作时不会发生碰撞，从而提高了工作效率和安全性。

Description

一种机器人运动轨迹控制方法

技术领域

本发明涉及机器人运动技术领域，特别涉及一种机器人运动轨迹控制方法。

背景技术

机器人运动轨迹控制是指对机器人执行器(例如关节或轮子)的运动进行精确控制，以使机器人能够按照预定的轨迹或路径执行任务。这种控制通常需要考虑机器人的动力学、运动规划、传感器反馈等多个因素，以确保机器人能够安全、高效地完成任务。机器人运动轨迹控制领域目前面临一些比较前沿的技术问题和挑战，这些挑战涉及到提高控制的精确性、适应性和鲁棒性，以应对复杂和多样的应用场景，例如：在工业生产中，多轴机器人协同工作的需求越来越普遍，解决多轴机器人之间的碰撞和协调问题，以实现高效的协同控制，是一个重要挑战。

发明内容

为了解决以上问题，本发明提供了一种机器人运动轨迹控制方法。

为实现上述目的，本发明所采用的技术方案如下：

一种机器人运动轨迹控制方法，包括如下步骤：

步骤1：对工作环境进行三维建模，包括所有参与协同工作的多轴机器人、工件、障碍物和工作空间的边界；

步骤2：为每个机器人生成可行的轨迹，以确保可以协同工作而不发生碰撞；

步骤3：将每个机器人的路径转化为轨迹，以便实际控制机器人的运动；

步骤4：控制机器人按照生成的轨迹移动；

步骤5：在每个时刻，需要进行碰撞检测，以确保多轴机器人之间不会发生碰撞；

步骤6：实时环境中执行轨迹控制，不断监测机器人的状态和环境变化，并根据需要对机器人的轨迹和控制进行调整，以应对不断变化的工作环境。

进一步的：所述步骤1包括：

使用传感器获取环境的三维数据点；

对从传感器获取的数据进行预处理，以确保数据的准确性和一致性；

将预处理后的数据表示为一个三维点云，其中每个点包含三维坐标(x，y，z)；

根据点云数据，定义工作空间W的边界和限制，以确保多轴机器人在这个环境中能够协同工作，工作空间W的边界和限制，使用以下公式表示：

W＝((x，y，z)|x_min≤x≤x_max，y_min≤y≤y_max，z_min≤z≤z_max)

其中：(x，y，z)表示三维坐标点，x_min、x_max、y_min、y_max、z_min、z_max分别表示工作空间的最小和最大边界坐标。

进一步的：所述步骤2包括：

随机选择一个初始位置q_init在工作空间W内，并将其作为RRT树的根节点，初始化一个空的RRT树，将q_init添加为树的唯一节点。

在每次迭代中，随机选择一个目标点q_rand在工作空间WW内，然后，从RRT树中找到最近的节点q_nearest，即使得q_nearest到q_rand的距离最小；

计算从q_nearest到q_rand的连接，并确保这条连接是可行的；

如果连接是可行的，将q_rand添加到RRT树中，作为新的节点，并以q_nearest为父节点，从而扩展树；

连接表示为以下公式：

q_new＝Steer(q_nearest，q_rand)

其中Steer是函数，用于计算从q_nearest到q_rand的有效路径；

重复上述步骤，直到达到终止条件；

一旦RRT树达到终止条件，从初始位置q_init到目标位置q_goal提取路径；

通过回溯RRT树，q_goal向树根q_init倒退，并收集连接节点的路径。

进一步的：所述步骤3包括：

将从得到的路径分为若干个线性段，线性段为两个节点之间的直线路径；

对于每个线性路径段，使用三次样条插值来生成平滑的轨迹；

三次样条插值表示为三次多项式函数，每个线性段有一个对应的三次多项式，三次多项式的一般形式如下：

P(t)＝a+bt+ct²+dt³

其中，P(t)是路径上的位置，t是时间或参数，而a，b，c，和d是待定系数；

使用计算出的插值系数，在每个线性段上生成轨迹点，从而得到平滑的轨迹。

进一步的：所述步骤4包括：

在每个时刻，根据当前轨迹的时间参数，确定机器人在路径上的目标位置q_target；

计算当前位置q_current与目标位置q_target之间的误差，表示为e(t)：

e(t)＝q_current-q_target

使用PID控制算法来计算控制输入u(t)，PID控制器的公式如下：

其中：u(t)是控制输入，Kp是比例增益，用于根据当前误差来调整控制输入；Ki是积分增益，用于根据误差的积分来消除系统的稳态误差；Kd是微分增益，用于根据误差变化率来改善系统的响应速度；

将计算出的控制输入u(t)应用于机器人的执行器，以调整机器人的位置或速度，使其接近目标位置；

重复上述步骤，直到机器人到达路径的终点。

进一步的：所述步骤5包括：

在每个时刻，对机器人之间的碰撞进行检测；

对于两个机器人i和j之间的碰撞检测，使用距离阈值来检测是否存在碰撞，如果机器人之间的距离小于阈值d_threshold，则认为存在碰撞：

d_ij(t)＝||q_i(t)-q_j(t)||

其中：d_ij(t)是机器人ii和jj之间的距离，q_i(t)和q_j(t)分别是机器人i和j在时刻t的位置。

进一步的：所述步骤6包括：

持续监测机器人的状态和环境的变化；

根据监测到的数据，不断更新机器人的状态信息，包括位置、速度、姿态和周围环境的信息；

持续进行碰撞检测，以确保机器人之间不会发生碰撞；

根据实时环境变化和机器人状态的更新；

根据环境和机器人状态的变化，对控制输入进行调整，以适应新的情况；

循环执行上述步骤，以确保机器人能够适应不断变化的工作环境。

本发明与现有技术相比，所取得的技术进步在于：

本方法充分考虑多轴机器人之间的碰撞和协调问题，通过实时的碰撞检测和避免机制，确保机器人在协同工作时不会发生碰撞，从而提高了工作效率和安全性。

本方法中包括实时控制与调整步骤，允许机器人根据实时环境变化动态调整轨迹和控制输入，这增加了系统的适应性和鲁棒性，使机器人能够应对不断变化的工作环境，通过三维空间建模，方法考虑了工作空间内的多轴机器人、工件和障碍物的准确表示，从而更准确地规划轨迹，避免碰撞，并确保高效的协同控制。

本方法采用三次样条插值来生成平滑的轨迹，以确保机器人在关节空间上的运动是可行的，有助于提高轨迹的可执行性和机器人的运动质量，使用PID控制器来实现轨迹跟踪，使机器人能够准确地遵循生成的轨迹，PID控制器具有广泛的应用和成熟的理论基础。

本方法综合考虑了路径规划、轨迹生成、控制、碰撞检测和避免以及实时调整等多个关键步骤，提供了一个全面的解决方案，以应对多轴机器人协同控制的各种挑战，在解决多轴机器人协同控制问题方面具有高度的综合性和适应性，可以提高生产效率，降低碰撞风险，适应复杂和动态的工作环境，是一个有前景的解决方案。

附图说明

附图用来提供对本发明的进一步理解，并且构成说明书的一部分，与本发明的实施例一起用于解释本发明，并不构成对本发明的限制。

在附图中：

图1为本发明的流程图。

具体实施方式

下面这几个具体的实施例可以相互结合，对于相同或相似的概念或过程可能在某些实施例中不再赘述。下面将结合附图，对本发明的实施例进行描述。

如图1所示的，本发明公开了一种机器人运动轨迹控制方法，包括如下步骤：

步骤1：三维空间建模

在这个步骤中，对工作环境进行三维建模，包括所有参与协同工作的多轴机器人、工件、障碍物和工作空间的边界。这可以使用激光扫描仪、摄像头和其他传感器来获取环境地图。

步骤2：路径规划

在这个步骤中，使用路径规划算法为每个机器人生成可行的轨迹，以确保它们可以协同工作而不发生碰撞。

步骤3：协同轨迹生成

在这个步骤中，将每个机器人的路径转化为轨迹，以便实际控制机器人的运动。可以使用插值方法，如三次样条插值，将路径转化为平滑的轨迹。

步骤4：轨迹控制

在这个步骤中，使用控制算法来控制机器人的运动，以按照生成的轨迹移动，以确保机器人按照预定轨迹运动。

步骤5：碰撞检测和避免

在每个时刻，进行碰撞检测，以确保多轴机器人之间不会发生碰撞，如果检测到潜在碰撞，需要采取措施来避免碰撞，如调整机器人的轨迹或速度。

步骤6：实时控制与调整

最后，在实时环境中执行轨迹控制，不断监测机器人的状态和环境变化，并根据需要对机器人的轨迹和控制进行调整，以应对不断变化的工作环境。

本方法可以在工业自动化中，尤其是在多轴机器人协同工作的情况下，提供高效的轨迹控制，在步骤5中的碰撞检测和避免机制可以确保机器人之间不会发生碰撞，从而提高了工作效率和安全性，此外，实时控制和调整使机器人能够在复杂和动态的工作环境中执行任务。

具体的，步骤1包括：

在步骤1中，三维空间建模是通过获取环境的传感器数据，并将其表示为一个三维坐标系的点云来实现的，以下是步骤1的具体实现：

使用激光扫描仪、摄像头或其他合适的传感器获取环境的三维数据点；

对从传感器获取的数据进行预处理，包括去噪、点云配准和坐标转换等，以确保数据的准确性和一致性；

将预处理后的数据表示为一个三维点云，其中每个点包含三维坐标(x,y,z)和可能的附加信息，如颜色或法向量；

根据点云数据，定义工作空间W的边界和限制，以确保多轴机器人在这个环境中能够协同工作，边界通常用几何体表示，如立方体或多边形；

使用数学公式来表示工作空间W的边界和限制，例如，对于一个立方体边界，可以使用以下公式表示：

W＝((x，y，z)|x_min≤x≤x_max，y_min≤y≤y_max，z_min≤z≤z_max)

其中：(x，y，z)表示三维坐标点，x_min、x_max、y_min、y_max、z_min、z_max分别表示工作空间的最小和最大边界坐标。

这个公式表示了工作空间W的边界，定义了多轴机器人协同工作的有效区域，建模后，此三维表示将用于后续路径规划和碰撞检测步骤，以确保机器人在工作环境中的安全和高效运动。

具体的，步骤2包括：

在步骤2中，使用Rapidly-exploring Random Trees(RRT)路径规划算法来生成机器人的路径，以确保它们可以协同工作而不发生碰撞，以下是具体实现步骤：

初始化RRT：

首先，随机选择一个初始位置q_init在工作空间W内，并将其作为RRT树的根节点，初始化一个空的RRT树，将q_init添加为树的唯一节点。

循环：

在每次迭代中，随机选择一个目标点在工作空间W内，然后，从RRT树中找到最近的节点q_nearest，即使得q_nearest到q_rand的距离最小。

扩展树：

计算从q_nearest到_qrand的连接的连接，并确保这条连接是可行的，即不会与障碍物碰撞，如果连接是可行的，将q_rand添加到RRT树中，作为新的节点，并以q_nearest为父节点，从而扩展树，这个连接表示为以下公式：

q_new＝Steer(q_nearest，q_rand)

其中Steer一个是函数，用于计算从q_nearest到q_rand的有效路径。

检查终止条件：

不断重复上述步骤，直到达到某个终止条件，可以是达到一定的树节点数、达到预定时间限制或其他条件。

路径提取：

一旦RRT树达到终止条件，可以从初始位置q_init到目标位置q_goal提取路径，可以通过回溯RRT树，从q_goal向树根q_init倒退，并收集连接节点的路径。

通过这个步骤，使用RRT路径规划算法成功生成了多轴机器人的路径，以确保它们可以在工作环境中协同工作而不发生碰撞，这个生成的路径将用于后续的轨迹生成和控制步骤，以实现多轴机器人的高效协同控制。

具体的，步骤3包括：

在步骤3中，使用三次样条插值方法将每个机器人的路径转化为平滑的轨迹。三次样条插值是一种常用的插值技术，它可以确保生成的轨迹在关节空间上是可行的，从而使机器人能够平滑地跟随路径，以下是具体的实现步骤：

路径分段：

首先，将从路径规划步骤得到的路径分为若干个线性段，线性段是两个节点之间的直线路径。

三次样条插值：

对于每个线性路径段，使用三次样条插值来生成平滑的轨迹，三次样条插值表示为三次多项式函数，每个线性段有一个对应的三次多项式。

三次多项式的一般形式如下：

P(t)＝a+bt+ct²+dt³

其中，P(t)是路径上的位置，t是时间或参数，而a，b，c，和d是待定系数。

计算插值系数：

为了确定三次多项式的系数a，b，c，和d，需要根据路径端点的位置和导数要求进行计算，这些条件通常包括起点和终点的位置，以及起点和终点的一阶和二阶导数(速度和加速度)，通过解线性方程组，可以求解出插值系数。

生成轨迹：

使用计算出的插值系数，可以在每个线性段上生成轨迹点，从而得到平滑的轨迹，轨迹可以根据需要进行时间缩放，以控制机器人的速度和加速度。

通过这个步骤，可以将从路径规划得到的路径转化为可行的、平滑的轨迹，以确保多轴机器人可以在关节空间上进行协同控制，跟随这些轨迹执行任务。

具体的，步骤4包括：

在步骤4中，使用PID(Proportional-Integral-Derivative)控制器作为控制算法，以控制机器人的运动，确保它按照生成的轨迹移动，以下是PID控制器的具体实现步骤：

设定目标点：

在每个时刻，根据当前轨迹的时间参数，确定机器人在路径上的目标位置q_target；

计算误差：

计算当前位置q_current与目标位置q_target之间的误差，表示为e(t)：

e(t)＝q_current-q_target

计算控制输入：

使用PID控制算法来计算控制输入u(t)，PID控制器的公式如下：

其中：u(t)是控制输入，K_p是比例增益，用于根据当前误差来调整控制输入；K_i是积分增益，用于根据误差的积分来消除系统的稳态误差；K_d是微分增益，用于根据误差变化率来改善系统的响应速度。

执行控制输入：

将计算出的控制输入u(t)应用于机器人的执行器，以调整机器人的位置或速度，使其接近目标位置。

循环：

不断重复上述步骤，直到机器人到达路径的终点或任务完成。

通过这个步骤，使用PID控制器将机器人的位置与生成的轨迹进行比较，并根据误差调整控制输入，以确保机器人按照预定轨迹进行运动，能够实现机器人的轨迹跟踪，从而实现高效协同控制。

具体的，步骤5包括：

在步骤5中，实现碰撞检测和避免以确保多轴机器人之间不会发生碰撞，以下是具体的实现步骤：

在每个时刻，对机器人之间以及机器人与障碍物之间的碰撞进行检测，可以使用几何碰撞检测方法或包围盒碰撞检测等方法来实现。

对于两个机器人i和j之间的碰撞检测，可以使用距离阈值来检测是否存在碰撞，如果机器人之间的距离小于阈值d_threshold，则认为存在碰撞。

d_ij(t)＝||q_i(t)-q_j(t)||

其中：d_ij(t)是机器人ii和jj之间的距离，q_i(t)和q_j(t)分别是机器人i和j在时刻t的位置。

碰撞避免：

如果检测到碰撞，需要采取措施来避免碰撞。这可以通过调整机器人的轨迹或速度来实现。

一种常见的方法是通过重新规划机器人的轨迹，以绕开碰撞障碍。这可以在路径规划步骤中实现，重新规划路径以绕开碰撞障碍物。

另一种方法是通过动态控制来避免碰撞。在轨迹控制步骤中，可以实时监测碰撞，并相应地调整机器人的速度和方向，以避免碰撞。

通过这个步骤，确保了多轴机器人之间不会发生碰撞，从而提高了工作的安全性和可行性，在检测到潜在碰撞时，采取适当的措施来避免碰撞，以确保机器人能够高效协同工作。

具体的，步骤6包括：

在步骤6中，需要实施实时控制与调整，以确保机器人能够在不断变化的工作环境中执行任务，以下是具体的实现步骤：

实时监测：

在实时环境中，需要持续监测机器人的状态和环境的变化，可以通过传感器数据、定位系统和环境感知来实现。

环境感知和状态更新：

根据监测到的数据，不断更新机器人的状态信息，包括位置、速度、姿态和周围环境的信息。

碰撞检测和避免：

持续进行碰撞检测，以确保机器人之间和机器人与障碍物之间不会发生碰撞，如果检测到碰撞，采取适当的措施来避免碰撞，如调整轨迹或速度。

轨迹更新：

根据实时环境变化和机器人状态的更新，可能需要调整机器人的轨迹，可以通过重新规划轨迹或根据新的目标点进行调整。

控制调整：

根据环境和机器人状态的变化，可能需要对控制输入进行调整，以适应新的情况，可以通过重新计算PID控制器中的控制输入来实现，以保持机器人在轨迹上的跟踪。

循环执行：

上述步骤需要在实时环境中持续执行，以确保机器人能够适应不断变化的工作环境，高效完成任务。

通过这个步骤，可以实现机器人的自适应性和鲁棒性，使其能够在实时环境中动态调整轨迹和控制，以适应环境变化，从而提高工作效率和安全性。

最后应说明的是：以上所述仅为本发明的优选实施例而已，并不用于限制本发明，尽管参照前述实施例对本发明进行了详细的说明，对于本领域的技术人员来说，其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明权利要求保护的范围之内。

Claims (7)

1.一种机器人运动轨迹控制方法，其特征在于，包括如下步骤：

步骤1：对工作环境进行三维建模，包括所有参与协同工作的多轴机器人、工件、障碍物和工作空间的边界；

步骤2：为每个机器人生成可行的轨迹，以确保可以协同工作而不发生碰撞；

步骤3：将每个机器人的路径转化为轨迹，以便实际控制机器人的运动；

步骤4：控制机器人按照生成的轨迹移动；

步骤5：在每个时刻，需要进行碰撞检测，以确保多轴机器人之间不会发生碰撞；

步骤6：实时环境中执行轨迹控制，不断监测机器人的状态和环境变化，并根据需要对机器人的轨迹和控制进行调整，以应对不断变化的工作环境。

2.根据权利要求1所述的一种机器人运动轨迹控制方法，其特征在于，所述步骤1包括：

使用传感器获取环境的三维数据点；

对从传感器获取的数据进行预处理，以确保数据的准确性和一致性；

将预处理后的数据表示为一个三维点云，其中每个点包含三维坐标(x,y,z)；

根据点云数据，定义工作空间W的边界和限制，以确保多轴机器人在这个环境中能够协同工作，工作空间W的边界和限制，使用以下公式表示：

W＝((x,y,z)|x_min≤x≤x_max，y_min≤y≤y_max，z_min≤z≤z_max)

其中：(x,y,z)表示三维坐标点，x_min、x_max、y_min、y_max、z_min、z_max分别表示工作空间的最小和最大边界坐标。

3.根据权利要求2所述的一种机器人运动轨迹控制方法，其特征在于，所述步骤2包括：

随机选择一个初始位置q_init在工作空间W内，并将其作为RRT树的根节点，初始化一个空的RRT树，将q_init添加为树的唯一节点。

在每次迭代中，随机选择一个目标点q_rand在工作空间WW内，然后，从RRT树中找到最近的节点q_nearest，即使得q_nearest到q_rand的距离最小；

计算从q_nearest到q_rand的连接，并确保这条连接是可行的；

如果连接是可行的，将q_rand添加到RRT树中，作为新的节点，并以q_nearest为父节点，从而扩展树；

连接表示为以下公式：

q_new＝Steer(q_nearest，q_rand)

其中Steer是函数，用于计算从q_nearest到q_rand的有效路径；

重复上述步骤，直到达到终止条件；

一旦RRT树达到终止条件，从初始位置q_init到目标位置q_goal提取路径；

通过回溯RRT树，q_goal向树根q_init倒退，并收集连接节点的路径。

4.根据权利要求3所述的一种机器人运动轨迹控制方法，其特征在于，所述步骤3包括：

将从得到的路径分为若干个线性段，线性段为两个节点之间的直线路径；

对于每个线性路径段，使用三次样条插值来生成平滑的轨迹；

三次样条插值表示为三次多项式函数，每个线性段有一个对应的三次多项式，三次多项式的一般形式如下：

P(t)＝a+bt+ct²+dt³

其中，P(t)是路径上的位置，t是时间或参数，而a,b,c,和d是待定系数；

使用计算出的插值系数，在每个线性段上生成轨迹点，从而得到平滑的轨迹。

5.根据权利要求4所述的一种机器人运动轨迹控制方法，其特征在于，所述步骤4包括：

在每个时刻，根据当前轨迹的时间参数，确定机器人在路径上的目标位置q_target；

计算当前位置q_current与目标位置q_target之间的误差，表示为e(t)：

e(t)＝q_current-q_target

使用PID控制算法来计算控制输入u(t)，PID控制器的公式如下：

其中：u(t)是控制输入，K_p是比例增益，用于根据当前误差来调整控制输入；K_i是积分增益，用于根据误差的积分来消除系统的稳态误差；K_d是微分增益，用于根据误差变化率来改善系统的响应速度；

将计算出的控制输入u(t)应用于机器人的执行器，以调整机器人的位置或速度，使其接近目标位置；

重复上述步骤，直到机器人到达路径的终点。

6.根据权利要求5所述的一种机器人运动轨迹控制方法，其特征在于，所述步骤5包括：

在每个时刻，对机器人之间的碰撞进行检测；

对于两个机器人i和j之间的碰撞检测，使用距离阈值来检测是否存在碰撞，如果机器人之间的距离小于阈值d_threshold，则认为存在碰撞：

d_ij(t)＝||q_i(t)-q_j(t)||

其中：d_ij(t)是机器人ii和jj之间的距离，q_i(t)和q_j(t)分别是机器人i和j在时刻t的位置。

7.根据权利要求6所述的一种机器人运动轨迹控制方法，其特征在于，所述步骤6包括：

持续监测机器人的状态和环境的变化；

根据监测到的数据，不断更新机器人的状态信息，包括位置、速度、姿态和周围环境的信息；

持续进行碰撞检测，以确保机器人之间不会发生碰撞；

根据实时环境变化和机器人状态的更新；

根据环境和机器人状态的变化，对控制输入进行调整，以适应新的情况；

循环执行上述步骤，以确保机器人能够适应不断变化的工作环境。