机械臂的过渡路径的位姿同步方法及装置与流程

本发明涉及机械臂的路径规划技术。

背景技术：

六自由度机械臂因其灵活的末端通常被作为通用型机械臂在各个领域中得到了广泛使用，尤其是焊接、涂胶等。六自由度机械臂的六个自由度分别指机械臂末端可沿坐标系的x、y、z轴平移以及绕x、y、z轴旋转。在使机械臂运动时，首先需要用户根据任务给定若干示教点，并指定示教点间的路径类型、是否需要圆滑以及其他运动属性,然后，机械臂的控制系统根据相关信息得到相应路径，并进行速度规划，进而驱动电机执行相应动作。在进行路径规划时，往往需要将位置与姿态分别进行规划，再进行同步。当机械臂不同路径间不需要圆滑时，执行速度从0到0规划，只需要按照比例进行同步即可。但当不同路径需要圆滑时，由于不同路径以及各种圆滑信息的不同，不能简单地采用比例的方式进行同步。

技术实现要素：

本发明所要解决的技术问题在于提供一种机械臂的过渡路径的位姿同步方法，其能根据用户给定的姿态过渡范围及已生成的位置过渡路径生成用同一节点规划参数进行同步的姿态过渡路径。

本发明实施例提供了一种机械臂的过渡路径的位姿同步方法，其中，过渡路径用于第一路径与第二路径之间的过渡，该机械臂的过渡路径的位姿同步方法包括以下步骤：

根据给定的姿态过渡范围及已知的位置过渡路径，确定待同步的姿态过渡路径的起始点端点条件和结束点端点条件；

根据待同步的姿态过渡路径的起始点端点条件和结束点端点条件生成姿态过渡路径。

本发明的又一实施例还提供了一种机械臂的过渡路径的位姿同步装置，包括存储器和处理器。存储器用于存储程序；处理器用于加载所述程序以执行前述的位姿同步方法。

本发明至少具有以下优点：

1、进行圆滑时，不同路径间需要根据用户给定的圆滑范围确定过渡路径范围。当位置与姿态同时存在时，本发明实施例的位姿同步方法能根据已生成的位置过渡路径生成用同一节点规划参数进行同步的姿态过渡路径；

2、采用根据本发明实施例的位姿同步方法生成的过渡路径可保证关节角具有与过渡路径相同阶次的连续性，并且与规划路径形式无关。

附图说明

图1示出了根据本发明一实施例的机械臂的过渡路径的位姿同步方法的示意图。

图2示出了若干段路径相接的示意图。

具体实施方式

请参阅图1。根据本发明一实施例的一种机械臂的过渡路径的位姿同步方法，其中，过渡路径用于第一路径与第二路径之间的过渡，过渡路径包括位置过渡路径及姿态过渡路径，该机械臂的过渡路径的位姿同步方法包括以下步骤：

根据给定的姿态过渡范围及已知的位置过渡路径，确定待同步的姿态过渡路径的起始点端点条件和结束点端点条件；

根据待同步的姿态过渡路径的起始点端点条件和结束点端点条件生成姿态过渡路径。

进一步地，该机械臂的过渡路径的位姿同步方法还包括以下步骤：

将所述位置过渡路径和姿态过渡路径进行合成，得到位姿过渡路径；

对该位姿过渡路径进行插补，得到离散的插补点；

对离散的插补点进行逆运动学计算，得到机械臂的各关节的运动控制指令；

用所述运动控制指令控制机械臂，使机械臂的末端按照所述位姿过渡路径进行移动。

其中，根据给定的姿态过渡范围及已知的位置过渡路径，确定待同步的姿态过渡路径的起始点端点条件和结束点端点条件，具体包括：

根据用户给定的姿态过渡范围，通过计算获得过渡路径的姿态插补函数在起始点处的姿态值；在下文描述的实施例中，该姿态值等于q1(θ12/θ1)，θ11和θ1是与用户给定的姿态过渡范围相关的参数；

通过计算获得过渡路径的姿态插补函数q(v)在起始点关于位置路程s的一阶左导数和二阶左导数

通过计算获得过渡路径的姿态插补函数q(v)在起始点关于位置路程s的一阶右导数和二阶右导数

使过渡路径的姿态插补函数q(v)在起始点关于位置路程s的一阶左导数与过渡路径的位置插补函数q(v)在起始点关于位置路程s的一阶右导数相等，过渡路径的姿态插补函数q(v)在起始点关于位置路程s的二阶左导数与过渡路径的位置插补函数q(v)在起始点处关于位置路程s的二阶右导数相等，得到待同步的姿态过渡路径的起始点端点条件；

根据用户给定的姿态过渡范围，通过计算获得过渡路径的姿态插补函数在结束点处的姿态；在下文描述的实施例中，该姿态值等于q2(θ21/θ2)；θ21和θ2是与用户给定的姿态过渡范围相关的参数；

通过计算获得过渡路径的姿态插补函数q(v)在结束点关于位置路程s的一阶左导数和二阶左导数

通过计算获得过渡路径的姿态插补函数q(v)在结束点关于位置路程s的一阶右导数和二阶右导数

使过渡路径的姿态插补函数q(v)在结束点关于位置路程s的一阶左导数与过渡路径的位置插补函数q(v)在结束点关于位置路程s的一阶右导数相等，过渡路径的姿态插补函数q(v)在结束点关于位置路程s的二阶左导数与过渡路径的位置插补函数q(v)在结束点处关于位置路程s的二阶右导数相等，得到待同步的姿态过渡路径的结束点端点条件。

以下结合附图2对上述方法的原理及过程做出更详细的说明。

图2示出了若干段路径相接的情形，要说明的是，图2中的直线只是表示包括位置和姿态的路径概念，其位置可以为直线、圆滑、spline等，姿态可以为slerp等。粗直线代表原路径，根据用户给定的示教点确定，a、b、c三点分别为用户给定路径始末示教点，每个点均带有位置和姿态信息。虚线圆f表示过渡范围，以两段路径相接点为中心，由用户给定过渡信息确定。过渡范围包括位置过渡范围和姿态过渡范围。在过渡范围内的粗曲线代表相邻路径间的过渡路径，也即过渡曲线。p11、p12、p21、p22分别为过渡曲线与第一路径s1、第二路径s2的相交点。

在图2中，第一路径s1在不圆滑情况下的位置路程长度为l1，也即a、b两点之间的位置路程长度为l1；a点为第一路径的端点，b点为第一路径和第二路径在不进行圆滑过渡时的相接点。第一路径与前后路径圆滑，a点到p11点的位置路程长度为l11，a点到p12点的位置路程长度为l12。p11点为第三路径s3和第一路径s1之间的过渡曲线与第一路径s1的相交点，第三路径s3和第一路径s1在不进行圆滑过渡时的相接点为a点。p12为过渡路径r1的起始点。类似地，第二路径s2在不圆滑情况下的位置路程长度为l2，即b、c两点之间的位置路程长度为l2；c点为第二路径的端点，b点为第一路径和第二路径在不进行圆滑过渡时的相接点。第二路径与前后路径圆滑，b点到p21点的位置路程长度为l21，b点到p22点的位置路程长度为l22。p22点为第四路径s4和第二路径s2之间的过渡曲线与第二路径s2的相交点，第四路径s4和第二路径s2在不进行圆滑过渡时的相接点为c点；p21点为过渡路径r1的终止点。

近似地，设第一路径不圆滑情况下的姿态路程长度为θ1，θ1为从a点到b点的姿态路程长度。由于第一路径与前后路径圆滑，第一路径的非过渡路径姿态起止路程长度，即a点到p11点的姿态路程长度为θ11，a点到p12点的姿态路程长度为θ12。设第二路径s2不圆滑情况下的姿态路程长度为θ2，θ2为b点到c点的姿态路程长度。由于第二路径s2与前后路径圆滑，第二路径非过渡路径姿态起止路程长度，即b点到p21点的姿态路程长度为θ21，b点到p22点的姿态路程长度为θ22。

由于在进行位置插补时，需对位置路程进行速度规划。因此，只需要在进入圆滑前后，姿态关于位置路程的导数连续即可。连续性根据速度规划的连续性来确定，如只需要速度连续，则只需姿态关于位置路程的一阶导数连续；如需要加速度连续，则需姿态关于位置路程的二阶导数连续；以此类推。下面以具有加速度连续性为例进行说明。

进入圆滑前，为非转阶段，位置与姿态采用比例法进行同步。假设在第一路径的非过渡段采用的姿态插补函数为q1(u)。在第一路径上，用于姿态插补的非过渡路径的节点规划参数u随位置路程s变化为：

在过渡路径r1(本实施例中即圆滑段)，位置姿态采用同一个节点规划参数v进行同步，v∈[0,1]。设过渡路径的位置插补函数为p(v)，过渡路径的姿态插补函数为q(v)。为了得到与位置过渡路径同步的姿态过渡路径，首先需要根据姿态路径信息及位置过渡路径求得姿态过渡路径起点p12处的端点条件。对于进入圆滑前，过渡路径的姿态插补函数q(v)关于位置路程s的一阶导数、二阶导数分别为

其中和分别为式①对参数u求关于位置路程参数s的一阶导数、二阶导数，即

代入式②可得

进入圆滑后，过渡路径的姿态插补函数q(v)关于位置路程s的一阶导数、二阶导数分别为

其中，和可根据位置路程s与过渡路径的位置插补函数为p(v)之间的关系求出，则有

将③、④式相结合，使过渡路径的姿态插补函数q(v)在起始点关于位置路程s的一阶左导数、二阶左导数分别与过渡路径的位置插补函数q(v)在起始点关于位置路程s的一阶右导数、二阶右导数相等，可得到姿态过渡路径在起始点p12处的端点条件为

同时，还有起始点端点条件

q(0)＝q1(θ12/θ1)

q(0)为过渡路径的姿态插补函数q(v)在起始点p12处的值。

同理，可以根据上述方法得到姿态过渡路径与第二路径相接处的端点条件。姿态过渡路径的结束点p22处的端点条件如下所示：

q(1)＝q2(θ21/θ2)

其中，q2(u)为第二路径的非过渡段的姿态插补函数。q(1)为过渡路径的姿态插补函数q(v)在结束点p22处的值。其中，和可根据位置路程s与过渡路径的位置插补函数为p(v)之间的关系求出，则有：

根据所得到的待同步的姿态过渡路径的起始点端点条件和结束点端点条件，即可得到位置和姿态采用同一个量进行同步的过渡路径。

本发明的又一实施例还提供了一种机械臂的过渡路径的位姿同步装置，包括存储器和处理器。存储器用于存储程序；处理器用于加载所述程序以执行前述的位姿同步方法。

显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。