[发明专利]一种用于机器人路径跟踪的时间最优轨迹规划方法

权利要求书

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

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

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

指令生成单元用于根据获得的最优轨迹，获得每个控制周期的路径位移，从而根据路径函数获得各轴的角位移，再转化为控制指令脉冲用于机器人的控制，其特征在于，包括以下步骤：

S1.获取任务路径；

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

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

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

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

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.根据约束条件、路径边界速度及相应加速度通过匀加速运动方程规划时间最优轨迹；

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

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

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