CN114290335B - 一种机器人轨迹规划方法 - Google Patents

Abstract

本发明属于机器人在线测量轨迹规划技术领域，具体涉及一种机器人轨迹规划方法。该方法首先得到关节轨迹序列。然后采用遗传算法求解目标函数，得到最优关节轨迹点时间间隔序列。确定适应度值的过程包括：若两个个体均在约束函数的可行域范围内或者随机生成的参数小于设定调整参数，则通过目标函数来判断两个个体的优劣，否则通过约束函数来判断两个个体的优劣；对种群中所有个体的优劣进行排序，根据排序结果赋予每个个体的适应度值。最后确定连续的关节轨迹。本发明引入随机排序算法来改进遗传算法中适应度值的计算，可有效平衡目标函数值与约束函数值对进化过程中早熟的影响，避免陷入局部最优，提高遗传算法的搜索能力，保证了轨迹规划精度。

Description

一种机器人轨迹规划方法

技术领域

本发明属于机器人在线测量轨迹规划技术领域，具体涉及一种机器人轨迹规划方法。

背景技术

随着制造业自动化技术的发展，工业机器人技术以其较好的灵活性和稳定性，在自动化产业中得到了很好的应用。其中，机器人的轨迹规划方案是机器人应用技术中的关键问题。常见的机器人PTP规划是通过示教编程的方法，通过人为的设定机器人关节参数，从而使机器人完成从起点到目标点的运动。由于机器人关节速度、加速度等参数设定直接关系到整个运动的平稳性和工作效率，所以一般通过优化这些参数方法来提高机器人工作效率，并且通过优化关节参数来使机器人运动更加平顺，减少突变的可能，进而减少冲击。

关节空间的轨迹优化，是优化给定的关节轨迹点间时间间隔序列、关节角序列、角速度序列、或者角加速度序列约束等因变量曲线平顺的时间序列。主要研究有能量最优、冲击最优、时间最优等，这些优化可以在实际的工作环境中提高机器人的工作效率和使用寿命。国内外此类优化研究主要采用遗传算法、模式搜索算法、混沌遗传算法等智能算法。此类智能算法在处理约束条件时多采用罚函数法，罚函数法中的惩罚因子需要人为设定，调试复杂。而且，采用此类算法容易在进化过程中出现“早熟”的现象，即陷入局部最优，而非全局最优，导致最终得到的机器人轨迹并非最优轨迹。

发明内容

本发明的目的在于提供一种机器人轨迹规划方法，用以解决采用现有技术中方法出现早熟现象导致得到的机器人轨迹非最优轨迹的问题。

为解决上述技术问题，本发明所提供的技术方案以及技术方案对应的有益效果如下：

本发明提供了一种机器人轨迹规划方法，包括如下步骤：

1)获取机器人的目标路径序列，结合建立的机器人运动学方程，由运动学逆解得到与目标路径序列对应的关节轨迹序列；

2)基于关节轨迹点时间间隔序列建立目标函数和约束函数，结合确定的约束条件，采用遗传算法求解所述目标函数，得到的最优解为该机器人的最优关节轨迹点时间间隔序列；其中，采用遗传算法求解所述目标函数时，种群中的个体为关节轨迹点时间间隔序列，且采用如下方法确定每个个体的适应度值：①对种群中两个个体的优劣进行评价：若两个个体均在约束函数的可行域范围内或者随机生成的参数小于设定调整参数，则通过目标函数来判断两个个体的优劣，否则通过约束函数来判断两个个体的优劣；②基于步骤①的判断结果，对种群中所有个体的优劣进行排序，根据排序结果赋予每个个体的适应度值，且个体越优，其适应度值越高；

3)利用得到的关节轨迹序列以及最优关节轨迹点时间间隔序列，得到对应的关节轨迹参数，根据关节轨迹参数确定连续的关节轨迹，实现机器人轨迹规划。

上述技术方案的有益效果为：本发明引入随机排序算法来改进遗传算法中适应度值的计算，即，在两个个体均在约束函数的可行域范围内或者随机生成的参数小于设定调整参数时通过目标函数来判断两个个体的优劣，否则通过约束函数来判断两个个体的优劣，可有效平衡目标函数值与约束函数值对进化过程中早熟的影响，避免陷入局部最优，提高遗传算法的搜索能力，保证了最终得到的关节轨迹点时间间隔序列为全局最优，提高了关节轨迹点时间间隔序列的精度，进而提高了机器人轨迹规划的精度。

对方法进一步改进，步骤2)中，赋予每个个体的适应度值为：

或者/>式中，fit(i)表示赋予个体i的适应度值；pos(i)表示个体i在所有个体从优至劣进行排序的排序结果中所处的位置；m表示所有个体的总数量；X_i表示方程0＝(1-m)x^m-1+x^m-2…+x+1的根，i∈[1,m]；X₁表示随机选取的其中一个根。

上述技术方案的有益效果为：采用线性或者非线性两种方式为每个个体赋予适应度值；其中，采用线性方式为直接依据排序结果中所处的位置来确定每个个体的适应度值，个体越优，适应度值越高，方法简单容易实现；采用非线性方式，除了依据排序结果中所处的位置外，还依据方程0＝(1-m)x^m-1+x^m-2…+x+1的根，结合两方面内容来确定每个个体的适应度值，可以对每个个体优劣情况加以区分。这两种方式可依据实际需求灵活选取。

对方法进一步改进，步骤2)所包括的步骤②中，采用冒泡法对种群中个体的优劣进行排序。

上述技术方案的有益效果为：采用冒泡法进行个体优劣排序，方法简单容易实现。

对方法进一步改进，步骤2)中采用遗传算法求解所述目标函数时，对种群进行的遗传操作包括依次进行的变异操作和选择操作。

对方法进一步改进，所述选择操作采用随机型p竞争法选择操作。

上述技术方案的有益效果为：随机型p竞争法选择操作基于竞争，使得适应度值越强的个体的后代从父代被移除的次数就越少，使得当前较好的个体得以完善，保证了最优解的精度。

对方法进一步改进，步骤2)中所述目标函数为：

式中，min f(H)表示关节轨迹点时间间隔序列H的目标函数值；h_j表示关节轨迹点时间间隔；n表示关节轨迹序列中轨迹点的总个数。

对方法进一步改进，步骤2)中所述约束函数为：

式中，gⁱ(H)表示关节轨迹点时间间隔序列H的约束函数值，且约束函数的可行域为gⁱ(H)＝0；v_imax、a_imax、j_imax分别表示机器人关节i的转角速度、转角加速度、转角加加速度的上界；分别表示关节i的转角速度、转角加速度、转角加加速度；f表示关节轨迹点时间间隔的总和。

对方法进一步改进，步骤2)中所述约束条件包括：依据转角范围对关节的转角的约束、依据转角速度范围对关节的转角速度的约束、以及依据转角加速度范围对关节的转角加速度的约束。

对方法进一步改进，步骤3)中所述关节轨迹参数包括各关节的转角、转角速度、转角加速度和转角加加速度且采用如下方法计算得到各关节的转角、转角速度、转角加速度和转角加加速度：依据确定的关节轨迹序列和最优关节轨迹点时间间隔序列，计算关节各轨迹点的转角加速度；依据各轨迹点的转角加速度、各轨迹点的时刻和相邻关节轨迹点间的时间间隔，采用插值算法，得到连续的各关节的转角、转角速度、转角加速度和转角加加速度。

对方法进一步改进，所述插值算法为三次样条插值算法。

上述技术方案的有益效果为：采用三次样条插值算法，方法简单容易实现。

附图说明

图1是本发明的机器人轨迹规划系统的结构示意图；

其中，1-机器人及其控制单元，2-优化计算单元，3-自动控制单元；

图2是本发明的机器人轨迹规划方法的流程图；

图3是本发明的采用遗传算法求解目标函数的流程图；

图4是本发明的得到连续的关节轨迹的流程图；

图5是本发明的对种群中所有个体的优劣进行排序的流程图。

具体实施方式

下面结合附图和实施例，对本发明进行详细说明。

方法实施例：

本实施例的机器人轨迹规划方法基于如图1所示的系统，该系统包括机器人及其控制单元1、自动控制单元3和优化计算单元2，机器人及其控制单元1包括机器人和机器人控制单元。机器人为某气动量仪测量圆柱轴承滚子的在线测量工艺中所使用的StanfordManipulator机器人，在自动控制单元的作用下，该机器人需要完成物料送料、检测、分拣收纳等不同空间之间的转移，以及抓取、测量、分类等动作；自动控制单元3用于控制送料、检测、分拣收纳之间的协同运行；优化计算单元2用于机器人轨迹规划计算，并向机器人控制单元传输计算数据。

整个工作流程为：以机器人末端手爪相对于机器人基坐标的位置P＝[x,y,z]^T和姿态R,P,Y表示机器人末端手爪的位姿T，自动控制单元确定该机器人末端工作路径具体位姿为Q₀→Q₅，并将该路径数据传输至优化计算单元中，由优化计算单元进行逻辑分析处理与计算，得到优化后的关节轨迹点时间间隔序列，进而得到该机器人连续的关节轨迹，即实现本发明的一种机器人轨迹规划方法，并将得到的结果传输至机器人控制单元，由机器人控制单元完成控制机器人的轨迹控制。下面结合图2，对该机器人轨迹规划方法进行详细介绍。

步骤一，优化计算单元以T为采样周期，将自动控制单元输出的操作空间工艺路径Q₀→Q₅采样离散化，得到机器人的目标路径序列p₁,p₂,…,p_n。

步骤二，建立Stanford Manipulator机器人DH模型，进行运动学逆解得到与目标路径序列对应的关节轨迹序列q₁,q₂,…,q_n。

1、建立Stanford Manipulator机器人DH模型求解机器人的正逆运动学方程，求得机器人末端参考点相对于基坐标系的运动方程如下：

且：

n_x＝c₁[c₂(c₄c₅c₆-s₄s₆)-s₂s₅c₆]-s₁(s₄c₅c₆+c₄s₆)

n_y＝s₁[c₂(c₄c₅c₆-s₄s₆)-s₂s₅c₆]+c₁(s₄c₅c₆+c₄s₆)

n_z＝-s₂(c₄c₅c₆-s₄s₆)-c₂s₅c₆

o_x＝c₁[-c₂(c₄c₅s₆+s₄c₆)+s₂s₅s₆]-s₁(-s₄c₅s₆+c₄c₆)

o_y＝s₁[-c₂(c₄c₅s₆+s₄c₆)+s₂s₅s₆]+c₁(-s₄c₅s₆+c₄c₆)

o_z＝s₂(c₄c₅s₆+s₄c₆)+c₂s₅s₆

a_x＝c₁(c₂c₄s₅+s₂c₅)-s₁s₄s₅

a_y＝s₁(c₂c₄s₅+s₂c₅)+c₁s₄s₅

a_z＝-s₂c₄s₅+s₂c₅

p_x＝c₁s₂d₃-s₁d₂

p_y＝s₁s₂d₃+c₁d₂

p_z＝c₂d₃

式中，c_i＝cos(θ_i),s_i＝sin(θ_i)；表示坐标系i相对于坐标系i-1的坐标变换；d_i表示连杆偏距。且Stanford机器人各连杆的运动学参数如表1所示。

表1

2、逆运动学求解得到θ₁、θ₂、d₃、θ₄、θ₅、θ₆的值分别如下：

θ₂＝atan2(c₁p_x+s₁p_y,p_z)

d₃＝s₂(c₁p_x+s₁p_y)+c₂p_z

θ₄＝atan2(-s₁a_x+c₁a_y,c₂(c₁a_x+s₁a_y)-s₂a_z)

θ₅＝atan2(c₄[c₂(c₁a_x+s₁a_y)-s₂a_z]+s₄[-s₁a_x+c₁a_y],s₂(c₁a_x+s₁a_y)+c₂a_z)

θ₆＝atan2(s₆,c₆)

式中，atan2()表示四象限反正切函数。

3、应用Stanford Manipulator机器人逆运动学求解得到目标路径序列对应的关节轨迹序列q₁,q₂,…,q_k,…,q_n(k＝1,2,…n)，n表示离散序列总点数。对于关节i，表示关节i轨迹序列，/>分别表示关节i在第j段轨迹的转角、转角速度、转角加速度和转角加加速度。

步骤三，基于关节轨迹点时间间隔序列H建立时间最优数学优化模型，该时间最优数学模型包括目标函数和约束函数，结合设置的约束条件，采用遗传算法求解该目标函数中的关节轨迹点时间间隔序列，得到的最优解为该机器人的最优关节轨迹点时间间隔序列。其中，约束条件包括：依据转角范围对关节的转角的约束、依据转角速度范围对关节的转角速度的约束、以及依据转角加速度范围对关节的转角加速度的约束。目标函数和约束函数分别为：

式中，f(H)表示关节轨迹点时间间隔序列H的目标函数值；h_j表示关节轨迹点时间间隔；n表示关节轨迹序列中轨迹点的总个数；v_imax、a_imax、j_imax分别表示机器人关节i的转角速度、转角加速度、转角加加速度的上界；gⁱ(H)表示机器人关节i对关节轨迹点时间间隔序列H的约束函数值；f表示关节轨迹点时间间隔的总和。

1、求解目标函数过程。

如图3所示，采用遗传算法求解目标函数的过程如下：

1)初始化m组关节轨迹点时间间隔序列H_m×(n-1)：h_j(j＝1,2,…,n-1)，初始化值与步骤一中的采样周期成正比且满足正态分布h_j∈N(T,σ_j)，并作为初始种群中的各个个体。并将初始种群作为当代种群执行步骤2)。

2)基于目标函数值和约束函数值，使用冒泡法对当代种群中个体的优劣进行排序，根据排序结果确定当代种群中每个个体的适应度值，以有效平衡目标函数与约束函数对进化过程中“早熟”的影响，提高算法的搜索能力。过程如下：

①比较相邻两个个体：若两个个体均在约束函数的可行域范围内(即g(H)＝0)或者随机生成的参数u∈U(0,1)小于设定调整参数p_f，则通过目标函数f(H)来判断两个个体的优劣，否则通过约束函数g(H)来判断两个个体的优劣。如果第一个个体优于第二个个体，则交换这两个个体。其中，设定调整参数p_f控制选择比较的概率，按经验选取范围为0.4≤p_f≤0.5。其中，计算目标函数的过程可参考图4，需要利用关节轨迹点时间间隔序列进行插值计算，得到关节轨迹以及关节轨迹参数，进而计算得到目标函数；进行插值计算的过程在下述“2、插值计算过程”进行详细描述。

②对每一对相邻个体做同步骤①相同的工作，从开始第一对到结尾的最后一对，从而使得最后的最优的个体。

③除了最后一个个体外，针对所有的个体重复步骤①和步骤②的工作，从而得到了按照从劣到优排序的个体。整个排序过程如图5所示。

④根据排序结果赋予每个个体的适应度值。具体地，最优的个体排序为m，m为种群个体的数量，最劣的个体排序为1，然后由每个个体i在排序中的位置pos(i)，根据下述线性公式或者非线性公式赋值每个个体适应度fit(i)：

或者/>

式中，X_i表示方程0＝(1-m)x^m-1+x^m-2…+x+1的根，i∈[1,m]；X₁表示随机选取的其中一个根。

3)判断是否满足迭代终止条件(迭代次数达到迭代次数要求)：如果满足迭代终止条件，则将当代种群中适应度值较高的个体作为最终输出的最优关节轨迹点时间间隔序列；如果不满足迭代终止条件，则执行步骤4)。

4)采用变异策略生成子代种群，变异公式如下：

式中，h_i表示当代种群；h_i'表示子代种群；N_i(0,1)表示服从均值为0、方差为1的正态分布的随机数；σ_i表示个体表现型分量的变异参数，即个体的各个维度变异强度。

5)采用选择策略生成下代种群，并将下代种群作为当代种群并执行步骤2)。其中，本实施例中使用随机型p竞争法选择策略，随机型p竞争法基于竞争，个体适应度越强，其后代从父代被移除的次数就会越少，从而使当前好的个体得以完善。采用选择策略生成下代种群的过程如下：

①将当代种群称为父代种群，从父代种群和子代种群中按照顺序挑选出一个个体K，并从父代种群和子代种群中随机选择p个个体，父代种群和子代种群中个体个数均为M。

②比较个体K与p个个体适应度值的优劣，将个体K的适应度值优于p个个体适应度值的次数作为个体K的得分w_K。

③按照步骤②的方式依次评价2M个个体，将得分前M个个体作为下代种群。

2、插值计算过程。

为了评价关节轨迹点时间间隔序列，在求解上述目标函数的过程中，需要结合设置的约束条件，而约束条件中的各参数(包括关节的转角、转角速度、转角加速度)采用如下方法求解：

1)在关节轨迹点处关节速度连续，故利用和轨迹速度的边界条件/>并结合确定的关节轨迹序列和最优关节轨迹点时间间隔序列，可得关节的转角加速度序列a₁,a₂,…,a_n。关节的转角加速度序列a₁,a₂,…,a_n、关节轨迹序列q₁,q₂,…,q_n、关节轨迹点时间间隔序列h₁,h₂,…,h_n-1的方程式如下所示：

Ha＝d

式中，d_j＝μ_ja_j-1+2a_j+λ_ja_j+1，j＝2,3,4…,n-1； v_n表示转角速度。

2)建立机器人的关节轨迹三次多项式的插值模型，如下所示：

式中，θ_j(t)表示三次多项式函数，用于插值关节空间中的关键点；t₁,t₂,…,t_n表示与关节轨迹序列p₁,p₂,…,p_n对应的时刻序列，h_j(j＝1,2,…,n-1)表示[t_j,t_j+1]间的时间间隔，即h_j＝t_j+1-t_j；a₁,a₂,…,a_n表示与关节轨迹序列p₁,p₂,…,p_n对应的转角加速度序列，a_j、a_j+1分别表示在时刻t_j、t_j+1处的加速度值；θ_j(t)、分别表示关节i在第j段的转角、转角速度、转角加速度、转角加加速度。

3)将关节轨迹序列、关节轨迹点时间间隔序、转角速度序列代入步骤2)中构建的关节轨迹三次多项式的插值模型中，得到与之对应的连续的关节轨迹参数。

步骤四，利用步骤二中得到的关节轨迹序列以及步骤三中得到的最优关节轨迹点时间间隔序列，得到对应的连续的关节轨迹参数，根据关节轨迹参数确定连续的关节轨迹，实现机器人轨迹规划。其中，得到连续的关节轨迹参数的具体过程可参考步骤三的第2节“2、插值计算过程”的内容，原理一致。

至此，便可完成机器人关节轨迹规划。该方法以随机排序算法改进进化策略算法中适应度函数的计算，采用耦合随机排序的方法综合考虑个体约束违反情况及目标函数值，优化平衡目标函数值与约束函数值对进化过程中“早熟”的影响，提高遗传算法的搜索能力，进而在满足转角、转角速度、转角加速度约束条件下，优化给定的目标工艺路径，以最优关节轨迹点时间间隔序列所对应的关节轨迹参数控制机器人高效、稳定的运行，进而提高机器人的效率。而且，在使用遗传算法过程中，采用随机型p竞争法选择操作，基于竞争使得个体适应度越强的后代从父代被一处的次数就会越少，从而使得当前较优的个体得以完善，提高了最优解的精度。该方法可广泛应用于机器人轨迹规划领域。

Claims (7)

1.一种机器人轨迹规划方法，其特征在于，包括如下步骤：

1)获取机器人的目标路径序列，结合建立的机器人运动学方程，由运动学逆解得到与目标路径序列对应的关节轨迹序列；

2)基于关节轨迹点时间间隔序列建立目标函数和约束函数，结合确定的约束条件，采用遗传算法求解所述目标函数，得到的最优解为该机器人的最优关节轨迹点时间间隔序列；其中，采用遗传算法求解所述目标函数时，种群中的个体为关节轨迹点时间间隔序列，且采用如下方法确定每个个体的适应度值：①对种群中两个个体的优劣进行评价：若两个个体均在约束函数的可行域范围内或者随机生成的参数小于设定调整参数，则通过目标函数来判断两个个体的优劣，否则通过约束函数来判断两个个体的优劣；②基于步骤①的判断结果，对种群中所有个体的优劣进行排序，根据排序结果赋予每个个体的适应度值，且个体越优，其适应度值越高；赋予每个个体的适应度值为：

或者/>

式中，fit(i)表示赋予个体i的适应度值；pos(i)表示个体i在所有个体从优至劣进行排序的排序结果中所处的位置；m表示所有个体的总数量；X_i表示方程0＝(1-m)x^m-1+x^m-2…+x+1的根，i∈[1,m]；X₁表示随机选取的其中一个根；

所述目标函数为：

式中，min f(H)表示关节轨迹点时间间隔序列H的目标函数值；h_j表示关节轨迹点时间间隔；n表示关节轨迹序列中轨迹点的总个数；

所述约束函数为：

式中，gⁱ(H)表示关节轨迹点时间间隔序列H的约束函数值，且约束函数的可行域为gⁱ(H)＝0；v_imax、a_imax、j_imax分别表示机器人关节i的转角速度、转角加速度、转角加加速度的上界；分别表示关节i的转角速度、转角加速度、转角加加速度；f表示关节轨迹点时间间隔的总和；

3)利用得到的关节轨迹序列以及最优关节轨迹点时间间隔序列，得到对应的关节轨迹参数，根据关节轨迹参数确定连续的关节轨迹，实现机器人轨迹规划。

2.根据权利要求1所述的机器人轨迹规划方法，其特征在于，步骤2)所包括的步骤②中，采用冒泡法对种群中个体的优劣进行排序。

3.根据权利要求1所述的机器人轨迹规划方法，其特征在于，步骤2)中采用遗传算法求解所述目标函数时，对种群进行的遗传操作包括依次进行的变异操作和选择操作。

4.根据权利要求3所述的机器人轨迹规划方法，其特征在于，所述选择操作采用随机型p竞争法选择操作。

5.根据权利要求1所述的机器人轨迹规划方法，其特征在于，步骤2)中所述约束条件包括：依据转角范围对关节的转角的约束、依据转角速度范围对关节的转角速度的约束、以及依据转角加速度范围对关节的转角加速度的约束。

6.根据权利要求1所述的机器人轨迹规划方法，其特征在于，步骤3)中所述关节轨迹参数包括各关节的转角、转角速度、转角加速度和转角加加速度；且采用如下方法计算得到各关节的转角、转角速度、转角加速度和转角加加速度：

依据确定的关节轨迹序列和最优关节轨迹点时间间隔序列，计算关节各轨迹点的转角加速度；

依据各轨迹点的转角加速度、各轨迹点的时刻和相邻关节轨迹点间的时间间隔，采用插值算法，得到连续的各关节的转角、转角速度、转角加速度和转角加加速度。

7.根据权利要求6所述的机器人轨迹规划方法，其特征在于，所述插值算法为三次样条插值算法。