CN110196590A - 一种用于机器人路径跟踪的时间最优轨迹规划系统及方法 - Google Patents

Abstract

本发明公开了一种用于机器人路径跟踪的时间最优轨迹规划系统及方法，该系统包括路径处理单元、轨迹规划单元和指令生成单元；路径处理单元执行路径位移计算处理、路径函数拟合处理、路径微分系数计算处理和路径规划点选取处理；轨迹规划单元用于规划最优轨迹；指令生成单元用于生成控制指令等。该方法包括以下步骤：S1路径位移计算处理；S2获得路径函数；S3路径微分系数计算处理；S4路径规划点选取；S5求解规划点路径边界速度及加速度；S6规划时间最优轨迹；S7获得每个控制周期路径位移；S8得到每个时刻的各轴角位移；S9各轴角位移将转化为控制指令脉冲信号等等。本发明减少路径跟踪时间和轨迹规划的计算时间。

Description

一种用于机器人路径跟踪的时间最优轨迹规划系统及方法

技术领域

本发明涉及机器人自动控制技术领域，具体涉及一种用于机器人路径跟踪的时间最优轨迹规划系统及方法。

背景技术

在利用机器人进行搬运，装配，机加工等任务时，使用机器人进行任务路径跟踪时，为了提高机器人的工作效率，应该使机器人在高速下运行。。传统的S型速度规划，并不能发挥电机的全部性能。为了使机器人始终处于允许的最大速度下工作，使机器人在临界条件下运动的轨迹规划方法称为时间最优轨迹规划方法。

自1980年代以来，时间最优控制方法已经在多篇论文中被提出并众所周知，其中应用最多的数值积分方法在面对复杂的动力学模型计算时，特别是非线性动力学模型时难以积分，导致了其只能应用于简单的动力学模型的情况，这导致了动力学模型的不准确性。同时数值积分每个控制周期都需要通过求解动力学和运动学约束来进行规划，大大加大了轨迹规划的计算时间。

发明内容

本发明的目的是提供一种用于机器人路径跟踪的时间最优轨迹规划系统及方法，用于复杂非线性动力学模型时的时间最优轨迹规划，用于在线规划机器人的轨迹，减少路径跟踪时间和轨迹规划的计算时间。

本发明采用以下的技术方案。

一种用于机器人路径跟踪的时间最优轨迹规划系统，该系统包括路径处理单元、轨迹规划单元和指令生成单元；

其中，路径处理单元执行路径位移计算处理、路径函数拟合处理、路径微分系数计算处理和路径规划点选取处理；

轨迹规划单元用于在路径位移速度平面规划机器人的时间最优轨迹，假设规划点之间为匀加速运动，通过匀加速运动方程进行规划；

指令生成单元用于根据获得的最优轨迹，获得每个控制周期的路径位移，从而根据路径函数获得各轴的角位移，再转化为控制指令脉冲用于机器人的控制。

一种用于机器人路径跟踪的时间最优轨迹规划方法，包括以下步骤：

S1.获取任务路径；

S2.利用路径处理单元执行路径位移计算处理，获得路径位移；

S3.路径函数拟合处理，获得路径函数；

S4.路径微分系数计算处理；

S5.选取路径规划点，减少需要规划的点数以减少规划时间；

S6.在轨迹规划单元中根据约束条件求解规划点路径边界速度及相应加速度；

S7.根据约束条件、路径边界速度及相应加速度通过匀加速运动方程规划时间最优轨迹；

S8.利用指令生成单元获得每个控制周期路径位移；

S9.每个控制周期的位移代入路径函数得到每个时刻的各轴角位移；

S10.各轴角位移将转化为控制指令脉冲信号，发送到机器人控制柜中对机器人进行控制。

进一步的，步骤S2所述的路径位移计算处理是通过从任务路径起点开始，沿着机器人末端路径移动方向得到的路径弧长，并将路径弧长进行归一化，作为路径位移，用s表示路径位移。

进一步的，步骤S3的路径函数拟合处理是通过分段最小二乘拟合机器人的各关节角位移为θ₁，θ₂，…，θ_n和路径位移s，从而得到各关节路径函数为θ₁(s)，θ₂(s)，…，θ_n(s)，n表示机器人的关节数，θ_n(s)为拟合的第n关节角位移θ_n跟路径位移s的分段函数，各关节角位移的集合为q＝[θ₁，θ₂，…，θ_n]^T，各关节路径函数的集合为q(s)＝[θ₁(s)，θ₂(s)，…，θ_n(s)]^T。

进一步的，步骤S4的路径微分系数计算处理是通过对各关节路径函数拟合得到的表达式进行求导，从而获得用于表示路径曲率的二阶微分函数q′(s)＝[θ₁′(s)，θ₂′(s)，…，θ_n′(s)]^T和用于表示路径曲率变化率三阶微分函数q″(s)＝[θ₁″(s)，θ₂″(s)，…，θ_n″(s)]^T。

进一步的，步骤S5的路径规划点选取如下：

S51.设定曲率变化阈值∈；

S52.以路径起点即第1个采样点为第1个规划点，从路径起点1开始沿着路径位移增大的方向搜索，直到第r个路径采样点时各关节的角度对于路径位移的差分满足以下条件时，通过如下公式选取第r-1个采样点作为第2个规划点：

max|q_计算′-q′(s(1))|＞∈

式中，q(s(1))表示在第1个规划点时的各关节角度，q(s(r))表示在第r个规划点时的各关节角度，s(1)表示在第1个采样点时的路径位移，s(r)表示在第r个路径采样点时的路径位移，q_计算′表示第r个路径采样点和第1个路径采样点的角度的计算差分，q′(s(1))表示二阶微分函数在第1个路径采样点时的值；

S53.从第2个规划点开始，根据步骤S52沿着路径位移增大的方向搜索，找到第3个规划点，以此类推，最终找到用于规划的N个规划点；

通过步骤S51-S53对路径进行精简，从而减少需要规划的点数，减少规划时间。

进一步的，步骤S6的求解过程包括以下过程：

根据约束条件

τ_min(k)≤τ(k)≤τ_max(k) (1)

其中：k表示第k个规划点，τ(k)表示n×1阶关节力矩矢量，τ_min(k)表示第k个规划点的最小约束力矩，τ_max(k)表示第k个规划点的最大约束力矩，表示第k个规划点的最小约束速度，表示第k个规划点的速度，表示第k个规划点的最大约束速度，表示第k个规划点的最大约束加速度，表示第k个规划点的最小约束加速度，表示第k个规划点的关节速度，其等价于路径函数q(s)关于时间t的一阶导数在的s(k)值，表示第k个规划点的关节加速度，其等价于路径函数q(s)关于时间t的二阶导数在的s(k)值，q(k)等价于q(s(k))表示在第k个规划点时的各关节角度，等价于等价于

由如下的动力学方程求得的各关节力矩：

其中，τ(k)表示n×1阶关节力矩矢量，即[τ₁(k)τ₂(k)…τ_n(k)]^T，其中n表示机器人关节数，τ_n(k)表示第n个关节力矩，上标T表示转置；M(q(k))表示n×n阶正定质量矩阵；B(q(k))表示n×n(n-1)/2阶的哥氏力系数矩阵，表示n(n-1)/2×1阶的关节速度积矢量，即 表示在第k个规划点时的第n关节角度；C(q(k))是n×n阶离心力系数矩阵，而是n×1阶矢量，即F_v表示粘滞摩擦力向量；F_c表示库伦摩擦力向量；G(q(k))表示在第k个规划点时的各关节角度重力向量；q(k)表示n×1阶关节角度矢量，即[θ₁(k)θ₂(k)…θ_n(k)]，表示n×1阶关节速度矢量，即 表示n×1阶关节加速度矢量，即sign表示符号函数；

根据链式规则，得到关节速度加速度和路径位移的关系：

其中，q′(s)和q″(s)为路径位移s的二阶和三阶微分函数，表示路径位移对时间t的一阶微分，即路径速度，表示路径位移对时间t的二阶微分，即路径加速度；

对于路径规划点k

其中，q′(s(k))和q″(s(k))分别为路径规划点k的路径位移的二阶和三阶微分函数，表示路径规划点k的路径位移对时间t的一阶微分，即路径速度，表示路径规划点k的路径位移对时间t的二阶微分，即路径加速度

将(4)(5)代入不等式约束(1)(2)(3)，得路径空间不等式约束为

其中k是规划点，k＝1～N，m(s(k))＝M(q(s(k)))q′(s(k))、c(s(k))＝M(q(s(k)))q″(s(k))+B(q(s(k))，q′(s(k)))q′(s(k))+C(q(s(k)))(q′(s(k)))²、f(s(k))＝F_v(q(s(k)))q′(s(k))都是用于简化动力学方程的中间变量；m(s(k))、c(s(k))、B(q(s(k))、q′(s(k)))、f(s(k))、Fv(q(s(k)))和g(s(k))均为用于简化动力学方程的中间变量；

令

其中，表示所有关节的力矩和加速度约束下的路径加速度上限；

令

其中，表示所有关节的力矩和加速度约束下的路径加速度下限；

当时可求得满足力矩和加速度约束的路径边界速度而且最大路径速度还受速度约束的限制，用表示速度约束下的路径边界速度，以路径速度区间速度作为初始搜索区间，f(X)＝|α(s(k)，X)-β(s(k)，X)|作为判断函数，式中X为表示路径速度的函数自变量，通过黄金分割法进行求解满足力矩、速度和加速度限制的第k个规划点的路径边界速度

通过匀加速运动方程求解相应的路径边界速度的加速度

进一步的，黄金分割法的步骤如下：

S61.判断当路径位移为s(k)，路径边界速度为时，代入式(6)(7)得到的路径加速度上限是否大于路径加速度下限如果是，则为路径边界速度否则继续步骤S62进行黄金分割搜索；

S62.设置求解精度ε；判断函数f(X)＝|α(s(k)，X)-β(s(k)，X)|，初始搜索区间

S63.计算黄金分割点λ＝a+0.382(b-a)，μ＝a+0.618(b-a)，式中a、b、λ和μ是用于表示区间位置的变量，0.382，0.618是黄金分割的比例；

S64.判断黄金分割区间长度λ-μ是否满足精度要求，即λ-μ＜ε，如果满足，则令作为路径边界速度；如果不满足，继续转到步骤S64；

S65.将黄金分割点λ、μ代入判断函数f(X)，如果f(λ)＞f(μ)，则令a＝λ，转到步骤S63-S64重新求黄金分割点，如果f(λ)＜f(μ)，则令b＝μ，转到步骤S63-S64重新求黄金分割点；

S66.重复步骤S63-S65，直到黄金分割区间长度λ-μ＜ε，则令作为路径边界速度。

进一步的，步骤S7的规划时间最优轨迹包括以下步骤：

步骤S71.从路径位移速度平面初始状态点(0，0)以最大加速度通过匀加速运动方程 沿着路径位移增加的方向往前计算，获得的规划曲线为α-profile，其中，已经规划的路径位移相应的最优速度用表示，sqrt()表示平方根函数，表示在路径位移为s(k-1)和路径速度为时的最大/最小路径加速度；

将代入式(6)得到路径加速度上限即为路径最大加速度，将代入式(7)得到路径加速度下限即为路径最小加速度；

直到第a个规划点时，其路径最大加速度用表示，如果第a个规划点的路径速度第a个规划点的路径边界速度则转到步骤S73；如果或a＝N且则转到步骤S72；如果a＝N且则转到步骤S77；

步骤S72.从路径位移速度平面最终状态点(1，0)沿着路径位移减少的方向往后计算，具体的，采用黄金分割法往后计算已知一规划点k的路径速度求解路径位移减少方向的相邻规划点的最大路径速度，具体的，从后一个规划点k-1的路径速度为0时的状态点(s(k-1)，0)沿着增大的方向按步长length搜索，直到在第j个步长时，有 式中，表示相乘，sminsk-1，j·length为路径位移为sk-1和路径速度为j·length代入式(7)得到的路径最小加速度，以[0，j·length]为初始搜索区间，为判断函数，其中X为表示路径速度的函数自变量，根据步骤S62-S66的黄金分割法，搜索后一个规划点k-1的路径速度通过上述黄金分割法，路径位移速度平面最终状态点(1，0)沿着路径位移减少的方向往后计算，直到第h个规划点时：如果路径速度第h个规划点的路径边界速度则转到步骤S73；如果路径速度第h个规划点的路径位移相应的最优速度则转到步骤S77；该步骤往后计算获得的规划曲线为β-profile；

步骤S73.如果规划点a的路径边界速度的加速度规划点a的路径最小加速度或则转到步骤S74；如果则该点为满足约束条件的边界可用点；沿着位移增大的方向向前搜索，直到第b个规划点时，如果规划点b的路径边界速度的加速度规划点b的路径最小加速度或规划点b的路径最大加速度令a＝b，转到步骤S74；如果b＝N，转到步骤S77；

该步骤所有边界可用点串联得到的曲线为γ-profile；

步骤S74.如果则转到步骤S75；如果则沿着位移增大的方向向前搜索，直到第c个规划点时，如果规划点c的路径边界速度的加速度规划点c的路径最小加速度且规划点c的路径最大加速度则转到步骤S76；如果从该规划点c处用步骤S72的黄金分割法沿着路径位移减少的方向往后计算，直到第d个规划点时，第d个规划点的路径速度第d个规划点的路径位移相应的最优速度往后计算结束，继续从规划点c处以最大路径加速度往前计算，直到第e个规划点时，如果第e个规划点的路径速度则令a＝e，转到步骤S73；如果第e个规划点的路径位移相应的最优速度则转到步骤S77；

该步骤中，往后计算获得的曲线为β-profile，往前计算获得的曲线为α-profile；

步骤S75.在a规划点用最大路径加速度往前计算，直到第f个规划点时，如果第f个规划点的路径速度则令a＝f，转到步骤S73；如果第f个规划点的路径位移相应的最优速度则转到步骤S77；

该步骤中，往前计算获得的曲线为α-profile；

步骤S76.从该规划点处用步骤S72的黄金分割法沿着路径位移减少的方向往后计算，直到第g个规划点时，第g个规划点的路径速度第g个规划点的路径位移相应的最优速度往后计算结束，继续往前规划，令a＝c，转到步骤S73；

该步骤中，往后计算获得的曲线为β-profile

步骤S77.将所有获得的α-profile，γ-profile和β-profile按顺序串联起来，即为路径位移-速度平面的时间最优规划曲线。

进一步的，步骤S8是通过匀加速运动方程获得每个脉冲控制周期T的路径位移s(T)，具体包括以下步骤：

步骤S81.根据步骤S7时间最优规划曲线，利用以下公式求得每个路径规划点的加速度和到达规划点的时间：

其中，t(k)表示到达路径规划点k所需的运行时间；

步骤S82.根据每个规划点k的路径位移s(k)，路径速度路径加速度和到达时间t(k)，通过以下步骤求解每个控制周期的路径位移s(T)：

S821.设定控制周期时间T；

S822.从第1个规划点开始，沿着路径位移增大方向搜索，直到第k个路径规划点时，t(k)＞T；

S823.计算控制周期时间T和到达第k-1个规划点的时间间隔Δt＝T-t(k-1)；

S824.通过以下的匀加速运动方程计算到达每个控制周期时间T的路径位移：

与现有技术相比，本发明的有益效果是：本发明有序减少用于规划的规划点数，减少规划时间，同时规划的结果能使机器人在满足约束的条件下在最小的工作时间内工作。

附图说明

图1为本发明的一种用于机器人路径跟踪的时间最优轨迹规划方法的流程图；

图2为本实施例的任务路径图；

图3为本实施例进行规划点选取后的任务路径图。

具体实施方式

下面通过具体实例对本发明的目的作进一步详细地描述，实例不能在此一一赘述，但本发明的实施方式并不因此限定于以下实例。

如图1所示的一种用于机器人路径跟踪的时间最优轨迹规划系统，该系统包括路径处理单元、轨迹规划单元和指令生成单元；

其中，路径处理单元执行路径位移计算处理、路径函数拟合处理、路径微分系数计算处理和路径规划点选取处理；

轨迹规划单元用于在路径位移速度平面规划机器人的最优轨迹，假设规划点之间为匀加速运动，通过匀加速运动方程进行规划；

指令生成单元用于根据获得的最优轨迹，获得每个控制周期的路径位移，从而根据路径函数获得各轴的角位移，再转化为控制指令脉冲用于机器人的控制。

如图1所示的一种用于机器人路径跟踪的时间最优轨迹规划方法，包括以下步骤：

S1.获取任务路径

在本实施例中，机器人通过示教器示教如图2所示的任务路径，图中圆点表示每个脉冲控制周期的路径采样点。

S2.计算路径位移s

通过从任务路径起点开始，沿着末端路径移动方向计算路径弧长，并将路径弧长进行归一化，作为路径位移。用s表示路径位移，对于任意采样周期t有s(t)∈[0，1]。

以图2为例，以图2的路径起点开始，计算路径弧长，路径采样点1和路径采样点2的路径弧长为：式中，L_1，2表示路径采样点1和路径采样点2的之间的路径弧长，X()表示路径采样点在笛卡尔坐标系中X轴方向的位置，Y()表示路径采样点在笛卡尔坐标系中Y轴方向的位置，Z()表示路径采样点在笛卡尔坐标系中Z轴方向的位置；

同样可以求得其他路径采样点之间的路径弧长，最后得到任务路径的弧长为：

L＝L_1，2+L_2，3+…+L_N-1，N

式中，N表示最后一个路径采样点；

对任务路径进行归一化，作为路径弧长，则任意路径采样点即任意采样周期t的路径位移为：

s(t)＝(L_1，2+L_2，3+…+L_t-1，t)/L

S3.拟合路径函数q(s)

通过分段最小二乘法拟合各关节角位移q＝[θ₁，θ₂，…，θ_n]^T和路径位移s。

以图2的路径为例，拟合关节1的角位移θ1跟路径位移s具体做法包括以下步骤：

S31.设置精度ε＝0.00001；

S32.选取三次多项式进行拟合θ₁(s)＝a₀+a₁s+a₂s²，式中a₀、a₁、a₂为待求解的多项式系数，θ₁(s)为拟合的第一关节角位移θ₁跟路径位移s的函数的第一个分段函数；

S33.从路径采样点1开始，沿着路径位移增大方向，直到路径采样点z，参数式中矩阵

s(1)表示路径采样点1的路径位移；s(2)表示路径采样点2的路径位移；s(z)表示路径采样点n的路径位移，θ₁(s(1))路径采样点1的关节1的角位移，θ₁(s(2))路径采样点2的关节2的角位移，θ₁(s(z))路径采样点z的关节z的角位移，最小二乘系数向量A＝[a₀，a₁，a₂]^T，其中上标T表示转置，A＝S\Q，\表示矩阵右除，mean()是求解矩阵里面的平均值的函数，W是用来判断拟合路径函数误差是否符合精度要求的参数；

S34.令

求得的a₀，a₁，a₂作为拟合函数拟合q₁(s)＝a₀+a₁s+a₂s²的系数，并且拟合函数为路径位移区间[s(1)，s(z)]的关节1的路径函数。

S35.根据步骤S31-S34，求解路径位移区间[0，1]内除了区间[s(1)，s(z)]的其他区间的路径函数，以及其他关节的分段路径函数，最终得到机器人所有关节的路径函数q(s)，其中，区间中0表示路径位移为0，是路径起点；1表示路径位移为1，是路径终点。

S4.计算路径微分q′(s)和q″(s)

通过对路径函数拟合得到的分段多项式进行求导，获得的二阶微分函数q′(s)用于表示路径曲率，三阶微分函数q″(s)用于表示路径曲率变化率。

S5.根据路径微分q′(s)，以及曲率阈值选取用于规划的路径采样点

由于路径采样点很多，将其都进行规划计算需要很多时间，因此需要进行选取，选取步骤如下：

S51.设定曲率变化阈值∈＝0.01；

S52.以路径起点即第1个采样点为第1个规划点，从路径起点1开始沿着路径位移增大的方向搜索，直到第r个路径采样点时各关节的角度对于路径位移的差分满足以下条件时，通过如下公式选取r-1作为第2个规划点：

max|q_计算′-q′(s(1))|＞∈

式中，q(s(1))表示在第1个规划点时的各关节角度，q(s(r))表示在第r个规划点时的各关节角度，s(1)表示在第1个采样点时的路径位移，s(r)表示在第r个路径采样点时的路径位移，q_计算′表示第r个路径采样点和第1个路径采样点的角度的计算差分，q′(s(1))表示二阶微分函数在第1个路径采样点时的值；

S53.从第2个规划点开始，根据步骤S52沿着路径位移增大的方向搜索，找到第3个规划点，以此类推，最终找到用于规划的N个规划点；

通过步骤S51-S53对路径进行精简，从而减少需要规划的点数，减少规划时间。图3为对图2进行精简后的任务路径，图3中的X表示规划点。

S6.求解规划点路径边界速度及相应加速度

根据约束条件：

τ_min(k)≤τ(k)≤τ_max(k) (1)

其中：k表示第k个规划点，τ(k)表示n×1阶关节力矩矢量，τ_min(k)表示第k个规划点的最小约束力矩，τ_max(k)表示第k个规划点的最大约束力矩，表示第k个规划点的最小约束速度，表示第k个规划点的速度，表示第k个规划点的最大约束速度，表示第k个规划点的最大约束加速度，表示第k个规划点的最小约束加速度，表示第k个规划点的关节速度，其等价于路径函数q(s)关于时间t的一阶导数在的s(k)值，表示第k个规划点的关节加速度，其等价于路径函数q(s)关于时间t的二阶导数在的s(k)值，q(k)等价于q(s(k))表示在第k个规划点时的各关节角度，等价于 等价于

由如下的动力学方程求得的各关节力矩：

其中，τ(k)表示n×1阶关节力矩矢量，即[τ₁(k)τ₂(k)…τ_n(k)]^T，其中n表示机器人关节数，τ_n(k)表示第n个关节力矩，上标T表示转置；M(q(k))表示n×n阶正定质量矩阵；B(q(k))表示n×n(n-1)/2阶的哥氏力系数矩阵，表示n(n-1)/2×1阶的关节速度积矢量，即 表示在第k个规划点时的第n关节角度；C(q(k))是n×n阶离心力系数矩阵，而是n×1阶矢量，即F_v表示粘滞摩擦力向量；F_c表示库伦摩擦力向量；G(q(k))表示在第k个规划点时的各关节角度重力向量；q(k)表示n×1阶关节角度矢量，即[θ₁(k)θ₂(k)…θ_n(k)]，表示n×1阶关节速度矢量，即 表示n×1阶关节加速度矢量，即sign表示符号函数；

根据链式规则，得到关节速度加速度和路径位移的关系：

其中，q′(s)和q″(s)为路径位移s的二阶和三阶微分函数，表示路径位移对时间t的一阶微分，即路径速度，表示路径位移对时间t的二阶微分，即路径加速度；

对于路径规划点k，有：

其中，q′(s(k))和q″(s(k))分别为路径规划点k的路径位移的二阶和三阶微分函数，表示路径规划点k的路径位移对时间t的一阶微分，即路径速度，表示路径规划点k的路径位移对时间t的二阶微分，即路径加速度；

将(4)(5)代入不等式约束(1)(2)(3)，得路径空间不等式约束为：

其中k是规划点，k＝1～N，m(s(k))＝M(q(s(k)))q′(s(k))、c(s(k))＝M(q(s(k)))q″(s(k))+B(q(s(k))，q′(s(k)))q′(s(k))+C(q(s(k)))(q′(s(k)))²、f(s(k))＝F_v(q(s(k)))q′(s(k))都是用于简化动力学方程的中间变量；m(s(k))、c(s(k))、B(q(s(k))、q′(s(k)))、f(s(k))、F_v(q(s(k)))和g(s(k))均为用于简化动力学方程的中间变量；

令

其中，表示所有关节的力矩和加速度约束下的路径加速度上限；

令

其中，表示所有关节的力矩和加速度约束下的路径加速度下限。

当时可求得满足力矩和加速度约束的路径边界速度而且最大路径速度还受速度约束的限制，用表示速度约束下的路径边界速度，以路径速度区间速度作为初始搜索区间，f(X)＝|α(s(k)，X)-β(s(k)，X)|作为判断函数，式中X为表示路径速度的函数自变量，通过黄金分割法进行求解满足力矩、速度和加速度限制的第k个规划点的路径边界速度

所述黄金分割法的步骤如下：

S61.判断当路径位移为s(k)，路径边界速度为时，代入式(6)(7)得到的路径加速度上限是否大于路径加速度下限如果是，则为路径边界速度否则继续步骤S62进行黄金分割搜索；

S62.设置求解精度ε；判断函数f(X)＝|α(s(k)，X)-β(s(k)，X)|，初始搜索区间

S63.计算黄金分割点λ＝a+0.382(b-a)，μ＝a+0.618(b-a)，式中a、b、λ和μ是用于表示区间位置的变量，0.382，0.618是黄金分割的比例；

S64.判断黄金分割区间长度λ-μ是否满足精度要求，即λ一μ＜ε，如果满足，则令作为路径边界速度；如果不满足，继续转到步骤S64；

S65.将黄金分割点λ、μ代入判断函数f(X)，如果f(λ)＞f(μ)，则令a＝λ，转到步骤S63-S64重新求黄金分割点，如果f(λ)＜f(μ)，则令b＝μ，转到步骤S63-S64重新求黄金分割点；

S66.重复步骤S63-S65，直到黄金分割区间长度λ-μ＜ε，则令作为路径边界速度；

通过匀加速运动方程求解相应的路径边界速度的加速度

进一步的，步骤S7的规划时间最优轨迹包括以下步骤：

步骤S71.从路径位移速度平面初始状态点(0，0)以最大加速度通过匀加速运动方程 沿着路径位移增加的方向往前计算，获得的规划曲线为α-profile，其中，已经规划的路径位移相应的最优速度用表示，sqrt()表示平方根函数，表示在路径位移为s(k-1)和路径速度为时的最大/最小路径加速度；

将代入式(6)得到路径加速度上限即为路径最大加速度，将代入式(7)得到路径加速度下限即为路径最小加速度；

直到第a个规划点时，其路径最大加速度.用表示，，如果第a个规划点的路径速度第a个规划点的路径边界速度则转到步骤S73；如果或a＝N且则转到步骤S72；如果a＝N且则转到步骤S77；

步骤S72.从路径位移速度平面最终状态点(1，0)沿着路径位移减少的方向往后计算，具体的，采用黄金分割法往后计算已知一规划点k的路径速度求解路径位移减少方向的相邻规划点的最大路径速度，具体的，从后一个规划点k-1的路径速度为0时的状态点(s(k-1)，0)沿着增大的方向按步长length搜索，直到在第j个步长时，有 式中，·表示相乘，为路径位移为s(k-1)和路径速度为j·length代入式(7)得到的路径最小加速度，以[0，j·length]为初始搜索区间，为判断函数，其中X为表示路径速度的函数自变量，根据步骤S62-S66的黄金分割法，搜索后一个规划点k-1的路径速度通过上述黄金分割法，路径位移速度平面最终状态点(1，0)沿着路径位移减少的方向往后计算，直到第h个规划点时：如果路径速度第h个规划点的路径边界速度则转到步骤S73；如果路径速度.第h个规划点的路径位移相应的最优速度则转到步骤S77；该步骤往后计算获得的规划曲线为β-profile；

步骤S73.如果规划点a的路径边界速度的加速度规划点a的路径最小加速度或则转到步骤S74；如果则该点为满足约束条件的边界可用点；沿着位移增大的方向向前搜索，直到第b个规划点时，如果规划点b的路径边界速度的加速度规划点b的路径最小加速度或规划点b的路径最大加速度令a＝b，转到步骤S74；如果b＝N，转到步骤S77；

该步骤所有边界可用点串联得到的曲线为γ-profile；

步骤S74.如果则转到步骤S75；如果则沿着位移增大的方向向前搜索，直到第c个规划点时，如果规划点c的路径边界速度的加速度规划点c的路径最小加速度且规划点c的路径最大加速度则转到步骤S76；如果从该规划点c处用步骤S72的黄金分割法沿着路径位移减少的方向往后计算，直到第d个规划点时，第d个规划点的路径速度第d个规划点的路径位移相应的最优速度往后计算结束，继续从规划点c处以最大路径加速度往前计算，直到第e个规划点时，如果第e个规划点的路径速度则令a＝e，转到步骤S73；如果第e个规划点的路径位移相应的最优速度则转到步骤S77；

该步骤中，往后计算获得的曲线为β-profile，往前计算获得的曲线为α-profile；

步骤S75.在a规划点用最大路径加速度往前计算，直到第f个规划点时，如果第f个规划点的路径速度则令a＝f，转到步骤S73；如果第f个规划点的路径位移相应的最优速度则转到步骤S77；

该步骤中，往前计算获得的曲线为α-profile；

步骤S76.从该规划点处用步骤S72的黄金分割法沿着路径位移减少的方向往后计算，直到第g个规划点时，第g个规划点的路径速度第g个规划点的路径位移相应的最优速度往后计算结束，继续往前规划，令a＝c，转到步骤S73；

该步骤中，往后计算获得的曲线为β-profile；

步骤S77.将所有获得的α-profile，γ-profile和β-profile按顺序串联起来，即为路径位移-速度平面的时间最优规划曲线。

S8.获得每个控制周期路径位移s(T)

通过匀加速运动方程获得每个脉冲周期T的路径位移s(T)，具体包括以下步骤：

步骤S81.根据步骤S7时间最优规划曲线，利用一下公式求得每个路径规划点的加速度和到达规划点的时间：

其中，t(k)表示到达路径规划点k所需的运行时间；

步骤S82.根据每个规划点k的路径位移s(k)，路径速度路径加速度和到达时间t(k)，通过以下步骤求解每个控制周期的路径位移s(T)：

S821.设定控制周期时间T；

S822.从第1个规划点开始，沿着路径位移增大方向搜索，直到第k个路径规划点的t(k)＞T；

S823.计算控制周期时间T和到达第k-1个规划点的时间间隔Δt＝T-t(k-1)。

S824.通过以下的匀加速运动方程计算到达每个控制周期时间T的路径位移：

S9.根据路径函数q(s)获得各轴角位移

将每个控制周期的位移s(T)代入路径函数q(s)可得到每个时刻的各轴角位移q(s(T))。

S10.转化为控制指令脉冲

将各轴角位移q(s(T))通过减速器传动比以及电机参数转化为电机的脉冲信号，用于机器人的伺服电机的控制。

步骤S1-S10示例性描述了本发明的一种用于机器人路径跟踪的时间最优轨迹规划方法的实施例。但是应当理解，本发明不限于所公开的示例性实施例。以下权利要求的范围被赋予最广泛的解释，从而涵盖所有这种修改以及等同的结构和功能。

Claims (10)

1.一种用于机器人路径跟踪的时间最优轨迹规划系统，其特征在于，该系统包括路径处理单元、轨迹规划单元和指令生成单元；

其中，路径处理单元执行路径位移计算处理、路径函数拟合处理、路径微分系数计算处理和路径规划点选取处理；

轨迹规划单元用于在路径位移速度平面规划机器人的时间最优轨迹，假设规划点之间为匀加速运动，通过匀加速运动方程进行规划；

指令生成单元用于根据获得的最优轨迹，获得每个控制周期的路径位移，从而根据路径函数获得各轴的角位移，再转化为控制指令脉冲用于机器人的控制。

2.一种利用权利要求1所述系统的用于机器人路径跟踪的时间最优轨迹规划方法，其特征在于，包括以下步骤：

S1.获取任务路径；

S2.路径处理单元执行路径位移计算处理，获得路径位移；

S3.路径函数拟合处理，获得路径函数；

S4.路径微分系数计算处理；

S5.选取路径规划点，减少需要规划的点数以减少规划时间；

S6.在轨迹规划单元中根据约束条件求解规划点路径边界速度及相应加速度；

S7.根据约束条件、路径边界速度及相应加速度通过匀加速运动方程规划时间最优轨迹；

S8.利用指令生成单元获得每个控制周期路径位移；

S9.每个控制周期的位移代入路径函数得到每个时刻的各轴角位移；

S10.各轴角位移将转化为控制指令脉冲信号，发送到机器人控制柜中对机器人进行控制。

3.根据权利要求2所述的一种用于机器人路径跟踪的时间最优轨迹规划方法，其特征在于，步骤S2所述的路径位移计算处理是通过从任务路径起点开始，沿着机器人末端路径移动方向得到路径弧长，并将路径弧长进行归一化，作为路径位移，用s表示路径位移。

4.根据权利要求2所述的一种用于机器人路径跟踪的时间最优轨迹规划方法，其特征在于，步骤S3的路径函数拟合处理是通过分段最小二乘拟合机器人的各关节角位移为θ₁，θ₂，…，θ_n和路径位移s，从而得到各关节路径函数为θ₁(s)，θ₂(s)，…，θ_n(s)，n表示机器人的关节数，θ_n(s)为拟合的第n关节角位移θ_n跟路径位移s的分段函数，各关节角位移的集合为q＝[θ₁，θ₂，…，θ_n]^T，各关节路径函数的集合为q(s)＝[θ₁(s)，θ₂(s)，…，θ_n(s)]^T。

5.根据权利要求2所述的一种用于机器人路径跟踪的时间最优轨迹规划方法，其特征在于，步骤S4的路径微分系数计算处理是通过对各关节路径函数拟合得到的表达式进行求导，从而获得用于表示路径曲率的二阶微分函数q′(s)＝[θ₁′(s)，θ₂′(s)，…，θ_n′(s)]^T和用于表示路径曲率变化率三阶微分函数q″(s)＝[θ₁″(s)，θ₂″(s)，…，θ_n″(s)]^T。

6.根据权利要求2所述的一种用于机器人路径跟踪的时间最优轨迹规划方法，其特征在于，步骤S5的路径规划点选取如下：

S51.设定曲率变化阈值∈；

S52.以路径起点即第1个采样点为第1个规划点，从路径起点1开始沿着路径位移增大的方向搜索，直到第r个路径采样点时各关节的角度对于路径位移的差分满足以下条件时，通过如下公式选取第r-1个采样点作为第2个规划点：

max|q_计算′-q′(s(1))|＞∈

式中，q(s(1))表示在第1个规划点时的各关节角度，q(s(r))表示在第r个规划点时的各关节角度，s(1)表示在第1个采样点时的路径位移，s(r)表示在第r 个路径采样点时的路径位移，q_计算′表示第r个路径采样点和第1个路径采样点的角度的计算差分，q′(s(1))表示二阶微分函数在第1个路径采样点时的值；

S53.从第2个规划点开始，根据步骤S52沿着路径位移增大的方向搜索，找到第3个规划点，以此类推，最终找到用于规划的N个规划点；

通过步骤S51-S53对路径进行精简，从而减少需要规划的点数，减少规划时间。

7.根据权利要求2所述的一种用于机器人路径跟踪的时间最优轨迹规划方法，其特征在于，步骤S6的求解过程包括以下过程：

根据约束条件

τ_min(k)≤τ(k)≤τ_max(k) (1)

其中，k表示第k个规划点，τ(k)表示n×1阶关节力矩矢量，τ_min(k)表示第k个规划点的最小约束力矩，τ_max(k)表示第k个规划点的最大约束力矩，表示第k个规划点的最小约束速度，表示第k个规划点的速度，表示第k个规划点的最大约束速度，表示第k个规划点的最大约束加速度，表示第k个规划点的最小约束加速度，表示第k个规划点的关节速度，其等价于路径函数q(s)关于时间t的一阶导数在的s(k)值，表示第k个规划点的关节加速度，其等价于路径函数q(s)关于时间t的二阶导数在的s(k)值，q(k)等价于q(s(k))表示在第k个规划点时的各关节角度，等价于等价于

由如下的动力学方程求得的各关节力矩：

其中，τ(k)表示n×1阶关节力矩矢量，即[τ₁(k)τ₂(k)…τ_n(k)]^T，其中n表示机器人关节数，τ_n(k)表示第n个关节力矩，上标T表示转置；M(q(k))表示n×n阶正定质量矩阵；B(q(k))表示n×n(n-1)/2阶的哥氏力系数矩阵，表示n(n-1)/2×1阶的关节速度积矢量，即 表示在第k个规划点时的第n关节角度；C(q(k))是n×n阶离心力系数矩阵，而是n×1阶矢量，即F_v表示粘滞摩擦力向量；F_c表示库伦摩擦力向量；G(q(k))表示在第k个规划点时的各关节角度重力向量；q(k)表示n×1阶关节角度矢量，即[θ₁(k)θ₂(k)…θ_n(k)]，表示n×1阶关节速度矢量，即 表示n×1阶关节加速度矢量，即sign表示符号函数；

以及根据链式规则，得到关节速度加速度和路径位移的关系：

其中，q′(s)和q″(s)为路径位移s的二阶和三阶微分函数，表示路径位移对时间t的一阶微分，即路径速度，表示路径位移对时间t的二阶微分，即路径加速度；

对于路径规划点k，有：

其中，q′(s(k))和q″(s(k))分别为路径规划点k的路径位移的二阶和三阶微分函数，表示路径规划点k的路径位移对时间t的一阶微分，即路径速度，表示路径规划点k的路径位移对时间t的二阶微分，即路径加速度；

将(4)(5)代入不等式约束(1)(2)(3)，得路径空间不等式约束为：

其中，k是规划点，k＝1～N，m(s(k))＝M(q(s(k)))q′(s(k))、c(s(k))＝M(q(s(k)))q″(s(k))+B(q(s(k))，q′(s(k)))q′(s(k))+C(q(s(k)))(q′(s(k)))²、f(s(k))＝F_v(q(s(k)))q′(s(k))都是用于简化动力学方程的中间变量；m(s(k))、c(s(k))、B(q(s(k))、q′(s(k)))、f(s(k))、F_v(q(s(k)))和g(s(k))均为用于简化动力学方程的中间变量；

令

其中，表示所有关节的力矩和加速度约束下的路径加速度上限；

令

其中，表示所有关节的力矩和加速度约束下的路径加速度下限；

当时可求得满足力矩和加速度约束的路径边界速度而且最大路径速度还受速度约束的限制，用表示速度约束下的路径边界速度，以路径速度区间速度作为初始搜索区间，f(X)＝|α(s(k)，X)-β(s(k)，X)|作为判断函数，式中X为表示路径速度的函数自变量，通过黄金分割法进行求解满足力矩、速度和加速度限制的第k个规划点的路径边界速度

通过匀加速运动方程求解相应的路径边界速度的加速度

8.根据权利要求7所述的一种用于机器人路径跟踪的时间最优轨迹规划方法，其特征在于，所述黄金分割法的步骤如下：

S61.判断当路径位移为s(k)，路径边界速度为时，代入式(6)(7)得到的路径加速度上限是否大于路径加速度下限如果是，则为路径边界速度否则继续步骤S62进行黄金分割搜索；

S62.设置求解精度ε；判断函数f(X)＝|α(s(k)，X)-β(s(k)，X)|，初始搜索区间

S63.计算黄金分割点λ＝a+0.382(b-a)，μ＝a+0.618(b-a)，式中a、b、λ和μ是用于表示区间位置的变量，0.382，0.618是黄金分割的比例；

S64.判断黄金分割区间长度λ-μ是否满足精度要求，即λ-μ＜ε，如果满足，则令作为路径边界速度；如果不满足，继续转到步骤S64；

S65.将黄金分割点λ、μ代入判断函数f(X)，如果f(λ)＞f(μ)，则令a＝λ，转到步骤S63-S64重新求黄金分割点，如果f(λ)＜f(μ)，则令b＝μ，转到步骤S63-S64重新求黄金分割点；

S66.重复步骤S63-S65，直到黄金分割区间长度λ-μ＜ε，则令作为路径边界速度。

9.根据权利要求2所述一种用于机器人路径跟踪的时间最优轨迹规划方法，其特征在于，步骤S7的规划时间最优轨迹包括以下步骤：

步骤S71.从路径位移速度平面初始状态点(0，0)以最大加速度通过匀加速运动方程 沿着路径位移增加的方向往前计算，获得的规划曲线为α-profile，其中，已经规划的路径位移相应的最优速度用表示，sqrt()表示平方根函数，表示在路径位移为s(k-1)和路径速度为时的最大/最小路径加速度；

将代入式(6)得到路径加速度上限即为路径最大加速度，将代入式(7)得到路径加速度下限即为路径最小加速度；

直到第a个规划点时，其路径最大加速度用表示，如果则转到步骤S73；如果或a＝N且则转到步骤S72；如果a＝N且则转到步骤S77；

步骤S72.从路径位移速度平面最终状态点(1，0)沿着路径位移减少的方向往后计算，具体的，采用黄金分割法往后计算已知一规划点k的路径速度求解路径位移减少方向的相邻规划点的最大路径速度，具体的，从后一个规划点k-1的路径速度为0时的状态点(s(k-1)，0)沿着增大的方向按步长length搜索，直到在第j个步长时，有 式中，·表示相乘，为路径位移为s(k-1)和路径速度为j·length代入式(7)得到的路径最小加速度，以[0，j·length]为初始搜索区间，为判断函数，其中X为表示路径速度的函数自变量，根据步骤S62-S66的黄金分割法，搜索后一个规划点k-1的路径速度通过上述黄金分割法，路径位移速度平面最终状态点(1，0)沿着路径位移减少的方向往后计算，直到第h个规划点时：如果 则转到步骤S73；如果 则转到步骤S77；该步骤往后计算获得的规划曲线为β-profile；

步骤S73.如果规划点a的 或则转到步骤S74；如果则该点为满足约束条件的边界可用点；沿着位移增大的方向向前搜索，直到第b个规划点时，如果 或 令a＝b，转到步骤S74；如果b＝N，转到步骤S77；

该步骤所有边界可用点串联得到的曲线为γ-profile；

步骤S74.如果则转到步骤S75；如果则沿着位移增大的方向向前搜索，直到第c个规划点时，如果 且 则转到步骤S76；如果从该规划点c处用步骤S72的黄金分割法沿着路径位移减少的方向往后计算，直到第d个规划点时， 往后计算结束，继续从规划点c处以最大路径加速度往前计算，直到第e个规划点时，如果第e个规划点的路径速度则令a＝e，转到步骤S73；如果则转到步骤S77；

该步骤中，往后计算获得的曲线为β-profile，往前计算获得的曲线为α-profile；

步骤S75.在a规划点用最大路径加速度往前计算，直到第f个规划点时，如果第f个规划点的路径速度则令a＝f，转到步骤S73；如果则转到步骤S77；

该步骤中，往前计算获得的曲线为α-profile；

步骤S76.从该规划点处用步骤S72的黄金分割法沿着路径位移减少的方向往后计算，直到第g个规划点时， 往后计算结束，继续往前规划，令a＝c，转到步骤S73；

该步骤中，往后计算获得的曲线为β-profile；

步骤S77.将所有获得的α-profile，γ-profile和β-profile按顺序串联起来，即为路径位移-速度平面的时间最优规划曲线。

10.根据权利要求2所述一种用于机器人路径跟踪的时间最优轨迹规划方法，其特征在于，步骤S8是通过匀加速运动方程获得每个脉冲控制周期T的路径位移s(T)，具体包括以下步骤：

步骤S81.根据步骤S7时间最优规划曲线，利用一下公式求得每个路径规划点的加速度和到达规划点的时间：

其中，t(k)表示到达路径规划点k所需的运行时间；

步骤S82.根据每个规划点k的路径位移s(k)，路径速度路径加速度和到达时间t(k)，通过以下步骤求解每个控制周期的路径位移s(T)：

S821.设定控制周期时间T；

S822.从第1个规划点开始，沿着路径位移增大方向搜索，直到第k个路径规划点的t(k)＞T；

S823.计算控制周期时间T和到达第k-1个规划点的时间间隔Δt＝T-t(k-1)；

S824.通过以下的匀加速运动方程计算到达每个控制周期时间T的路径位移：