CN111399514B - 一种机器人时间最优轨迹规划方法 - Google Patents

Abstract

本发明提供了一种机器人时间最优轨迹规划方法，属于机器人控制技术领域。它解决了现有技术中轨迹规划方法在实际中适用性较差的问题。本机器人时间最优轨迹规划方法，包括获取并离散任务路径，获取各离散点坐标，还包括A、获取运动学参数；B、确定变量与约束条件，并建立约束不等式；C、计算得出最优变量约束最值以及实际允许的最优解；D、获取各离散点信息，生成离散点插补数组；E、重新生成关于时间的离散点序列，计算并输出关于时间的关节信息，提高了方法应用的适应性，降低计算难度，提升工作效率。

Description

一种机器人时间最优轨迹规划方法

技术领域

本发明属于机器人控制技术领域，涉及一种机器人时间最优轨迹规划方法。

背景技术

随着工业自动化的不断发展，工业环境日趋复杂，生产需求快速多变，对工业机器人运动性能的要求越来越高，机器人要更快更准更稳定，因此需要对机器人的轨迹规划进行更加深入的研究。传统机器人运动轨迹主要是S型轨迹，但S型轨迹采用固定运动速度和加速度，并不能发挥电机的全部性能。为了使机器人以最大速度运动，缩短运动时间，需要研究电机性能工作在最大值临界状态下的运动，这种方法称为时间最优轨迹规划方法。

在现有技术中存在完成规划时间最优轨迹的方法，最常见的是数值积分方法和凸优化方法，其中数值积分方法容易引起动力学奇异性问题，在面对复杂的动力学模型计算时难以积分，导致其只能应用于简单的动力学模型的情况。凸优化方法的计算量较大，受制于现阶段硬件计算能力的，难以做到实时计算，在实际应用中难度较大。

发明内容

本发明的目的是针对现有的技术存在上述问题，提出了一种机器人时间最优轨迹规划方法，本发明所要解决的技术问题是：现有的轨迹规划方法在实际中适用性较差。

本发明的目的可通过下列技术方案来实现：一种机器人时间最优轨迹规划方法，包括获取并离散任务路径，确定运动空间，获取各离散点对应的空间坐标，其特征在于，所述规划方法还包括如下步骤：

A、获取运动学参数：获取离散点长度序列s_k，并求解关于路径长度的微分，采用空间对应计算方法，获得关于路径长度的运动学参数，运动学参数包括关节坐标、关节角速度以及关节角加速度；

B、确定约束条件：根据约束集合选定约束条件，设定s、

为变量参数，并建立目标函数与约束不等式，再选定/>

为最优变量，其中s为路径长度，/>

为路径长度s对时间的一阶微分，即路径速度，/>

为路径长度s对时间的二阶微分，即路径加速度；

C、求解最优变量实际允许最优解：以第N个离散点为起点，迭代计算反向路径上各离散点满足约束条件的最优变量的最大值与最小值，根据最优变量的最值和约束条件，以第1个离散点为起点，逐点递推计算正向路径上各离散点实际允许的最优变量的最优解，其中N为总的离散点个数；

D、生成离散点插补数组：已知最优变量值的最优解，计算并生成离散点信息，形成插补数组，其中，离散点信息包括离散点位置、路径速度、路径加速度以及运动时间；

E、输出关于时间的关节信息：根据插补数组以及设定的插补周期，重新生成离散点，逐点计算并输出对应的关于时间的关节坐标、关节角速度以及关节角加速度。

本方法首先通过获取任务路径，机器人在做单步运动时，可以直接等距离散任务路径，获取离散点，机器人在做多步运动时，将路径分为线性段和混合段，并采用不同的方式对任务路径进行等距离散，从而生成离散点长度序列s_k；根据工作空间，获取对应空间的离散点坐标，并根据空间对应计算方法，计算获得运动学系数，为获取时间最优轨迹提供数据支持；并根据已知关节坐标、关节角速度以及关节角加速度结合定义的变量参数，建立约束不等式与目标函数，通过反向迭代计算方法求解各离散点上的最优变量的最大值与最小值；再通过正向逐点计算方法求解实际允许的最优变量值，根据最优变量求出未知变量，从而得到任务路径行走的实际最优时间；然后计算出每个离散点的所有未知信息，根据所有已知数据，重新生成等时间离散的离散点序列，再计算每个离散点对应的关于插补时间的关节坐标、关节角速度以及关节角加速度，最后输出关节信息，驱动机器人运动；其中，最优变量选定为路径速度，本方法选定的变量之间为微分计算关系，由于路径角速度、路径角加速度为路径长度关于时间的一次微分与二次微分，则将最优变量定义为路径速度，便于正向计算路径加速度，降低计算难度；步骤A中关节空间的关节角速度、关节角加速度分别是关节坐标关于路径长度的一次微分与二次微分，步骤E中的关节角速度、关节角加速度分别是关节坐标关于时间的一次微分与二次微分。

本方法针对应用需求，选择合适的约束条件，根据约束条件建立约束不等式以及目标函数，再通过迭代计算反向路径与逐点计算正向路径的方法结合，求解出实际允许的最优参数值，这样可以使计算出的最优参数值贴近实际最优参数值，在保证机器人运载安全的情况下达到时间最优的目的以及修正误差。本方法只需进行正向路径与反向路径的迭代计算，相比于现有技术需要针对每个优化周期都进行迭代计算，本方法提高了计算效率，并结合对轨迹的插值优化，通过两次对路径离散化，并将两次离散点信息结合，有效保证机器人行走路径的精度以及轨迹的光滑性。

在上述的一种机器人时间最优轨迹规划方法中，所述约束集合包括：关节空间角速度、关节空间角加速度、关节空间力矩、笛卡尔空间运动线速度、笛卡尔空间线加速度、笛卡尔空间角速度以及笛卡尔空间角加速度。

其中约束组合方式有：

(1)关节空间角速度与关节空间角加速度；

(2)关节空间角速度与关节空间力矩；

(3)关节空间角速度、关节空间角加速度、笛卡尔空间线速度以及笛卡尔空间角速度；

(4)关节空间角速度、关节空间力矩、笛卡尔空间线速度以及笛卡尔空间角速度；

(5)关节空间角速度、关节空间角加速度、笛卡尔空间线速度、笛卡尔空间线加速度、笛卡尔空间角速度以及笛卡尔空间角加速度；

(6)关节空间角速度、关节空间力矩、笛卡尔空间线速度、笛卡尔空间线加速度、笛卡尔空间角速度以及笛卡尔空间角加速度。

根据实际应用需求，选取不同的约束组合，生成所需要的运动轨迹，满足多种应用多种要求，本方法具有很强的实用性和适应性；机器人运动路径包括加速段、匀速段以及减速段，本方法可以通过调节加速段与减速段的约束条件，进行非对称轨迹规划设计。

在上述的一种机器人时间最优轨迹规划方法中，所述约束条件必须包括关节空间角速度，且至少包括关节空间角加速度和关节空间力矩的其中一项。关节空间角速度与关节空间角加速度为微分关系，实际运动中，若关节空间角加速度能够满足约束所需，则可以优先选用关节空间角加速度，不涉及动力学计算，计算量小，若无法满足，则可以选用关节空间力矩作为约束条件之一，此时电机可以发挥最大驱动能力，实现时间最优运动。

在上述的一种机器人时间最优轨迹规划方法中，若所述约束条件包括关节力矩，采用待定系数法，计算动力学方程系数。采用待定系数法求解，既可以保证动力学参数计算的准确性，又保证动力学参数求解的效率。

在上述的一种机器人时间最优轨迹规划方法中，所述迭代计算采用线性规划方法进行最优变量求解。相比于现有技术中轨迹数学表达方式复杂，计算轨迹运动学约束时使用解析法无法求取轨迹的最值，采用数值解法又因为复杂度较高、求解效率低，本方法的线性规划方法通过求解最优变量的方式可以有效地解决求解轨迹约束最优解问题，计算量小，求解效率高。

在上述的一种机器人时间最优轨迹规划方法中，在上述步骤C中，所述反向路径和正向路径的始末点路径速度允许非零。因此既可以用于单步运动，又可以用于多步之间连续运动。

在上述的一种机器人时间最优轨迹规划方法中，在上述步骤C中，最优变量实际允许最优解的计算过程包括以下步骤：

a、根据已知路径速度的最值计算路径加速度的第一区间；确定约束条件，计算在所述约定条件下允许的路径加速度第二区间；结合第一区间与第二区间获得路径加速度的最值；

b、在机器人正常运动时，通过公式：

逐点计算实际允许的路径速度；机器人运动中途停止运动时，通过公式/>

逐点计算实际允许的路径速度，其中，k为离散点，/>

为路径速度的平方，/>

为最大路径加速度，/>

为最小路径加速度，delta为离散点序列s_k在每个区间的增量。

计算路径加速度需要考虑路径速度的最值与关节空间角加速度，或者路径速度的最值与关节空间力矩的不同约束组合的计算方式，约束条件中关节空间角加速度或者关节空间力矩不同，则计算的第二区间不同；确定路径加速度的最值方法为：最大值取第一区间最大值与第二区间最大值中的最小值，最小值取第一区间最小值与第二区间最小值中的最大值。

机器人在启动时，关节电机输出最大力矩，从而产生最大加速度，使机器人尽快达到最大速度，减少加速过程时间，正常运动过程中，电机也保持最大约束值，保证机器人始终有最高运动效率；机器人运动停止前的减速段，通过降低最大力矩约束，以较小加速度减速，可实现机器人平稳停止，防止冲击力过大，引起机器人末端抖动，影响精度并对机器人造成损伤。

在上述的一种机器人时间最优轨迹规划方法中，在上述步骤D中，根据最优路径速度,通过公式：

t_k+1＝t_k+Δt

计算并生成离散点信息；

其中，v_k为第k个离散点的路径速度，u_k为第k个离散点的路径加速度，delta为离散点序列在每个区间的增量，Δt为相邻两个离散点之间的时间增量，t_k为从初始状态到达第k个离散点所用的时间。

离散点信息包括该离散点的位置信息、路径速度、路径加速度以及运动时间，将已知的离散点信息排列成数组，便于进行轨迹插补计算。

在上述的一种机器人时间最优轨迹规划方法中，在上述步骤E中，先设定插补周期T_s，再根据生成的离散点信息，通过公式：

Δt＝nT_s-t_k

重新生成离散点；

其中，Δt为整倍数插补周期时间和距离最近的等距离散点的时间的差值，n为插补周期的整倍数，t_k为从初始状态到达第k个等距离散点所用的时间，s_kk为等时间离散的离散点序列，s_k为等距离散的离散点序列，v_k为第k个等距离散点的路径速度，u_k为第k个离散点的路径加速度。

根据已知的离散点信息，根据公式重新生成等时间分布的离散点序列，本离散点序列为插补序列，根据插补序列进行路径离散后，对每个离散点重新进行数据计算，计算出每个离散点的关节坐标，并且本次关节坐标与关节角速度、关节角加速度之间为关于插补时间的微分关系，即本次关节角速度为关节角度关于插补时间的一次微分，关节角加速度为关节坐标关于插补时间的二次微分。将上述关于路径的离散点序列与关于时间的离散点序列结合，能够进一步地细化机器人运动轨迹，保证轨迹生成的平滑以及机器人运动的准确度。

在上述的一种机器人时间最优轨迹规划方法中，在上述步骤A中，若运动空间为关节空间，所述空间对应计算方法包括关节运动位移计算以及关节运动位移关于路径长度的微分计算，若运动空间为笛卡尔空间，所述空间对应计算方法包括运动学逆解、雅可比矩阵以及海森矩阵。

若机器人做关节空间运动，直接计算出各关节在各离散点处的关节坐标，再进行微分获得关节角速度与关节角加速度；若为笛卡尔空间，需先采用运动学逆解将笛卡尔空间轨迹映射到关节空间，获取关节坐标，再采用雅可比矩阵以及海森矩阵进行计算获得关节角速度与关节角加速度。

由于获取的轨迹规划是被约束在笛卡尔空间内，而机器人运动是通过关节来实现，因为将笛卡尔空间转换为关节空间的方法计算较为复杂，且关节空间轨迹规划可以保证关节轨迹平滑连续，运动保持平稳，有效避免奇异形位，则可以通过将离散点的笛卡尔坐标转换为关节信息，再进行轨迹优化，降低计算难度。

与现有技术相比，本发明具有以下优点：

1、本方法中约束条件可以根据实际需要进行适当选择，不同的约束组合具有很强的实用性和广泛应用的适应性。

2、在计算最优路径速度时，先通过反向路径迭代计算离散点最优路径速度的最大值和最小值，再通过正向路径迭代计算实际允许路径速度，这样计算流程简单，计算量小，适合实时计算，并且无需考虑动力学奇异等特殊情况，算法稳定性强。

3、本方法通过两次生成不同的离散点序列，并将各离散点处的关节信息进行整合输出，能够进一步地细化机器人运动轨迹，保证轨迹平滑和精确性。

4、本方法在机器人加速段与正常运动段，保持相同的电机最大速度和力矩(或加速度)约束，能够有效地缩短运动时间，提高运动效率，并且机器人减速段中，通过降低约束最大值，可实现机器人平稳停止，防止冲击力过大引起机器人末端抖动，影响精度并对机器人造成损伤。

附图说明

图1是本发明的具体流程示意图。

图2是本发明中步骤C的正反向路径示意图。

图3是本发明中实施例一的运动轨迹曲线示意图。

图4是本发明中实施例三的运动轨迹曲线示意图。

图5是本发明中实施例四的运动轨迹曲线示意图。

具体实施方式

以下是本发明的具体实施例并结合附图，对本发明技术方案作进一步的描述，但本发明并不限于这些实施例。

实施例一：

机器人做笛卡尔空间运动，根据运动约束集合中选择关节空间角速度、关节空间力矩、笛卡尔空间线速度以及笛卡尔空间角速度为约束条件，本机器人时间最优轨迹规划方法包括：

建立离散点长度序列s_k：先获取任务路径，计算路径长度s并确定机器人位置运动与姿态运动的主轴与从轴关系，对任务路径进行等距离散，生成若干个离散点，并获取离散点的笛卡尔坐标；其中s_k＝[s₁,…s_k…,s_N],k∈[1,2,…,N],k为离散点编号；

A、获取运动学参数：对于笛卡尔空间运动，确定机器人的笛卡尔位置，笛卡尔位置包括：基于坐标系的位置信息和姿态信息，即xyzabc，其中，xyz为末端执行器相对于机器人基坐标系的位置信息，abc为末端执行器相对于机器人基坐标系的姿态信息；再根据已知位姿分量，采用运动学逆解将机器人坐标从笛卡尔空间映射到关节空间，并采用雅克比矩阵以及海森矩阵，计算运动学参数q、qs’、qs”，其中，q为关节坐标，qs’为关节角速度，qs”为关节角加速度。

具体的，已知各离散点长度序列s_k，通过对离散点求解关于路径长度s的一次微分和二次微分，可以得到：

s’_k＝d(s_k)/ds

s”_k＝d²(s_k)/ds²

根据s_k、s’_k、s”_k计算出空间运动位移r，r＝(x,y,z,a,b,c),设定位置矢量为^AP，用

来表示末端执行器的位置信息，其中，P表示末端执行器在空间的点，p_x、p_y、p_z是点P在参考坐标系{A}中的三个坐标分量，且分别等价于x,y,z，坐标系{A}表示任务轨迹所在坐标系；再通过角坐标系表示法计算末端执行器的姿态信息：

其中，

表示为旋转矩阵，上标A代表参考坐标系{A}，下标B代表被描述的坐标系{B}。根据末端执行器的位姿信息，采用机器人运动学逆解，计算各关节坐标q；

对空间运动位移r进行关于路径长度的一次微分rs’与二次微分rs”：

rs’＝d(r)/ds

rs”＝d²(r)/ds²

根据已知的空间运动位移r以及其微分，采用雅克比矩阵以及海森矩阵分别求解qs’、qs”，其中，qs’为关节角速度，qs”为关节角加速度。

上述运动位移计算、运动学逆解、雅可比矩阵以及海森矩阵均为现有技术。

B、确定约束条件：设定s、

为变量参数，且选取/>

为最优变量，根据变量参数建立时间最优目标函数/>

i＝1,…,N-1，其中，T为机器人沿最优轨迹运动所用总时间，N为离散点数量，t_k为第k段所用的时间，s为路径长度，/>

为路径速度，

为路径加速度。

再根据约束条件建立约束不等式，针对约束条件，一共有N-1个离散段数，相当于有N-1段子轨迹，任取其中一段进行如下计算，依旧保持一般性。

建立约束不等式方程组

V_j，min＝-V_j，max；

T_j,min≤τ(k)≤T_j,max,T_j,min＝-T_j,max；

0≤v(k)≤v_p,max；

0≤w(k)≤v_w,max；

其中，

为关节角速度，τ(k)为关节力矩,v(k)为笛卡尔空间线速度，w(k)为笛卡尔空间角速度，V_j,max为机器人关节的最大角速度，V_j,min为机器人关节的最小角速度；T_j,min为最小关节力矩，T_j,max为最大关节力矩，j表示关节，v_p,max为笛卡尔空间最大线速度，v_w,max为笛卡尔空间最大角速度。

(1)关节角速度

设某段轨迹机械臂的关节角速度为：

最小角速度是最大角速度的相反数，所以机械臂某关节的角速度约束为：

/>

其中，V_j，max为机器人关节的最大角速度，V_j，min为机器人关节的最小角速度。

(2)关节力矩

建立机器人力矩约束方程

n自由度机械臂动力学基本方程：

其中，q，

是关节位置，速度和加速度；M(q)是质量矩阵；C(q)表示离心力和科氏力矩阵；G(q)表示重力和库仑摩擦力；τ(k)是关节力矩矢量。

根据求导链式法则，得到关节速度

关节加速度/>

和路径长度s的关系：

并将其代入运动学方程。

其中，

M(s)＝M(q)qs’

C(s)＝M(q)qs”+qs’^TC(q)qs’

G(s)＝G(q)＝f(q)+g(q)

T_j，min为最小关节力矩，T_j，max为最大关节力矩，f(q)为库仑摩擦力，g(q)为重力。

已知动力学方程

采用待定系数法推导出公式中的动力学方程系数as，bs，cs，其中，as，bs，cs分别等价于M(s)、C(s)、G(s)，具体计算如下：

令

根据机器人逆向动力学函数计算关节力矩τ，逆向动力学函数如下：

输入参数q，/>

分别为关节坐标、关节角速度、关节角加速度，计算公式如下：

逆向动力学函数具体计算方式为现有技术。

(3)设笛卡尔空间角速度w(k)

笛卡尔空间最大角速度一般为给定值，且不为负数，笛卡尔空间角速度约束为：

0≤w(k)≤v_w，max；

(4)设笛卡尔空间线速度v(k)

笛卡尔空间最大线速度一般为给定值，且不为负数，笛卡尔空间线速度约束为：

0≤v(k)≤v_p，max；

C、求解最优变量实际允许最优解：

(a)先求解最优变量满足约束条件的最大值与最小值：基于约束不等式，设置起点的路径速度平方

以第N个离散点为起点，第1个离散点为终点，采用线性规划方法，在反向路径上依次迭代求解各离散点处最优变量路径速度平方/>

的最大值和最小值，其中，N为总的离散点个数。

具体的，根据第k+1个离散点路径速度平方

结合各种约束条件，求解第k个离散点路径速度平方，计算步骤主要包括：

(1)计算关节力矩约束的系数

a[i]＝(-T_j，max[i]-cs[i])/as[i]

b[i]＝-bs[i]/as[i]

c[i]＝(T_j，max[i]-cs[i])/as[i]

d[i]＝-bs[i]/as[i]

i＝1，...，n，n为关节个数。

(2)计算第k+1个离散点路径速度平方对应的系数

b[n+1]＝-1/(k·delta)

d[n+1]＝-1/(k·delta)

(3)计算第k个离散点路径速度的平方

A_ij＝d[i]-b[j]

B_ij＝-c[i]+a[j]

其中：i≠j

(4)加入关节角速度约束，计算第k个离散点路径速度的平方

(5)加入笛卡尔空间线速度和角速度，计算第k个离散点路径速度的平方

/>

temp2＝angle(rs′.a，rs′.b，rs′.c)

其中，rs′为笛卡尔空间运动位移s关于路径长度的一次微分，有xyzabc6个分量，angle为根据欧拉角速度求解空间姿态角速度函数，

为笛卡尔空间线速度约束下第k个离散点路径速度的平方，/>

为笛卡尔空间角速度约束下第k个离散点路径速度的平方。

最终求得第k个离散点路径速度平方的最大值与最小值。

(b)求解实际最优变量的最优解：根据已知最优变量的最大值、最小值以及约束条件，以第1个离散点为起点，沿正向路径逐点递推计算各离散点处实际允许的最优变量值。

根据最优变量的最大值、最小值，计算路径加速度

的第一区间，区间最大值

和最小值/>

的公式如下：

其中，

为当前离散点的路径速度平方，delta为离散点序列s_k在每个区间的增量。

已知关节空间的最大力矩、最小力矩以及动力学方程系数，计算关节允许的加速度

的第二区间，区间的最大值/>

和最小值/>

的公式如下：

上式中，T_j,max,T_j,min分别代表关节的最大力矩和最小力矩，as,bs,cs分别代表动力学方程系数。

实际路径加速度

的最大值取/>

与/>

之间的最小值；路径加速度/>

的最小值取/>

与/>

之间的最大值。

即：

/>

根据路径加速度求解路径速度，采用迭代方式，从第k个离散点求第k+1个离散点，计算公式如下：

正常运动时，包括加速和减速过程，均采用上述公式计算。若运动中停止，用下面公式计算：

D、生成离散点插补数组：已知离散点上最优变量

即路径速度v，根据下述公式计算路径加速度u和时间t。

t_k+1＝t_k+Δt

其中，v_k为第k个离散点的路径速度，Δt为相邻两个离散点之间的时间增量，t_k为从初始状态到达第k个离散点所用的时间，u_k为第k个离散点的路径加速度，通过计算生成离散点的路径位置、速度、加速度以及时间信息。

E、输出关于时间的关节信息：设定插补周期为T_s，从上述离散点信息内找到对应插补周期内各离散点信息，并通过公式计算每个插补周期的路径s数值：

Δt＝nT_s-t_k

其中，Δt为整倍数插补周期时间和距离最近的等距离散点的时间的差值，n为插补周期的整倍数，t_k为从初始状态到达第k个等距离散点所用的时间，s_kk为等时间离散的离散点序列，s_k为等距离散的离散点序列，v_k为第k个等距离散点的路径速度，u_k为第k个等距离散点的路径加速度。

重新生成等时间离散的离散点序列s_kk，通过插补计算得到笛卡尔空间位姿r，再运动学逆解求出关节角度最后通过时间微分计算出关节角速度以及关节角加速度。

实施例二

本实施例同实施例一的结构及原理基本相同不同之处在于：

(1)机器人运动为关节空间运动，在步骤A、获取关节信息中，通过对各离散点进行关于路径长度s的微分计算，获得一次微分s’_k与二次微分s”_k，对于关节空间运动，不需要求解笛卡尔坐标，不需要采用运动学逆解进行坐标映射。根据s_k、s’_k、s”_k进行关节运动位移q的计算，以及位移q关于路径长度s的微分计算，获得一次微分qs’与二次微分qs”,其中，q为关节角度，qs’为关节角速度，qs”为关节角加速度。

(2)运动约束集合中，仅选择关节空间角速度与关节空间力矩，不选择笛卡尔空间线速度和笛卡尔空间角速度。

实施例三

本实施例同实施例一的结构及原理基本相同，不同之处在于：将约束条件中的关节力矩替换为关节角加速度。

设机械臂在某段轨迹的关节角加速度是

且关节最大角加速度为给定值。最小角加速度是最大角加速度的相反数，机械臂关节的角加速度约束为：

其中，A_j,max为机器人关节的最大角加速度，A_j,min为机器人关节的最小角加速度。

实施例四

本实施例同实施例一的结构及原理基本相同，不一样的地方在于：机器人为多步连续运动，在步骤A生成任务路径时，需多增加一步对线性段和混合路径的分段离散过程，具体如下：

首先生成路径，将相邻两段路径进行混合：通过绝对值混合条件，计算相邻两条路径的拼接点，以拼接点为界，将路径划分为线性段和混合段，多个线性段和混合段连接在一起，构成一条完整运动路径。

其次，对整条运动路径离散化，由于线性段和混合段所用路径变量不同，需要分段离散化。线性段所用变量s为实际路径长度，混合段所用变量s为归一化值，取值[0,1]。

再次，若机器人运动为笛卡尔空间运动，则需根据变量s依次计算各离散点的一维位置、空间运动三维位姿变量、关节角度、关节角速度以及关节角加速度；若机器人运动为关节空间运动，则根据变量s计算各离散点的一维位置、关节角度、关节角速度以及关节角加速度。具体计算方式与实施例一一致。

并且在后续步骤B,C,D,E计算各离散点信息时，线性段与混合段需分开计算，涉及运动插补时，各段需按照各自的插补公式分别计算。

本文中所描述的具体实施例仅仅是对本发明精神作举例说明。本发明所属技术领域的技术人员可以对所描述的具体实施例做各种各样的修改或补充或采用类似的方式替代，但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。

尽管本文较多地使用了路径加速度、路径速度等术语，但并不排除使用其它术语的可能性。使用这些术语仅仅是为了更方便地描述和解释本发明的本质；把它们解释成任何一种附加的限制都是与本发明精神相违背的。

Claims (8)

1.一种机器人时间最优轨迹规划方法，包括获取并离散任务路径，确定运动空间，获取各离散点对应的空间坐标，其特征在于，所述规划方法还包括如下步骤：

A、获取运动学参数：获取离散点长度序列s_k，并求解关于路径长度的微分，采用空间对应计算方法，获得关于路径长度的运动学参数，运动学参数包括关节坐标、关节角速度以及关节角加速度；

B、确定约束条件：根据约束集合选定约束条件，设定s、

为变量参数，并建立目标函数与约束不等式，再选定/>

为最优变量，其中s为路径长度，/>

为路径长度s对时间的一阶微分，即路径速度，/>

为路径长度s对时间的二阶微分，即路径加速度；

C、求解最优变量实际允许最优解：以第N个离散点为起点，迭代计算反向路径上各离散点满足约束条件的最优变量的最大值与最小值，根据最优变量的最值和约束条件，以第1个离散点为起点，逐点递推计算正向路径上各离散点实际允许的最优变量的最优解，其中N为总的离散点个数，最优变量实际允许最优解的计算过程包括以下步骤：

a、根据已知路径速度的最值计算路径加速度的第一区间；确定约束条件，计算在所述约束条件下允许的路径加速度第二区间；结合第一区间与第二区间获得路径加速度的最值；

b、在机器人正常运动时，通过公式：

逐点计算实际允许的路径速度；机器人运动中途停止运动时，通过公式/>

逐点计算实际允许的路径速度，其中，k为离散点，/>

为路径速度的平方，/>

为最大路径加速度，/>

为最小路径加速度，delta为离散点序列s_k在每个区间的增量；

D、生成离散点插补数组：已知最优变量值的最优解，计算并生成离散点信息，形成插补数组，其中，离散点信息包括离散点位置、路径速度、路径加速度以及运动时间，根据最优路径速度，通过公式：

t_k+1＝t_k+Δt

计算并生成离散点信息；

其中，v_k为第k个离散点的路径速度，u_k为第k个离散点的路径加速度，delta为离散点序列在每个区间的增量，Δt为相邻两个离散点之间的时间增量，t_k为从初始状态到达第k个离散点所用的时间；

E、输出关于时间的关节信息：根据插补数组以及设定的插补周期，重新生成离散点，逐点计算并输出对应的关于时间的关节坐标、关节角速度以及关节角加速度。

2.根据权利要求1所述的一种机器人时间最优轨迹规划方法，其特征在于，所述约束集合包括：关节空间角速度、关节空间角加速度、关节空间力矩、笛卡尔空间运动线速度、笛卡尔空间线加速度、笛卡尔空间运动角速度以及笛卡尔空间角加速度。

3.根据权利要求2所述的一种机器人时间最优轨迹规划方法，其特征在于，所述约束条件必须包括关节空间角速度，且至少包括关节空间角加速度和关节空间力矩的其中一项。

4.根据权利要求3所述的一种机器人时间最优轨迹规划方法，其特征在于，若所述约束条件包括关节力矩，采用待定系数法，计算动力学方程系数。

5.根据权利要求1或2或3或4所述的一种机器人时间最优轨迹规划方法，其特征在于，在上述步骤C中，所述迭代计算采用线性规划方法进行最优变量求解。

6.根据权利要求2所述的一种机器人时间最优轨迹规划方法，其特征在于，在上述步骤C中，所述反向路径和正向路径的始末点路径速度允许非零。

7.根据权利要求1所述的一种机器人时间最优轨迹规划方法，其特征在于，在上述步骤E中，先设定插补周期T_s，再根据生成的离散点信息，通过公式：

Δt＝nT_s-t_k

重新生成离散点；

其中，Δt为整倍数插补周期时间和距离最近的等距离散点的时间的差值，n为插补周期的整倍数，t_k为从初始状态到达第k个等距离散点所用的时间，s_kk为等时间离散的离散点序列，s_k为等距离散的离散点序列,v_k为第k个等距离散点的路径速度，u_k为第k个离散点的路径加速度。

8.根据权利要求1所述的一种机器人时间最优轨迹规划方法，其特征在于，在上述步骤A中，若运动空间为关节空间，所述空间对应计算方法包括关节运动位移计算以及关节运动位移关于路径长度的微分计算，若运动空间为笛卡尔空间，所述空间对应计算方法包括运动学逆解、雅可比矩阵以及海森矩阵。

Images