CN110703684A - 一种端点速度不限的轨迹规划方法及装置 - Google Patents

Abstract

本发明公布了一种端点速度不限的轨迹规划方法，主要包括以下步骤：根据给定的运动学约束条件和边界条件，通过坐标变换将原始轨迹规划问题转换为归一化问题；求解归一化问题，得到归一化问题的位移时间函数；由归一化问题的位移时间函数，经过坐标反变换得到原始轨迹规划问题的时间位移函数,完成原始轨迹规划问题的求解。本发明还公开了一种端点速度不限的轨迹规划装置。本发明公开的轨迹规划方法和装置能够保证运动学约束的严格满足，且能够处理一维空间点到点运动以非静止状态开始和结束的轨迹规划问题。

Description

一种端点速度不限的轨迹规划方法及装置

技术领域

本发明涉及机器人运动规划领域，具体涉及一种端点速度不限的轨迹规划方法及装置。

背景技术

在机器人运动规划中，无论是关节空间点到点运动规划，还是任务空间直线和圆弧运动规划，一般都等效为一维空间点到点运动的轨迹规划问题。

一维空间点到点运动是在两个端点间的运动，其中一个端点为起点，另一个端点为终点，轨迹规划的目的在于确定一个位置关于时间的函数，且需要满足给定约束条件和边界条件。约束条件分为运动学约束和动力学约束两类，实际应用中都采用运动学约束，如速度限制、加速度限制。边界条件则给定了端点的位置、速度和加速度。

通常的点到点运动都是从静止状态开始，并以静止状态结束，对应的边界条件是端点速度、加速度都为零。对于此种边界条件，轨迹规划是容易解决的。但对于复合路径轨迹规划，或是在线轨迹规划，端点速度可能不为零，传统的方法是根据边界条件使用高阶多项式进行整体插值，但通常不能够保证运动学约束全程严格满足，因而难以获得满意的轨迹。

发明内容

本发明所要解决的技术问题在于提供一种端点速度不限的轨迹规划方法及装置，以解决上述背景技术中传统轨迹规划方法难以保证运动学约束全程严格满足的问题。

本发明通过以下技术方案来解决上述技术问题：

一种端点速度不限的轨迹规划方法，包括以下步骤：

S1，根据给定的运动学约束条件和边界条件，通过坐标变换将原始轨迹规划问题转换为归一化问题；

S2，求解归一化问题，得到归一化问题的位移时间函数；

S3，由归一化问题的位移时间函数，经过坐标反变换得到原始轨迹规划问题的时间位移函数,完成原始轨迹规划问题的求解。

作为本发明进一步的方案,步骤S1中给定的运动学约束条件为：最大允许速度v'_max，最大允许加速度大小a'_max以及最大允许急动度大小J'_max；给定的边界条件为：起点处位移为l'_s，起点处速度为v'_s，起点处加速度为0,其中0≤v'_s≤v'_max；终点处位移为l'_e，终点处速度为v'_e，终点处加速度为0,其中l'_e＞l'_s且0≤v'_e≤v'_max。

作为本发明进一步的方案,步骤S1中的坐标变换为：

s＝(s'-l′_s)/L，t＝t/T

，式中s'为实际位移，t'为实际时间,s为坐标变换后的位移，t为坐标变换后的时间,L＝(l'_e-l'_s)为实际的总行程,T＝/v'_max。

作为本发明进一步的方案,步骤S1中经过坐标变换，总行程和最大允许速度被归一化，原始轨迹规划问题转化为归一化问题，归一化问题的运动学约束条件为：最大允许速度为1，最大允许的加速度大小a_max＝a'_max/(L/T²)，最大允许的急动度大小J_max＝J'_max/(L/T³)；归一化问题的边界条件为：起点处位移为0，起点处速度为v_s＝v'_s/v'_max，起点处加速度为0；终点处的位移为1，终点处速度为v_e＝v'_e/v'_max，终点处加速度为0。

作为本发明进一步的方案,步骤S2中包括三个步骤：

S21，确定全程最大速度v_m；

S22，根据v_m的取值将全程划分为若干个阶段，获取每个阶段的急动度变化规律，组合得到全程的急动度变化规律J＝J(t)；

S23，将急动度变化规律J＝J(t)对时间连续积分三次，得到位移时间函数s＝s(t)。

作为本发明进一步的方案，步骤S21中v_m确定方法如下：在位移-速度构成的相平面s-v内绘制两条曲线，两条曲线的参数方程分别为

，

式中，u为参数曲线的参数，函数f(u)的表达式为

，

函数g(u)的表达式为

；

若两条曲线在相平面区域(0,1)×(0,1)内交于一点，则交点的纵坐标对应于全程最大速度v_m；若两条曲线都位于区域(0,1)×(0,1)内，但不相交，则全程最大速度v_m＝1。

作为本发明进一步的方案，步骤S22中全程划分方法如下：若v_m＜1，全程只包含加速和减速两个阶段，若v_m＝1,在加速和减速阶段之间存在匀速阶段。

作为本发明进一步的方案：每个阶段急动度关于时间为分段常值函数，急动度变化规律可以用函数值和区间长度的列表表示；对于加速阶段，速度由v_s增加到v_m，急动度变化规律公式如下：

，

对于减速阶段，速度由v_m减小到v_e，急动度变化规律公式如下：

，

对于匀速阶段，速度保持最大速度v_m＝1，急动度变化规律公式如下：

{{0，1-(f(1)+g(1))}}，

上述各式中，急动度变化规律列表中元素形式为{J_d,t_d}，代表分段常值函数的一段，其中J_d为急动度幅值，t_d为持续时间；组合各个阶段的急动度变化规律，即可得到全程急动度变化规律J＝J(t)。

作为本发明进一步的方案：步骤S3中，由归一化问题的时间位移函数s＝s(t)，经过如下坐标反变换：

s′＝Ls+l′_s，t′＝Tt

得到原始轨迹规划问题的时间位移函数s'＝s'(t')，完成原始轨迹规划问题的求解。

一种端点速度不限的轨迹规划装置，包括：

归一化模块，用于根据给定的运动学约束条件和边界条件，通过坐标变换将原始轨迹规划问题转换为归一化问题；

求解模块，用于求解归一化问题，得到归一化问题的位移时间函数；

坐标反变换模块，用于由归一化问题的位移时间函数，经过坐标反变换得到原始轨迹规划问题的时间位移函数,完成原始轨迹规划问题的求解。

与现有技术相比，本发明有以下有益效果：

1、本发明通过对位置和时间进行坐标变换，将原始轨迹规划问题转换为归一化问题，归一化问题的的总行程和最大允许速度都为1，统一了问题描述，便于分析和计算。

2、对于归一化问题，考虑运动学约束，在相平面内求解全程最大速度，由全程最大速度确定各阶段的急动度变化规律，再积分得到位移时间函数，最后经过坐标反变换得到原始轨迹规划问题的时间位移函数，这种处理方法能够保证运动学约束全程严格满足。

3、本发明中的方法能够处理一维空间点到点运动以非静止状态开始和结束的轨迹规划问题，适用范围广。

附图说明

图1为本发明实施例1中一种端点速度不限的轨迹规划方法的流程示意图；

图2为本发明实施例1中相平面曲线；

图3为本发明实施例1中急动度和加速度随时间的变化曲线。

图4为本发明实施例1中速度和位移随时间的变化曲线。

图5为本发明实施例2中一种端点速度不限的轨迹规划装置的结构示意图。

具体实施方式

为使对本发明的结构特征及所达成的功效有更进一步的了解与认识，用以较佳的实施例及附图配合详细的说明，说明如下：

实施例1

一种端点速度不限的轨迹规划方法，包括以下步骤：

S1，根据给定的运动学约束条件和边界条件，通过坐标变换将原始轨迹规划问题转换为归一化问题；

给定的运动学约束条件为：最大允许速度v'_max，最大允许加速度大小a'_max以及最大允许急动度大小J'_max；给定的边界条件为：起点处位移为l'_s，起点处速度为v'_s，起点处加速度为0,其中0'_s≤v'_max；终点处位移为l'_e，终点处速度为v'_e，终点处加速度为0,其中l'_e＞l'_s且0≤v'_e≤v'_max。

所述坐标变换的公式为：

s＝(s'-l′_s)/L，t＝t/T

其中，s'为实际位移，t'为实际时间,s为坐标变换后的位移，t为坐标变换后的时间,L＝l'_e-l'_s)，代表实际的总行程,T＝L/v'_max表示以最大允许速度v'_max匀速行驶完实际总行程的时间；

利用坐标变换公式，将原始轨迹中位移和时间进行坐标变换，总行程和最大允许速度被归一化，原始轨迹规划问题转化为归一化问题，归一化问题的约束条件变为最大允许速度为1，最大允许的加速度大小a_max＝a'_max/(L/T²)，最大允许的急动度大小J_max＝J'_max/(L/T³)；归一化问题的边界条件变为：起点处位移为0，起点处速度为v_s＝v'_s/v'_max，起点处加速度为0；终点处的位移为1，终点处速度为v_e＝v'_e/v'_max，终点处加速度为0。

对于归一化问题，用v_m表示全程最大速度，且有v_m≤1；依据速度变化情况的不同，设定全程(位移从0变化到1的过程)可以划分为多个阶段：起始阶段为加速阶段，速度由v_s增加到v_m，结束阶段为减速阶段，速度由v_m增加到v_e；当v_m＜1时，全程只包含加速阶段和减速阶段；当v_m＝1时，在加速阶段和减速阶段之间存在匀速阶段，匀速阶段速度保持为1。

对于各个阶段，分析其急动度的变化规律，并得到各阶段的行程计算公式，讨论如下：

1)、对于加速阶段，速度由v_s增加到v_m，加速度由0变化到0。考虑加速度和急动度约束，在所有可能的加速方案中，确定时间最短的加速方案，包括

以及

两种情况，方法具体如下：

11)、当

时,时间最短加速方案包含两个阶段，第一阶段急动度为J_max，加速度由0线性增加到

持续时间为

第二阶段急动度为-J_max，加速度线性减小到0,持续时间为

对急动度连续积分三次，得到两阶段加速方案的行程为

12)、当

时,时间最短加速方案包含三个阶段，第一阶段急动度为J_max，加速度由0线性增加a_max，持续时间为a_max/J_max；第二阶段急动度为0，加速度保持a_max，持续时间为

第三阶段急动度为-J_max，加速度线性减小到0,持续时间为a_max/J_max；对急动度连续积分三次，得到三阶段加速方案的行程为

2)、对于减速阶段，速度由v_m减小到v_e，加速度由0变化到0。考虑加速度和急动度约束，在所有可能的减速方案中，确定时间最短的减速方案，包括了

以及

两种情况；具体如下：

21)、当

时,时间最短减速方案包含两个阶段，第一阶段急动度为-_max，加速度由0线性减小到

持续时间为第二阶段急动度为J_max，加速度按照J＝J_max线性增加到0,持续时间为

对急动度连续积分三次，得到两阶段减速方案的行程为

22)、当

时,减速方案包含三个阶段，第一阶段急动度为-_max，加速度由0减小到-a_max，持续时间a_max/J_max；第二阶段急动度为0，加速度保持-a_max，持续时间为

第三阶段急动度为J_max，加速度线性增加到0,持续时间为a_max/J_max；对急动度连续积分三次，得到三阶段减速方案的行程为

综上所述，加速阶段和减速阶段的行程取决于v_m，用f(v_m)表示加速阶段的行程，g(v_m)表示减速阶段的行程，具体计算公式如下：

3)、当v_m＝1时，在加速阶段和减速阶段之间存在匀速阶段，匀速阶段速度保持最大允许速度1，行程为总行程1减去加速阶段和减速阶段的行程之和(f(1)+g(1))，匀速阶段急动度为0，持续时间为1-(f(1)+g(1))。

由以上讨论，加速阶段、减速阶段和匀速阶段的急动度关于时间都为分段常值函数，急动度的变化规律可以用函数值和区间长度的列表表示，其中，加速阶段的急动度变化规律表示为

，减速阶段的急动度变化规律表示为：

，匀速阶段的急动度变化规律表示为：

{{0，1-(f(1)+g(1))}} (5)

式(3)-(5)中，列表元素{J_d,t_d}代表分段常值函数的一段，其中J_d为急动度幅值，t_d为持续时间。例如，表示急动度幅值为J_max，保持一段时间后变为-_max，再保持一段时间

S2，求解归一化问题，得到归一化问题的位移时间函数；具体如下：

S21，确定全程最大速度v_m；

在位移-速度构成的相平面s-v内绘制两条曲线，两条曲线的参数方程分别为

，其中，u为曲线的参数，f(u)为式(1)中将v_m用u替换得到的表达式，g(u)为式(2)中将v_m用u替换得到的表达式；若两条曲线在相平面区域(0,1)×(0,1)内交于一点，则交点的纵坐标对应于全程最大速度v_m，有f(v_m)＝1-g(v_m)或f(v_m)+g(v_m)＝1，即加速阶段的行程和减速阶段的行程之和等于总行程1；若两条曲线都位于区域(0,1)×(0,1)内，但不相交，则全程最大速度v_m＝1，有f(v_m)＜1-g(v_m)或f(v_m)+g(v_m)＜1,即加速阶段的行程和减速阶段的行程之和小于总行程1，此时在加速阶段和减速阶段之间存在一个匀速阶段。图2为相平面曲线的示意图，两条曲线在(0,1)×(0,1)内交于一点，交点纵坐标对应于最大速度v_m且有v_m＜1。

S22，由v_m将全程划分为若干个阶段，获取每个阶段的急动度变化规律，组合得到全程的急动度变化规律J＝J(t)；

若v_m＜1，全程只包含减速和减速两个阶段；若v_m＝1,在加速和减速阶段之间存在匀速阶段。将v_m代入公式(3)-(5)，可以确定全程各阶段的急动度变化规律，由于全程由各个阶段组成，在确定了各阶段的急动度变化规律后，全程各阶段的急动度变化规律J＝J(t)也得以确定。

图3左图显示了由图2中的v_m所确定的急动度变化规律，包含加速和减速两个阶段：加速阶段从时刻0开始到时刻t₁截止，对应三阶段的加速方案；减速阶段从时刻t₁开始直到结束，对应两阶段的减速方案。

S23，急动度变化规律J＝J(t)对时间连续积分三次得到位移时间函数s＝s(t)。

急动度对时间积分一次得到加速度，积分两次得到速度，积分三次得到位移。图3右图，图4左图，图4右图分别为图3左图所示急动度变化规律经过积分得到的加速度，速度和位移随时间的变化曲线。

S3，由归一化问题的位移时间函数，经过如下坐标反变换：

s′＝Ls+l′_s，t′＝Tt

得到原始轨迹规划问题的时间位移函数s'＝s'(t')，完成原始轨迹规划问题的求解。

实施例2

如图5，图5为本发明实施例2中一种端点速度不限的轨迹规划装置的结构示意图；一种端点速度不限的轨迹规划装置，包括：

归一化模块301，用于根据给定的运动学约束条件和边界条件，通过坐标变换将原始轨迹规划问题转换为归一化问题；

求解模块302，用于求解归一化问题，得到归一化问题的位移时间函数，还包括：

S21，确定全程最大速度v_m；

S22，根据v_m的取值将全程划分为若干个阶段，获取每个阶段的急动度变化规律，组合得到全程的急动度变化规律J＝J(t)；

S23，急动度变化规律J＝J(t)对时间连续积分三次得到位移时间函数s＝s(t)。

坐标反变换模块303，用于由归一化问题的位移时间函数，经过坐标反变换得到原始轨迹规划问题的时间位移函数,完成原始轨迹规划问题的求解。

以上显示和描述了本发明的基本原理、主要特征和本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是本发明的原理，在不脱离本发明精神和范围的前提下本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明的范围内。本发明要求的保护范围由所附的权利要求书及其等同物界定。

Claims (10)

1.一种端点速度不限的轨迹规划方法，其特征在于，包括以下步骤：

S1，根据给定的运动学约束条件和边界条件，通过坐标变换将原始轨迹规划问题转换为归一化问题；

S2，求解归一化问题，得到归一化问题的位移时间函数；

S3，由归一化问题的位移时间函数，经过坐标反变换得到原始轨迹规划问题的时间位移函数,完成原始轨迹规划问题的求解。

2.如权利要求书1所述的一种端点速度不限的轨迹规划方法，其特征在于，步骤S1中给定的运动学约束条件为：最大允许速度v'_max，最大允许加速度大小a'_max以及最大允许急动度大小J'_max；给定的边界条件为：起点处位移为l'_s，起点处速度为v'_s，起点处加速度为0,其中0≤v'_s≤v'_max；终点处位移为l'_e，终点处速度为v'_e，终点处加速度为0,其中l'_e＞l'_s且0≤v'_e≤v′_max。

3.如权利要求书1所述的一种端点速度不限的轨迹规划方法，其特征在于，步骤S1中的坐标变换公式为：

s＝(s′-l′_s)/L,t＝t′/T，

其中，s'为实际位移，t'为实际时间,s为坐标变换后的位移，t为坐标变换后的时间,L＝(l'_e-l'_s)为实际的总行程,T＝/v'_max。

4.如权利要求书3所述的一种端点速度不限的轨迹规划方法，其特征在于，步骤S1中位移和时间经过坐标变换，总行程和最大允许速度被归一化，原始轨迹规划问题转化为归一化问题，归一化问题的运动学约束条件为：最大允许速度为1，最大允许的加速度大小a_max＝a'_max/(L/T²)，最大允许的急动度大小J_max＝J'_max/(L/T³)；归一化问题的边界条件为：起点处位移为0，起点处速度为v_s＝v'_s/v'_max，起点处加速度为0；终点处的位移为1，终点处速度为v_e＝v'_e/v'_max，终点处加速度为0。

5.如权利要求书1所述的一种端点速度不限的轨迹规划方法，其特征在于，步骤S2包括三个步骤：

S21，确定全程最大速度v_m；

S22，根据v_m的取值将全程划分为若干个阶段，获取每个阶段的急动度变化规律，组合得到全程的急动度变化规律J＝J(t)。

S23，将急动度变化规律J＝J(t)对时间连续积分三次，得到位移时间函数s＝s(t)。

6.如权利要求书5所述的一种端点速度不限的轨迹规划方法，其特征在于，步骤S21中v_m确定方法如下：在位移-速度构成的相平面s-v内绘制两条曲线，两条曲线的参数方程分别为

，

式中，u为两条参数曲线的参数，函数f(u)的表达式为

，

函数g(u)的表达式为

；

若两条曲线在相平面区域(0,1)×(0,1)内交于一点，则交点的纵坐标对应于全程最大速度v_m；若两条曲线都位于区域(0,1)×(0,1)内，但不相交，则全程最大速度v_m＝1。

7.如权利要求书5所述的一种端点速度不限的轨迹规划方法，其特征在于，步骤S22中全程划分方法如下：若v_m＜1，全程只包含加速和减速两个阶段；若v_m＝1,在加速和减速阶段之间存在匀速阶段。

8.如权利要求书7所述的一种端点速度不限的轨迹规划方法，其特征在，步骤S22中，获取全程的急动度变化规律J＝J(t)，包括：每个阶段的急动度变化规律是关于时间的分段常值函数，用函数值和区间长度的列表表示；其中，对于加速阶段，急动度变化规律公式如下：

，

对于减速阶段，急动度变化规律公式如下：

，

对于匀速阶段，急动度变化规律为{{0,1-(f(1)+g(1))}}，

上述各式中，急动度变化规律列表中元素形式为{J_d,t_d}，代表分段常值函数的一段，其中J_d为急动度幅值，t_d为持续时间；组合各个阶段的急动度变化规律，即可得到全程的急动度变化规律J＝J(t)。

9.如权利要求书1所述的一种端点速度不限的轨迹规划方法，其特征在于步骤S3中，由归一化问题的时间位移函数s＝s(t)，经过如下坐标反变换：

s′＝Ls+l′_s,t′＝Tt，

得到原始轨迹规划问题的时间位移函数s'＝s'(t')，完成原始轨迹规划问题的求解。

10.一种基于权利要求1-9任一所述的端点速度不限的轨迹规划方法的规划装置，其特征在于，包括：

归一化模块(301)，用于根据给定的运动学约束条件和边界条件，通过坐标变换将原始轨迹规划问题转换为归一化问题；

求解模块(302)，用于求解归一化问题，得到归一化问题的位移时间函数；

坐标反变换模块(303)，用于由归一化问题的位移时间函数，经过坐标反变换得到原始轨迹规划问题的时间位移函数,完成原始轨迹规划问题的求解。

Images