位姿路径平滑过渡规划方法及计算机可读存储介质、设备

技术领域

本申请涉及路径规划领域，尤其涉及一种位姿路径平滑过渡规划方法及计算机可读存储介质、设备。

背景技术

在相关技术中，为了避障同时提高现场工艺节拍，多自由度工业机器人通常会采用连续路径(Continuous Path)运动，需要在两段笛卡尔空间的运动(直线、圆弧、样条曲线等)之间插入平滑的位姿过渡曲线。

然而，一般的位姿路径过渡方法存在以下几个问题：(1)不具有通用性，例如三维圆弧与三维圆弧之间可能无法采用圆弧过渡；(2)无法保证路径二阶连续，如圆弧过渡。

发明内容

本申请旨在至少解决现有技术中存在的技术问题之一。为此，本申请提出一种位姿路径平滑过渡规划方法及计算机可读存储介质、设备，本申请所提供的位姿路径平滑过渡方法具有通用性，且可保证整段路径二阶连续。

本申请实施例第一方面提供一种位姿路径平滑过渡规划方法，包括：预设位姿路径平滑过渡参数；根据所述位姿路径平滑过渡参数，得到第一段位姿路径的离散段数、第二段位姿路径的离散段数；根据位姿路径平滑过渡参数、所述第一段位姿路径的离散段数、所述第二段位姿路径的离散段数计算得到过渡点路径标量；根据所述过渡点路径标量，对所述第一段位姿路径与所述第二段位姿路径进行矢量过渡平滑处理。

本申请实施例中位姿路径平滑过渡规划方法包括如下技术效果：具有通用性，且可保证整段路径二阶连续。

在一些实施例中，所述位姿路径平滑过渡参数，包括：位置路径平滑过渡半径、位置路径离散最大步长、姿态路径离散最大步长。

在一些实施例中，所述根据所述位姿路径平滑过渡参数，得到第一段位姿路径的离散段数、第二段位姿路径的离散段数，还包括：

对所述第一段位姿路径的路径标量进行归一化规划，得到第一路径标量导数的最大值；根据所述第一路径标量导数的最大值、第一位置路径长度、所述位置路径离散最大步长、第一四元数夹角、所述姿态路径离散最大步长得到所述第一段位姿路径的离散段数。

在一些实施例中，所述根据所述位姿路径平滑过渡参数，得到第一段位姿路径的离散段数、第二段位姿路径的离散段数，还包括：

对所述第二段位姿路径的路径标量进行归一化规划，得到第二路径标量导数的最大值；

根据所述第二路径标量导数的最大值、第二位置路径长度、所述位置路径离散最大步长、第二四元数夹角、所述姿态路径离散最大步长得到所述第二段位姿路径的离散段数。

在一些实施例中，所述根据所述第一路径标量导数的最大值、第一位置路径长度、所述位置路径离散最大步长、第一四元数夹角、所述姿态路径离散最大步长得到所述第一段位姿路径的离散段数，还包括：根据所述第一路径标量导数的最大值、所述第一位置路径长度、所述位置路径离散最大步长得到所述第一离散段数；根据所述第一路径标量导数的最大值、所述第一四元数夹角、所述姿态路径离散最大步长得到所述第二离散段数；对所述第一离散段数、所述第二离散段数进行比较处理、向上取整处理得到所述第一段位姿路径的离散段数。

在一些实施例中，所述根据所述第二路径标量导数的最大值、第二位置路径长度、所述位置路径离散最大步长、第二四元数夹角、所述姿态路径离散最大步长得到所述第二段位姿路径的离散段数，还包括：根据所述第二路径标量导数的最大值、所述第二位置路径长度、所述位置路径离散最大步长得到所述第三离散段数；根据所述第二路径标量导数的最大值、所述第二四元数夹角、所述姿态路径离散最大步长得到所述第四离散段数；对所述第三离散段数、所述第四离散段数进行比较处理、向上取整处理得到所述第二段位姿路径的离散段数。

在一些实施例中，所述根据位姿路径平滑过渡参数、所述第一段位姿路径的离散段数、所述第二段位姿路径的离散段数计算得到过渡点路径标量，包括：根据所述位置路径平滑过渡半径、所述第一段位姿路径的离散段数、所述第二段位姿路径的离散段数得到所述过渡点路径标量。

在一些实施例中，所述根据所述过渡点路径标量对位姿路径进行矢量过渡处理，还包括：

根据所述第一段位姿路径的离散段数与端点、所述第一路径标量，将所述第一段位姿路径离散化处理；根据所述第二段位姿路径的离散段数与端点、所述第二路径标量，将所述第二段位姿路径离散化处理；根据所述过渡点路径标量，对所述第一段位姿路径与所述第二段位姿路径进行矢量过渡平滑处理。

本申请实施例第二方面提供一种计算机可读存储介质，存储有计算机可执行指令，所述计算机可执行指令用于：执行上述任一实施例中的位姿路径平滑过渡方法。

本申请实施例第三方面提供一种设备，包括：处理器；存储器，其上存储有可在所述处理器上运行的计算机程序；其中，所述计算机程序被所述处理器执行时实现上述任一实施例中的的位姿路径平滑过渡方法的步骤。

本申请的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显。

附图说明

下面结合附图和实施例对本申请做进一步的说明，其中：

图1A-图1B为本申请实施例一种位姿路径平滑过渡方法的路径平滑过渡示意图；

图2为本申请一实施例一种位姿路径平滑过渡方法的流程图；

图3为本申请再一实施例一种位姿路径平滑过渡方法的流程图；

图4为本申请又一实施例一种位姿路径平滑过渡方法的流程图；

图5为本申请还一实施例一种位姿路径平滑过渡方法的流程图；

图6为本申请还一实施例一种位姿路径平滑过渡方法的流程图；

图7为本申请还一实施例一种位姿路径平滑过渡方法的流程图。

具体实施方式

下面详细描述本申请的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本申请，而不能理解为对本申请的限制。

在本申请的描述中，需要理解的是，涉及到方位描述，例如上、下、前、后、左、右等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本申请和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本申请的限制。

在本申请的描述中，若干的含义是一个以上，多个的含义是两个以上，大于、小于、超过等理解为不包括本数，以上、以下、以内等理解为包括本数。如果有描述到第一、第二只是用于区分技术特征为目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量或者隐含指明所指示的技术特征的先后关系。

本申请的描述中，除非另有明确的限定，设置、安装、连接等词语应做广义理解，所属技术领域技术人员可以结合技术方案的具体内容合理确定上述词语在本申请中的具体含义。

本申请的描述中，参考术语“一个实施例”、“一些实施例”、“示意性实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本申请的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。

在相关技术中，为了避障同时提高现场工艺节拍，多自由度工业机器人通常会采用连续路径(Continuous Path)运动，需要在两段笛卡尔空间的运动(直线、圆弧、样条曲线等)之间插入平滑的位姿过渡曲线。

然而，一般的位姿路径过渡方法存在以下几个问题：(1)不具有通用性，例如三维圆弧与三维圆弧之间可能无法采用圆弧过渡；(2)无法保证路径二阶连续，如圆弧过渡。

基于上述存在的技术问题，本申请提供一种位姿路径平滑过渡方法，该位姿路径平滑过渡方法具有通用性，且可保证整段路径二阶连续。

请参照图1A-图1B，本申请所提供的位姿路径平滑过渡方法包括：位置路径平滑过渡与姿态路径平滑过渡，可适用于是直线与直线、直线与圆弧、圆弧与圆弧等路径的平滑过渡。图1A为位置路径平滑过渡示意图，图1B为姿态路径平滑过渡示意图。

请参照图2，本申请实施例提供一种位姿路径平滑过渡方法，包括：步骤S100、预设位姿路径平滑过渡参数；步骤S200、根据位姿路径平滑过渡参数，得到第一段位姿路径的离散段数、第二段位姿路径的离散段数；步骤S300、根据位姿路径平滑过渡参数、第一段位姿路径的离散段数、第二段位姿路径的离散段数计算得到过渡点路径标量；步骤S400、根据过渡点路径标量，对第一段位姿路径与第二段位姿路径进行矢量过渡平滑处理。

根据需要进行平滑过渡的位姿路径预设位姿路径平滑过渡参数，并根据位姿路径平滑过渡参数得到第一段位姿路径的离散段数、第二段位姿路径的离散段数。可以理解的是，通过对待平滑过渡的位姿路径进行离散化处理，并得到对应于第一段位姿路径的离散段数、对应于第二段位姿路径的离散段数。

进一步地，根据位姿路径平滑过渡参数、第一段位姿路径的离散段数、第二段位姿路径的离散段数计算得到过渡点路径标量；根据过渡点路径标量进行矢量过渡处理，以对位姿路径进行平滑过渡处理。可以理解的是，通过计算得到过渡点路径标量分别对第一段位姿路径、第二段位姿路径进行矢量过渡处理，以实现位姿路径平滑过渡，并保证路径二阶连续。

在一些实施例中，预设位姿路径平滑过渡参数，包括：位置路径平滑过渡半径、位置路径离散最大步长、姿态路径离散最大步长。

通过设置位置路径平滑过渡半径、位置路径离散最大步长、姿态路径离散最大步长以对位姿路径平滑过渡的参数进行限定。

进一步地，通过对位置路径平滑过渡半径、位置路径离散最大步长、姿态路径离散最大步长的数值大小进行调节，以控制位姿路径平滑过渡的过渡误差。

请参照图3，在一些实施例中，步骤S200、根据位姿路径平滑过渡参数，得到第一段位姿路径的离散段数、第二段位姿路径的离散段数，还包括：步骤S210、对第一段位姿路径的路径标量进行归一化规划得到第一路径标量导数的最大值；步骤S220、根据第一路径标量导数的最大值、第一位置路径长度、位置路径离散最大步长、第一四元数夹角、姿态路径离散最大步长得到第一段位姿路径的离散段数。

通过对第一段位姿路径的路径标量进行归一化规划得到第一路径标量导数的最大值，并根据第一路径标量导数的最大值、第一位置路径长度、位置路径离散最大步长、第一四元数夹角、姿态路径离散最大步长得到第一段位姿路径的离散段数。

通过第一路径标量导数的最大值、第一段位姿路径、位置路径离散最大步长作为一组参数，并将第一路径标量导数的最大值、第一四元数夹角、姿态路径离散最大步长作为另外一组参数。通过对上述两组参数进行求解比较以确定第一段位姿路径的离散段数，以使得第一段位姿路径的离散段数同时满足位置路径平滑过渡、姿态路径平滑过渡的需求。

请参照图4，进一步地，在一些实施例中，步骤S220、根据第一路径标量导数的最大值、第一位置路径长度、位置路径离散最大步长、第一四元数夹角、姿态路径离散最大步长得到第一段位姿路径的离散段数，还包括：步骤S221、根据第一路径标量导数的最大值、第一位置路径长度、位置路径离散最大步长得到第一离散段数；步骤S222、根据第一路径标量导数的最大值、第一四元数夹角、姿态路径离散最大步长得到第二离散段数；步骤S222、对第一离散段数、第二离散段数进行比较处理、向上取整处理得到第一段位姿路径的离散段数。

可以理解的是，通过对第一离散段数、第二离散段数进行比较处理确定两者之间的最大值，并通过对最大值进行向上取整处理，以确保求解得到的第一段位姿路径的离散段数是整数。可以理解的，由于直接计算得到的第一离散段数、第二离散段数一般是非整数。通过对第一离散段数、第二离散段数进行向上取整处理以确保得到的第一段位姿路径的离散段数为整数，以适用于后续的计算处理。

请参照图5，在一些实施例中，步骤S200、根据位姿路径平滑过渡参数，得到第一段位姿路径的离散段数、第二段位姿路径的离散段数，还包括：步骤S230、对第二段位姿路径的路径标量进行归一化规划得到第二路径标量导数的最大值；步骤S240、根据第二路径标量导数的最大值、第二位置路径长度、位置路径离散最大步长、第二四元数夹角、姿态路径离散最大步长得到第二段位姿路径的离散段数。

通过对第二段位姿路径的路径标量进行归一化规划得到第二路径标量导数的最大值，并根据第二路径标量导数的最大值、第二位置路径长度、位置路径离散最大步长、第二四元数夹角、姿态路径离散最大步长得到第二段位姿路径的离散段数。

例如，通过第二路径标量导数的最大值、第二段位姿路径、位置路径离散最大步长作为一组参数，并将第二路径标量导数的最大值、第二四元数夹角、姿态路径离散最大步长作为另外一组参数。通过对上述两组参数进行求解比较以确定第二段位姿路径的离散段数，以使得第二段位姿路径的离散段数同时满足位置路径平滑过渡、姿态路径平滑过渡的需求。

基于上述实施例中，第一段位姿路径的离散段数、第二段位姿路径的离散段数可兼顾位置路径平滑过渡、姿态路径平滑过渡对离散段数的需求。

请参照图6，在一些实施例中，步骤S240、根据第二路径标量导数的最大值、第二位置路径长度、位置路径离散最大步长、第二四元数夹角、姿态路径离散最大步长得到第二段位姿路径的离散段数，还包括：步骤S241、根据第二路径标量导数的最大值、第二位置路径长度、位置路径离散最大步长得到第三离散段数；步骤S242、根据第二路径标量导数的最大值、第二四元数夹角、姿态路径离散最大步长得到第四离散段数；步骤S243、对第三离散段数、第四离散段数进行比较处理、向上取整处理得到第二段位姿路径的离散段数。

可以理解的是，通过对第三离散段数、第四离散段数进行比较处理确定两者之间的最大值，并通过对最大值进行向上取整处理，以确保求解得到的第二段位姿路径的离散段数是整数。可以理解的，由于直接计算得到的第三离散段数、第四离散段数一般是非整数。通过对第三离散段数、第四离散段数进行向上取整处理以确保得到的第一段位姿路径的离散段数为整数，以适用于后续的计算处理。

在一些实施例中，步骤S300、根据位姿路径平滑过渡参数、第一段位姿路径的离散段数、第二段位姿路径的离散段数计算得到过渡点路径标量，包括：步骤S310、根据位置路径平滑过渡半径、第一段位姿路径的离散段数、第二段位姿路径的离散段数得到过渡点路径标量。

请参照图7，在一些实施例中，步骤S400、根据过渡点路径标量进行矢量过渡处理，以对位姿路径进行平滑过渡处理，还包括：步骤S410、根据第一段位姿路径的离散段数与端点、第一路径标量，将第一段位姿路径离散化处理；步骤S420、根据第二段位姿路径的离散段数与端点、第二路径标量，将第二段位姿路径离散化处理；步骤S430、根据过渡点路径标量，对第一段位姿路径与第二段位姿路径进行矢量过渡平滑处理。

可以理解的是，根据第一段位姿路径的离散段数与端点、第一路径标量，将第一段位姿路径离散化处理；根据第二段位姿路径的离散段数与端点、第二路径标量，将第二段位姿路径离散化处理；根据过渡点路径标量，对第一段位姿路径与第二段位姿路径进行矢量过渡平滑处理。

以下结合具体的计算步骤及公式对上述实施例中的位姿路径平滑过渡方法进行详细说明，其中，计算步骤主要以AB段进行计算说明。

在一些实施例中，预设位姿路径平滑过渡参数，包括：位置路径平滑过渡半径R、位置路径离散最大步长L

对第一段位姿路径、第二段位姿路径的参数进行预设，其中，位置表示为p(s)，姿态表示为q(s)、路径标量表示s，s∈[0,1]。

其中，对第一段位姿路径的路径标量进行归一化规划，得到第一路径标量导数的最大值；根据第一路径标量导数的最大值、第一位置路径长度、位置路径离散最大步长、第一四元数夹角、姿态路径离散最大步长得到第一段位姿路径的离散段数，具体包括以下计算步骤：

对路径标量s作归一化三次多项式规划，其中，端点条件为：

根据公式(1)计算得到归一化三次多项式为：

s(u)＝-2u

计算公式(2)的一阶导数最大值为s

进一步地，根据第一路径标量导数的最大值、第一位置路径长度、位置路径离散最大步长得到第一离散段数；根据第一路径标量导数、第一四元数夹角、姿态路径离散最大步长得到第二离散段数；对第一离散段数、第二离散段数进行比较处理、向上取整处理得到第一段位姿路径的离散段数。

其中，AB段位姿路径的离散段数(第一段位姿路径的离散段数)：

其中，L

其中，BC段位姿路径的离散段数(第二段位姿路径的离散段数)：

其中，L

进一步，根据路径平滑过渡半径R、第一段位姿路径的离散段数n

进一步地，根据第一段位姿路径的离散段数与端点、第一路径标量，将第一段位姿路径离散化处理；根据第二段位姿路径的离散段数与端点、第二路径标量，将第二段位姿路径离散化处理；根据过渡点路径标量，对第一段位姿路径与第二段位姿路径进行矢量过渡平滑处理，具体包括如下计算步骤：

直线AB(第一段位姿路径)的位置路径参数化方程为：

p(s)＝p

其中，p

直线AB(第一段位姿路径)的姿态路径采用四元数球面线性插值：

其中，q

q(s)＝(1-s)q

其中，q

利用矢量过渡的方法进行位姿路径平滑过渡：

其中，s

根据不同的位姿路径可适应性地选择第一位置路径长度、第二位置路径长度与位置路径参数化的计算方法(这里以直线为例，也可以是圆弧、样条等)，以使得该位姿路径平滑过渡方法适应于不同路径之间。

在一些实施例中，计算机可读存储介质，存储有计算机可执行指令，计算机可执行指令用于：执行上述任一实施例中的位姿路径平滑过渡方法。

例如，计算机可读存储介质通过快速执行任一实施例中的位姿路径平滑过渡方法，以快速地实现位姿路径平滑过渡。

在一些实施例中，设备，包括：处理器；存储器，其上存储有可在处理器上运行的计算机程序；其中，计算机程序被处理器执行时实现上述任一实施例中的位姿路径平滑过渡方法的步骤。

上面结合附图对本申请实施例作了详细说明，但是本申请不限于上述实施例，在所属技术领域普通技术人员所具备的知识范围内，还可以在不脱离本申请宗旨的前提下作出各种变化。此外，在不冲突的情况下，本申请的实施例及实施例中的特征可以相互组合。