一种应用于机械手臂的笛卡尔空间运动预测方法与流程

本发明涉及机械手臂空间运动预测技术领域，具体是一种应用于机械手臂的笛卡尔空间运动预测方法。

背景技术：

随机出现的奇异点、运动边界等极限位置是机械臂运动时不可避免的一些状态，在这些特殊的位置点，机械臂会出现运动异常，必须紧急减速停止，现有技术方案的解决方法为当机械臂遇到极限位置时，控制系统会控制机械臂紧急减速，机械臂只能在关节空间内控制各关节电机各自尽快减速为0，从而实现机械手臂的整体减速停止，现有技术主要有以下两个缺点：1、突然的减速会对机械臂电机、减速机等硬件造成比较大的冲击，影响硬件寿命；2、减速过程中机械臂终端会偏离笛卡尔空间下规划的运动轨迹，影响机械臂的轨迹精度。

技术实现要素：

本发明的目的在于提供一种应用于机械手臂的笛卡尔空间运动预测方法，以解决背景技术中提出的问题。

为实现上述目的，本发明提供如下技术方案；

一种应用于机械手臂的笛卡尔空间运动预测方法，所述应用于机械手臂的笛卡尔空间运动预测方法包括：

s1：设定所述机械手臂无法到达的最远距离pm；

s2：通过以下运动规划方程计算出所述机械手臂的运动轨迹：

其中，p(t)为机械手臂的运动轨迹，pt1为机械手臂匀速运动的初始位置，p0为机械手臂运动的初始位置，v0为机械手臂运动的初始速度，vm为机械手臂运动的最大速度，a为机械手臂运动的加速度，t为机械手臂运动的时间，t1为机械手臂开始匀速运动的时间，tm为机械手臂到达最远距离pm的时间；

s3：通过不断迭代机械手臂运动的时间t，对机械手臂的运动轨迹p(t)进行反向运动学求解；

s4：当所述运动规划方程无解时，得到所述机械手臂的极限位置，执行步骤s5；

s5：根据所述机械手臂的极限位置，预测所述机械手臂的减速点，并对所述机械手臂的运动轨迹进行规划。

可选的，所述机械手臂在笛卡尔空间下的运动，其运动轨迹为直线、圆弧或曲线中的一种或多种。

可选的，所述机械手臂的运动包括匀加速运动阶段、匀速运动阶段及匀减速运动阶段。

可选的，使用如下运动控制方程对所述机械手臂的运动轨迹进行规划：

其中，p0为机械手臂运动的初始位置，v0为机械手臂运动发热初始速度，vm为机械手臂运动的最大速度，a为机械手臂运动的加速度，pt1、pt2为机械手臂开始匀速运动和结束匀速运动的初始位置，t1为机械手臂开始匀速运动的时间，t2为机械手臂结束匀速运动的时间，t3为机械手臂停止运动的时间。

可选的，所述机械手臂停止运动的位置小于或等于所述机械手臂运动的极限位置。

与现有技术相比，本发明的有益效果是：通过对机械手臂在运动前进行快速的预测计算，根据笛卡尔空间的运动轨迹推演出机械手臂的减速点和极限位置，从而提前规划好机械手臂的速度控制策略及运动轨迹，根据设定的运动轨迹及减速点从容减速停止，不会偏离运动轨迹，实现机械手臂的安全减速和稳定停止，减少对硬件的冲击，避免轨迹偏离，提高系统稳定性和整体寿命。

附图说明

图1为梯形速度控制法的速度时间图像；

图2为本发明提供的一种应用于机械手臂的笛卡尔空间运动预测方法的流程图；

图3为本发明提供的设定极限位置时的示意图；

图4为本发明提供的设定减速点时的示意图；

图5为本发明提供的机械手臂平稳减速的效果图。

具体实施方式

目前，如图1所示，机械手臂在笛卡尔空间下的运动(直线、圆弧、任意曲线)普遍基于梯形速度控制法，可以使用如下运动控制方程进行描述：

其中，p0为机械手臂运动的初始位置，v0为机械手臂运动发热初始速度，vm为机械手臂运动的最大速度，a为机械手臂运动的加速度，pt1、pt2为机械手臂开始匀速运动和结束匀速运动的初始位置，t1为机械手臂开始匀速运动的时间，t2为机械手臂结束匀速运动的时间，t3为机械手臂停止运动的时间。

通过将时间t1、t2代入上述运动控制方程计算得出机械手臂的终端姿态，再通过运动学反解(ik)即可求得机械手臂的关节空间位置，从而实现机械手臂的控制。但是，机械手臂在笛卡尔空间下存在奇异点、极限点等极限位置的问题，上述方法并没有考虑这些问题，所以在遇到奇异点或极限点时，会紧急减速停止，对机械手臂的硬件造成非常大的冲击，并且造成轨迹的偏离。

本发明提出的应用于机械手臂的笛卡尔空间运动预测方法可以在机械手臂在运动前进行快速的预测计算，根据笛卡尔空间的运动轨迹推演出需要减速的关键点和极限位置，从而增加一段安全减速距离，提前规划好机械臂的速度控制策略，让机械手臂在到达极限位置之前，根据设定的减速时间从容减速停止。从而解决现有技术中，机械臂在运动状态下遇到极限位置紧急减速所导致的硬件冲击、轨迹偏离等问题。

下面将结合本发明实施例中的附图，对本发明实施例中的技术方面进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

请参阅图2，本发明提供的一种实施例：

一种应用于机械手臂的笛卡尔空间运动预测方法，所述应用于机械手臂的笛卡尔空间运动预测方法包括：

s1：设定所述机械手臂无法到达的最远距离pm；

s2：通过以下运动规划方程计算出所述机械手臂的运动轨迹：

其中，p(t)为机械手臂的运动轨迹，pt1为机械手臂匀速运动的初始位置，p0为机械手臂运动的初始位置，v0为机械手臂运动的初始速度，vm为机械手臂运动的最大速度，a为机械手臂运动的加速度，t为机械手臂运动的时间，t1为机械手臂开始匀速运动的时间，tm为机械手臂到达最远距离pm的时间；

s3：通过不断迭代机械手臂运动的时间t，对机械手臂的运动轨迹p(t)进行反向运动学求解；

s4：当所述运动规划方程无解时，得到所述机械手臂的极限位置，执行步骤s5；

s5：根据所述机械手臂的极限位置，预测所述机械手臂的减速点，并对所述机械手臂的运动轨迹进行规划。

具体的，首先假定运动的极限位置，规划初始的运动轨迹。在奇异点、极限点未知的情况下，如图3所示，我们设定一个机械手臂无法到达的最远位置pm，并通过下面的运动规划方程进行运动规划：

接着，在机械手臂正真开始运动之前，或者在运动过程中多线程实现预测，不断迭代时间周期t，代入(4)、(5)公式中，可以求得每个运动周期的机械手臂终端姿态，通过运动学ik求解。如果求解成功，说明当前迭代的点位不是一个极限位置，继续迭代更新；如果求解失败，则说明找出了所述机械手臂的极限位置，可退出循环。

最后，如图4所示，根据预测的极限位置，重新规划运动轨迹，并加入减速点，因为已经可以预测得到极限位置，根据机械手臂的性能，可指定极限位置前的减速点，使用公式(1)、(2)、(3)实现机械手臂加速、匀速、减速三个阶段的控制，避免了突然减速停止的情况，当然，所述机械手臂停止运动的位置小于或等于所述机械手臂运动的极限位置。

具体的，为了清楚的解释本发明，本实施例提供以下具体计算的例子：

1、根据假设的极限位置进行运动规划

假设机械手臂的起始位置在空间中的a点(0，0，0)，期望沿x轴方向直线运动，极限位置在m(10，0，0)处。机械手臂终端的运动速度上限为2，加速度为2。由于在机械手臂运动之前，我们无法准确知道机械手臂在运动方向上的极限位置在m处，所以不妨先假设机械手臂的极限位置在笛卡尔空间之外的一点pm(100，0，0)处。

接着根据运动规划方程进行运动规划：

由可推导出

这样我们就可以得到机械手臂运动到pm处时的运动规划方程：

2、求解

接下来，根据以上计算得到的运动规划方程，循环迭代时间周期t，求得机械手臂的终端姿态，并进行反向运动学求解。

当计算到m(10，0，0)处时，到达机械手臂实际的极限位置，反向运动学无法求得机械手臂有效解，此时我们就预测得到了机械手臂的极限位置，并且根据此极限位置重新进行运动规划。

3、根据预测得到的机械手臂极限位置，重新进行运动规划

如图4所示，在第1步中已经计算得到：

又根据第2步的迭代预测得到：pt3＝10

从而可以得到pt2＝pt3-pt1＝10-1＝9

所以：t2-t1＝(pt2-pt1)/vm＝(9-1)/2＝4

t2＝4+t1＝4+1＝5

t3＝5+1＝6

到此，我们就预测得到了机械手臂从起始位置沿x轴方向运动过程中的运动控制方程，可以在极限位置m(10，0，0)处平滑停止：

如果使用通用的运动规划方法，运动控制方程使用(4)、(5)，则无法提前预测极限位置并实现平滑减速，将导致机械手臂在运动极限位置紧急停止，对硬件冲击较大，很容易偏离轨迹。所以本发明所提出的运动预测算法效果更加优异。

最后请参阅图5，将本发明提出的技术方案应用于实际的机械臂控制系统，进行过完整的测试，图5是根据本发明进行提前预测后实现的平稳减速效果，不会发生偏移轨迹的现象，对比现有技术方案，控制效果具有明显的优势。

尽管参照前述实施例对本发明进行了详细的说明，对于本领域的技术人员来说，其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。