CN107199569B - 一种基于关节能量均衡分配的关节机器人轨迹规划方法 - Google Patents

Abstract

本发明属于机器人领域，并公开了一种基于关节能量均衡分配的关节机器人轨迹规划方法，包括以下步骤：1)建立机器人连杆模型；2)建立机器人动力学模型，获得各关节转矩计算公式；3)辨识机器人的动力学参数；4)得到各个关节的力矩曲线，对机器人各个关节力矩曲线进行拟合，利用曲率最大法对拟合曲线进行判别，记录最大插值点数及所对应的关节；5)建立评价函数作为能量均衡分配的判断标准，利用粒子群算法寻求评价函数最优解，构成机器人运行轨迹。通过本方法机器人能够自主判断运动过程中各关节电机能量相对均衡受力的位置点，生成运动轨迹。可以保证机器人在运动过程中轨迹平滑，运动时间短，各关节承受负载均衡。

Description

一种基于关节能量均衡分配的关节机器人轨迹规划方法

技术领域

本发明属于工业机器人领域，更具体地，涉及一种关节机器人轨迹规划方法。

背景技术

机器人动力学是研究机器人运动与受力之间的关系，建立机器人动力学模型是机器人动态控制和高精度运动控制、动态设计及运动仿真的基础。目前，机器人动力学计算的方法主要有：牛顿-欧拉法，拉格朗日法，凯恩法，旋量定理法。应用不同的动力学方法所得到的动力学方程是不同的，其计算效率有很大差别，牛顿-欧拉方程是采用形位坐标作为独立变量的传统力学方法，在处理少数关节时，形式简洁，在本发明中选择牛顿-欧拉法计算机器人动力学。

机器人动力学模型的建立依赖于机器人连杆的运动参数和机器人连杆惯性参数的确定，前者容易获得，后者必须加以辨识。机器人连杆的惯性参数辨识是机器人动力学的重要部分。现有的机器人惯性参数辨识方法有：解体测量计算法，解体实验测量法，不解体实验测量法，三维模型测量法，理论辨识法。理论辨识法不需要解体机器人,也不需要专门的实验平台，可以获得较高的精度，因此，本设计采用理论辨识法辨识机器人的惯性参数。

粒子群算法和遗传算法类似，也是一种基于群体叠代的算法，但并没有遗传算法的交叉以及变异，而是粒子在解空间追随最优的粒子进行搜索。粒子群算法的优势在于简单，容易实现，无需梯度信息，参数少，特别适合于处理实优化问题。

现阶段对机器人的轨迹规划多为基于时间最优和整体能量最优，没有将机器人关节电机力矩和能量均衡作为机器人运动中轨迹的判定条件，难以满足实际生产中对机器人精确控制的要求。对于重载机器人运动过程抖动，能耗过大等问题无法通过运动学轨迹规划解决。

发明内容

针对现有技术的以上缺陷或改进需求，本发明提供了一种基于关节能量均衡分配的关节机器人轨迹规划方法，其目的在于通过动力学计算不同位姿、速度下机器人的关节力矩，建立机器人关节电机能量均衡的评价函数，使机器人通过动态粒子群算法自动规划运动轨迹，从而优化器人运动过程中各关节电机负载不均衡，能耗过大的问题。

为实现上述目的，按照本发明，提供了一种基于关节能量均衡分配的关节机器人轨迹规划方法，其特征在于，包括以下步骤：

1)根据D-H参数法建立n关节机器人连杆模型；

2)建立机器人动力学模型，获得机器人关节力矩关系式；

3)辨识机器人在不同位姿的动力学参数；

4)输入机器人起始关节角度和终止关节角度，得到各个关节的力矩曲线，对机器人各个关节力矩曲线进行拟合，利用曲率最大法挑选特征点，获得各关节对应的特征点数，记录最大特征点数及获得最大特征点数的关节；

5)建立评价函数作为能量均衡分配的判断标准，利用粒子群算法寻求评价函数最优解，则获得机器人运行轨迹。

优选地，步骤1)具体步骤如下：

根据D-H参数法建立关节机器人的连杆坐标系模型，列出参数表，定义参数表中的关节角度θ＝[θ₁,θ₂,...,θ_n]，分别求解机器人关节i相对于关节i-1的变换矩阵：

其中，2≤i≤n，α_i-1表示关节i的轴线相对关节i-1轴线转动的角度，d_i表示a_i-1和a_i之间的距离。

优选地，步骤2)具体过程如下：

利用牛顿-欧拉法，首先向外递推计算各连杆的速度和加速度，从而获得各连杆的惯性力和力矩，然后向内递推计算各连杆相互作用的力和力矩，以及计算各关节的驱动力和驱动力矩，最终得到机器人关节力矩τ、角度、角速度及角加速度

的关系式：

其中，

表示惯性力矩，

表示离心力矩与哥氏力矩之和，G(θ)表示重力力矩，机器人关节力矩τ是上述力矩的合成。

优选地，步骤3)具体过程如下：

根据理论辨识法对机器人不同位置及姿态下各个连杆的惯性张量进行辨识，惯性张量表示如下所示：

其中，I_xx＝∫∫∫_V(y²+z²)ρdυ、I_yy＝∫∫∫_V(x²+z²)ρdυ、I_zz＝∫∫∫_V(x²+y²)ρdυ表示机器人各连杆绕各自连杆坐标轴x、y、z的质量惯性矩，I_xy＝∫∫∫_Vxyρdυ、I_yz＝∫∫∫_Vyzρdυ、I_zx＝∫∫∫_Vzxρdυ表示惯性积。

优选地，步骤4)具体过程如下：

4.1)输入机器人起始关节角度及目标关节角度，对起始位置到目标位置的路径进行初始规划，使机器人各关节同时启动，同时到达，并以S型加减速运行，使机器人运行平稳且运行时间最优，辨识在对应位姿下机器人的惯性特性后，带入步骤2)中机器人关节力矩的关系式中，从而得到各关节对应的力矩曲线；

4.2)利用曲率最大点的方法确定需要插入的中间离散点，中间离散点的曲率计算如下：

并且

其中，|κ_ir|表示机器人关节i上第r个离散点p_ir所对应的曲率，β_ir机器人关节i上r-1、r、r+1离散点相连而得到的夹角，||p_ir+1-p_ir-1||表示r-1和r+1离散点连线的模长，p_ir-1、p_ir和p_ir+1分别表示离散点坐标(x_ir-1,τ_ir-1)、(x_ir,τ_ir)和(x_ir+1,τ_ir+1)，x_ir-1、x_ir和x_ir+1表示第r-1、r和r+1个离散点,τ_ir-1、τ_ir和τ_ir+1表示第r-1、r和r+1个离散点的力矩。

如果κ_ir＞κ_ir-1、κ_ir＞κ_ir+1和κ_ir＞a·κ_iavg这三个条件同时满足，则在各关节中挑选满足上述条件的中间离散点作为插值点，遍历各关节，得到各个关节的插值点数N_i，从而获得特征插值点数N_e＝max(N₁,N₂,...,N_n)，并记录特征插值点数所对应关节e。

优选地，步骤5)具体过程如下：

5.1)针对机器人能量均衡的目标建立评价函数f(p):

其中，P_i＝T_i·ω_i，

P_e＝[P_e1,P_e2,...,P_en]表示各关节电机的额定功率，c_i对应关节i的功率权重；

5.2)对于关节e，以记录的N_e个特征点为已知关节角度x_ie，规划关节角速度

分别由粒子群算法计算对应θ_e下，其他五个关节最优角度，具体步骤如下：

5.2.1)初始化粒子规模L，每个粒子在空间中位置表示为

粒子速度表示为其中l∈[1,L]，表示第l个粒子，j∈[1,N_e]表示离散点j，k表示当前的迭代次数；

5.2.2)计算第l个粒子在k+1次迭代后的速度和位置

通过第l个粒子在经过k次迭代后的速度和位置

及第k次迭代后该粒子自己搜索的历史最优值

和全部粒子搜索的历史最优值

得到，计算公式如下所示：

其中，ω是惯性系数，是粒子保持原有速度的系数，c₁是粒子跟踪自己历史最优值的权重系数，c₂是粒子跟踪群体最优值的权重系数，ξ,η∈[0,1]，代表随机数，r代表约束因子；

5.2.3)搜索得到关节e角度为θ_ej时，对应一组机器人关节角，缩小机器人角度范围后，再对θ_ej+1利用粒子群算法得到一组机器人关节角，直到N_e个特征点都得到一组最优关节值。

优选地，n＝5或n＝6。

总体而言，通过本发明所构思的以上技术方案与现有技术相比，能够取得下列有益效果：

1)在机器人轨迹规划过程中，将动力学作为主要判定依据，能够实现机器人的动态控制与高精度控制。

2)机器人根据运动过程中关节所需力矩值，自主搜索运动轨迹，可以保证机器人运动过程中关节能量均衡分配，并且所需要的力矩值更优，节约机器人能耗。

附图说明

图1是华中数控6008型机器人的结构图；

图2是本发明实施的求解流程图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

参照图1～图2，以华中数控6008机器人为例，机器人结构如图1所示。

具体实施步骤如下所示：

S1：利用D-H参数法建立n关节机器人连杆坐标系，得到机器人连杆参数如表1所示，计算机器人位姿矩阵，该机器人为6关节机器人，则n＝6；

表1机器人连杆参数表

S2：以角度，角速度及角加速度为输入量，建立机器人动力学模型。利用牛顿—欧拉法正向递推相对角速度

相对角加速度相对加速度连杆质心加速度

连杆质心力矩

及连杆质心力矩然后反向递推关节受力f_i ⁱ及关节力矩

关节力矩表示为：

式中

表示坐标系i+1相对于坐标系i的旋转矩阵，

表示坐标系i+1相对于坐标系i的位置矩阵。

S3：辨识机器人处于不同位姿所对应的惯性张量：

其中，I_xx＝∫∫∫_V(y²+z²)ρdυ、I_yy＝∫∫∫_V(x²+z²)ρdυ、I_zz＝∫∫∫_V(x²+y²)ρdυ表示机器人各连杆绕各自连杆坐标轴x、y、z的质量惯性矩，I_xy＝∫∫∫_Vxyρdυ、I_yz＝∫∫∫_Vyzρdυ、I_zx＝∫∫∫_Vzxρdυ表示惯性积。

用辨识得到的时变惯性张量，代替动力学模型中的静态惯性张量，进行关节力矩的计算。

S4：以θ＝[0°,90°,0°,0°,-90°,0°]为机器人起始点关节角度，θ＝[60°,120°,-60°,30°,-30°,0°]为机器人终止点关节角度，对机器人进行轨迹规划，六个关节同时启动，同时到达，得到一组机器人关节力矩曲线。

S5：根据力矩曲线计算插值点数，计算离散点的曲率，选择曲率最大、产生突变的离散点作为插值点，曲率计算如下所示：

并且

其中，|κ_ir|表示机器人关节i上第r个离散点p_ir所对应的曲率，β_ir机器人关节i对应拟合曲线上r-1、r、r+1离散点相连而得到的夹角，||p_ir+1-p_ir-1||表示r-1和r+1离散点连线的模长，p_ir-1、p_ir和p_ir+1分别表示离散点坐标(x_ir-1,τ_ir-1)、(x_ir,τ_ir)和(x_ir+1,τ_ir+1)。

如果同时满足κ_ir＞κ_ir-1、κ_ir＞κ_ir+1和κ_ir＞a·κ_iavg，则挑选成为特征点，得到特征点数N_e＝max(N₁,N₂,N₃,N₄,N₅,N₆)，并记录最大特征点数所对应关节e。

S6：针对机器人能量均衡的目标建立评价函数，机器人各关节的质量值如表2所示。

表2机器人各关节参数表

i	m<sub>i</sub>(kg)	Cm<sub>i</sub>(mm)	P<sub>ei</sub>(kw)
				1	89<sub>.</sub>303	(-84.845,-4.973,-127.436)	1
2	20.121	(95.384,90,27.670)	2.5
				3	26.074	(75.335,-60.73,-6.994)	1
4	8.084	(0.066,-67.6,105.307)	0.2
				5	2.082	(-0.26,63.63,-20.5)	0.2
6	0.04	(0,0,38.25)	0.1

根据表2，可得6008机器人关节电机额定功率P_e＝[1,2.5,1,0.2,0.2,0.1]，取c₁＝c₂＝c₃＝0.8，c₄＝c₅＝c₆＝0.2，可得评价函数f(P):

其中，P_i＝T_i·ω_i，

S7：对六个关节角进行插点，插入点数为N_e，以关节e为标准，按曲率极值点分段，利用粒子群法计算其他五个关节在各段所对应的关节角度。

S8：粒子群算法中粒子数过小可能导致收敛到局部，粒子数过大会使时间过长，取粒子规模L＝24，对6008机器人除关节e外五个关节有五个维度，第l个粒子在经过k次迭代后在空间中位置表示为粒子速度表示为

S9:第l个粒子在k+1次迭代后的速度和位置可以用下列公式计算：

其中，

表示第l个粒子的速度和位置，

表示k次迭代后该粒子自己搜索的历史最优值，

表示全部粒子搜索的历史最优值，ξ,η∈[0,1]，代表0到1间均匀随机数。

S10：对关节e，每个角度值都对应一组解，从第一个插值点开始，求得一组解后缩小机器人角度范围，再利用粒子群算法求下一个插值点机器人关节角度，知道N_e个特征点都求得对应的最优关节值。

本发明求得的机器人轨迹是以关节能量均衡分配为目标，同时采用S加减速对求得插值点进行插值，使机器人运动时间也能保证最优，速度，加速度连续变化，运动轨迹平滑。

本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (2)

1.一种基于关节能量均衡分配的关节机器人轨迹规划方法，其特征在于，包括以下步骤：

1)根据D-H参数法建立n关节机器人连杆模型；

步骤1)具体步骤如下：

根据D-H参数法建立关节机器人的连杆坐标系模型，列出参数表，定义参数表中的关节角度θ＝[θ₁,θ₂,...,θ_n]，分别求解机器人关节i相对于关节i-1的变换矩阵：

其中，2≤i≤n，α_i-1表示关节i的轴线相对关节i-1轴线转动的角度，d_i表示a_i-1和a_i之间的距离；

2)建立机器人动力学模型，获得机器人关节力矩关系式；

步骤2)具体过程如下：

利用牛顿-欧拉法，首先向外递推计算各连杆的速度和加速度，从而获得各连杆的惯性力和力矩，然后向内递推计算各连杆相互作用的力和力矩，以及计算各关节的驱动力和驱动力矩，最终得到机器人关节力矩τ、角度、角速度及角加速度的关系式：

其中，

表示惯性力矩，

表示离心力矩与哥氏力矩之和，G(θ)表示重力力矩，机器人关节力矩τ是上述力矩的合成；

3)辨识机器人在不同位姿的动力学参数；

步骤3)具体过程如下：

根据理论辨识法对机器人不同位置及姿态下各个连杆的惯性张量进行辨识，惯性张量表示如下所示：

其中，I_xx＝∫∫∫_V(y²+z²)ρdυ、I_yy＝∫∫∫_V(x²+z²)ρdυ、I_zz＝∫∫∫_V(x²+y²)ρdυ表示机器人各连杆绕各自连杆坐标轴x、y、z的质量惯性矩，I_xy＝∫∫∫_Vxyρdυ、I_yz＝∫∫∫_Vyzρdυ、I_zx＝∫∫∫_Vzxρdυ表示惯性积；

4)输入机器人起始关节角度和终止关节角度，得到各个关节的力矩曲线，对机器人各个关节力矩曲线进行拟合，利用曲率最大法挑选特征点，获得各关节对应的特征点数，记录最大特征点数及获得最大特征点数的关节；

步骤4)具体过程如下：

4.1)输入机器人起始关节角度及目标关节角度，对起始位置到目标位置的路径进行初始规划，使机器人各关节同时启动，同时到达，并以S型加减速运行，使机器人运行平稳且运行时间最优，辨识在对应位姿下机器人的惯性特性后，带入步骤2)中机器人关节力矩的关系式中，从而得到各关节对应的力矩曲线；

4.2)利用曲率最大点的方法确定需要插入的中间离散点，中间离散点的曲率计算如下：

并且

其中，|κ_ir|表示机器人关节i上第r个离散点p_ir所对应的曲率，β_ir机器人关节i上r-1、r、r+1离散点相连而得到的夹角，||p_ir+1-p_ir-1||表示r-1和r+1离散点连线的模长，p_ir-1、p_ir和p_ir+1分别表示离散点坐标(x_ir-1,τ_ir-1)、(x_ir,τ_ir)和(x_ir+1,τ_ir+1)，x_ir-1、x_ir和x_ir+1表示第r-1、r和r+1个离散点，τ_ir-1、τ_ir和τ_ir+1表示第r-1、r和r+1个离散点的力矩；

如果κ_ir＞κ_ir-1、κ_ir＞κ_ir+1和κ_ir＞a·κ_iavg这三个条件同时满足，则在各关节中挑选满足上述条件的中间离散点作为插值点，遍历各关节，得到各个关节的插值点数N_i，从而获得特征插值点数N_e＝max(N₁,N₂,...,N_n)，并记录特征插值点数所对应关节e；

5)建立评价函数作为能量均衡分配的判断标准，利用粒子群算法寻求评价函数最优解，则获得机器人运行轨迹；

步骤5)具体过程如下：

5.1)针对机器人能量均衡的目标建立评价函数f(p):

其中，P_i＝T_i·ω_i，

P_e＝[P_e1,P_e2,...,P_en]表示各关节电机的额定功率，c_i对应关节i的功率权重；

5.2)对于关节e，以记录的N_e个特征点为已知关节角度x_ie，规划关节角速度

分别由粒子群算法计算对应θ_e下，其他五个关节最优角度，具体步骤如下：

5.2.1)初始化粒子规模L，每个粒子在空间中位置表示为

粒子速度表示为

其中l∈[1,L]，表示第l个粒子，j∈[1,N_e]表示离散点j，k表示当前的迭代次数；

5.2.2)计算第l个粒子在k+1次迭代后的速度和位置

通过第l个粒子在经过k次迭代后的速度和位置

及第k次迭代后该粒子自己搜索的历史最优值

和全部粒子搜索的历史最优值

得到，计算公式如下所示：

其中，ω是惯性系数，是粒子保持原有速度的系数，c₁是粒子跟踪自己历史最优值的权重系数，c₂是粒子跟踪群体最优值的权重系数，ξ,η∈[0,1]，代表随机数，r代表约束因子；

5.2.3)搜索得到关节e角度为θ_ej时，对应一组机器人关节角，缩小机器人角度范围后，再对θ_ej+1利用粒子群算法得到一组机器人关节角，直到N_e个特征点都得到一组最优关节值。

2.根据权利要求1所述的一种基于关节能量均衡分配的关节机器人轨迹规划方法，其特征在于，n＝5或n＝6。

Images