[发明专利]一种基于关节能量均衡分配的关节机器人轨迹规划方法

权利要求书

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。