CN105159096A - 一种基于粒子群算法的冗余度空间机械臂关节力矩优化方法 - Google Patents

Abstract

本发明实施例提出了一种基于粒子群算法的冗余度空间机械臂关节力矩优化方法，包括：将机械臂操作空间点到点的转移任务从笛卡尔空间转换至关节空间，利用七次多项式插值法对机械臂关节变量进行参数化处理，获得粒子群算法的优化控制参数；将机械臂各关节力矩均值和值最小作为该算法求解的目标函数；依据所述优化控制参数以及所述目标函数，利用粒子群算法的寻优原理对空间机械臂运行轨迹进行优化求解，得出空间机械臂的运行路径；该路径即可使得空间机械臂关节力矩均值和值最小。根据本发明实施例提供的技术方案，在满足任务约束条件下执行操作空间中点到点转移任务时，依照上述规划路径运行可以使得空间机械臂各关节力矩得以优化。

Description

一种基于粒子群算法的冗余度空间机械臂关节力矩优化方法

【技术领域】

本发明涉及自动化控制技术，尤其涉及一种基于粒子群算法的冗余度空间机械臂关节力矩优化方法。

【背景技术】

当冗余度空间机械臂末端执行器运动时，其任务空间维数小于其关节空间维数。所以，当冗余度空间机械臂末端执行器按一定轨迹运动时，其关节驱动力矩满足任务要求的解并不是唯一的。正是由于其冗余度的存在,使得我们能够优化其在满足主运动任务下的辅助性能指标。

对空间机械臂的关节驱动力矩进行优化是非常重要的，一方面是从工作过程的安全性来考虑，如果机械臂在运动过程中要求的关节驱动力矩过大或超限，会使机械臂动力学性能下降，算法失效，并加快机械臂因关节应力过大导致的机械结构性能衰减速率，严重时甚至损坏机械臂本身；另一方面是希望机械臂能以较小的关节驱动力矩完成规定的操作程序，以进一步降低能耗，改变空间机械臂时刻面临燃料有限、补给困难的局面，同时可以减轻机械臂的机构磨损，延缓因关节应力过大导致的机械结构性能衰减速率，以降低机械臂的使用代价进而延长其使用寿命。

【发明内容】

有鉴于此，本发明实施例提出了一种基于粒子群算法的冗余度空间机械臂关节力矩优化方法，以降低机械臂任务执行所需的能耗，同时可以减轻机械臂的机构磨损，延缓因关节应力过大导致的机械结构性能衰减速率。

本发明实施例提出了一种基于粒子群算法的冗余度空间机械臂关节力矩优化方法，包括：

利用七次多项式插值法对机械臂关节变量进行参数化处理，获得粒子群算法的优化控制参数；

将空间机械臂各关节力矩均值的和值最小作为粒子群算法求解过程中的目标函数；

依据所述优化控制参数以及所述目标函数，利用粒子群算法的寻优原理对空间机械臂运行轨迹进行优化求解，规划得出空间机械臂的运行路径；所述运行路径即可使得所述空间机械臂关节力矩得以优化。

上述方法中，获得粒子群算法的优化控制参数的方法为：利用七次多项式插值方法对各个关节进行插值遍历，根据空间机械臂轨迹连续、速度连续等要求，基于多项式插值法得出空间机械臂关节角表达式，再利用机械臂执行点到点的转移任务时初始、终止状态的关节角度、角速度、角加速度约束条件将多项式系数用某几个参数表示，所获得的参数即为粒子群算法的优化控制参数。

使用上述方法中七次多项式插值法获得的空间机械臂关节角的表达式为：

θ(t)＝a_i7t⁷+a_i6t⁶+a_i5t⁵+a_i4t⁴+a_i3t³+a_i2t²+a_i1t+a_i0

其中，θ表示关节角序列，为n维列向量，a_i0,a_i1,...a_i7为七次多项式系数，i＝1,2,...,n表示第i个关节；

利用机械臂执行点到点的转移任务时的初始、终止关节角度、角速度、角加速度约束条件建立关节角约束方程为：

q_int＝θ_int

q_des＝θ_des

${\stackrel{\·}{q}}_{\mathrm{int}}={\stackrel{\·\·}{q}}_{\mathrm{int}}=0$

${\stackrel{\·}{q}}_{des}={\stackrel{\·\·}{q}}_{des}=0$

其中q_int，分别为空间机械臂关节角表达式所描述的所述空间机械臂执行点到点的转移任务时的初始关节角度，角速度以及角加速度；q_des,分别为空间机械臂关节角表达式所描述的所述空间机械臂执行点到点的转移任务时的终止关节角度，角速度以及角加速度；θ_int,θ_des是任务给定的初始及终止的关节角度；

将a_i6和a_i7选为粒子群算法的优化控制参数，对机械臂关节角进行七次多项式插值，所得到的8个多项式系数均可以通过a_i6和a_i7两个未知参数表示，具体表示如下：

a_i0＝θ_int

a_i1＝a_i2＝0

$a_{i3}=\frac{10({\mathrm{\θ}}_{des}-{\mathrm{\θ}}_{\mathrm{int}})-a_{i6}{t}^6-3a_{i7}{t}^7}{t^3}$

$a_{i4}=\frac{-15({\mathrm{\θ}}_{des}-{\mathrm{\θ}}_{\mathrm{int}})+3a_{i6}{t}^6+8a_{i7}{t}^7}{t^4}$

$a_{i5}=\frac{6({\mathrm{\θ}}_{des}-{\mathrm{\θ}}_{\mathrm{int}})-3a_{i6}{t}^6-6a_{i7}{t}^7}{t^5}$

上述方法中，将机械臂各关节力矩均值的和值最小作为粒子群算法求解过程中的目标函数。当冗余度机械臂的末端执行器运动时，其任务空间维数小于其关节空间维数。所以，冗余度机械臂末端执行器按一定轨迹运动时，其关节驱动力矩的解将不唯一。因此由于冗余度的存在使得我们能够对机械臂的关节驱动力矩进行优化。

所述粒子群算法求解过程中的目标函数为：

$Z=\underset{i=1}{\overset{n}{\Σ}}{\widetilde{\mathrm{\τ}}}_i$

其中，i＝1,2,...,n表示机械臂的第i个关节，Z为机械臂在任务执行过程中所有关节力矩的均值和，为机械臂的第i个关节在任务执行过程中力矩的均值；

所述机械臂第i个关节力矩的均值为：

${\widetilde{\mathrm{\τ}}}_i=\frac{{\∫}_0^{t_f}\left|{\mathrm{\τ}}_i\right|dt}{t_f}$

其中，τ_i为空间机械臂的关节力矩矢量τ＝(τ₁,τ₂,…,τ_n)^T中的第i个分量，t_f为规划时间；

所述关节力矩矢量τ可由空间机械臂在关节空间的动力学基本方程求得，所述空间机械臂在关节空间的动力学基本方程如下：

$D\left(\mathrm{\θ}\right)\stackrel{\·\·}{\mathrm{\θ}}+H(\mathrm{\θ},\stackrel{\·}{\mathrm{\θ}})+G\left(\mathrm{\θ}\right)=\mathrm{\τ}$

式中，θ表示关节角序列，为n维列向量，可由权利要求2中空间机械臂关节角的表达式求得；D(θ)∈R^n×n为其关节空间中惯性矩阵；为其哥氏力和离心力矢量矩阵；G(θ)∈R^n×1为其重力项；τ＝(τ₁,τ₂,…,τ_n)^T为其关节力矩矢量。

上述方法中，依据所述优化计算参数以及所述目标函数，利用粒子群算法的寻优原理对空间机械臂运行轨迹进行优化求解，规划得出空间机械臂的运行路径，使空间机械臂的关节力矩得到优化，包括：根据机械臂轨迹的任务要求，利用粒子群优化算法，以机械臂各关节力矩均值的和值最小为优化性能指标，以机械臂关节插值多项式的未知系数a_i6和a_i7为控制参数展开优化计算；

利用所述粒子群算法求解机械臂最小关节力矩均值和的具体步骤如下：

1)利用七次多项式插值法得到的控制参数a＝[a_i6,a_i7],i＝1,2,...,7，将关节空间的关节角数据描述为插值多项式函数值的离散序列；

2)设定微粒群算法中的初始变量，包括微粒数popsize、认知系数c₁、社会系数c₂、迭代次数max_length、微粒子速度上限V_max、下限V_min以及问题域中的问题值的上下限等；

3)微粒参数初始化。结合所设定的初始参数和微粒速度优化方程，利用随机函数得到微粒群中各粒子的初始位置、初始速度，并将其记录到当前位置表中；

粒子k的当前位置表示为：

X_k(T)＝(a₁₆(T),a₁₇(T),a₂₆(T),a₂₇(T),a₃₆(T)…a₇₆(T),a₇₇(T))

其中，k表示第k个粒子，k＝1,2,...,popsize；X_k(T)为14维行向量，表示粒子k在14维空间中的当前位置；T为迭代次数，T＝1,2,...,max_length；第k个粒子初始位置表示为X_k(1)；a_i6(T)与a_i7(T)为第T次迭代后的控制参数，i＝1,2,...,7；

粒子k的当前速度表示为：

V_k(T)＝(v₁₆(T),v₁₇(T),v₂₆(T),v₂₇(T),v₃₆(T)…v₇₆(T),v₇₇(T))

V_k(T)为14维行向量，表示粒子k在14维空间中的当前速度；其中v₁₆(T)表示与a₁₆(T)相对应维度的速度分量，其余维度类似，第k个粒子初始速度表示为V_k(1)；

4)根据粒子当前位置计算出当前的七次多项式插值曲线，从而得到目标函数相对应的适应度值，记为f(X)，f(X)是关于粒子在空间中位置的函数，用于反映粒子在空间中的位置与目标函数间的关系，记录当前个体最优位置和历史最优位置到当前个体最优表和历史最优表中；

5)计算当前迭代次数下的惯性因子ω的值，通过微粒子速度变异方程迭代计算微粒子新的当前速度，所述微粒子速度变异方程为：

V_k(T+1)＝ω·V_k(T)+c₁·r₁·(pBest_k(T)-X_k(T))+c₂·r₂·(gBest(T)-X_k(T))

其中，V_k(T+1)为微粒子k第T+1次迭代时速度，V_k(T)为微粒子k第T次迭代时速度，ω为当前迭代次数下的惯性因子，r₁和r₂为[0,1]的随机数，pBest_k(T)为微粒子k迭代T次后的个体最优位置，gBest_k(T)为迭代T次后整个粒子群的全局最优位置；

惯性因子ω的值由下式确定：

$\mathrm{\ω}=({\mathrm{\ω}}_1-{\mathrm{\ω}}_2)\×\frac{(\max \_length-T)}{\max \_length}+{\mathrm{\ω}}_2$

式中，ω₁和ω₂分别是惯性权重的初始值和终端值；max_length和T分别为最大迭代次数和当前迭代次数；

6)通过更新微粒子当前的速度值，进而更新微粒子的当前位置，并修改当前位置表中的位置参数；

微粒子位置更新方程为：

X_k(T+1)＝X_k(T)+V_k(T+1)

其中，X_k(T+1)为微粒子k更新后的当前位置；

7)通过当前得到的控制参数(即粒子的当前位置X_k(T))，通过多项式方程计算得到基于当前多项式参数的关节角度、角速度和角加速度，然后利用SOA逆动力学方程计算出机械臂当前各关节力矩的均值和，并以此作为目标函数的当前适应度值；

8)通过当前个体适应度值修改粒子个体历史最优值表和最优位置表中的最优值和最优位置参数；

粒子k的当前最优位置由下式确定：

${\mathrm{pbest}}_k(T+1)=\left\{\begin{array}{cc}{\mathrm{pbest}}_k\left(T\right)& f\left(X_k\right(T+1))\&GreaterEqual;f\left({\mathrm{pbest}}_k\right(T))\\ X_k(T+1)& f\left(X_k\right(T+1))<f\left({\mathrm{pbest}}_k\right(T))\end{array}\right.$

9)修改群体最优值表和最优位置表中的最优值和最优位置，并转到步骤(5)，进行循环优化计算，直至迭代次数max_length达到设定值；

所述全局最优位置gBest(T)由下式确定：

gBest(T)＝min{f(pBest₁(T)),f(pBest₂(T)),…,f(pBest_popsize(T))}

10)统计最优值数据，并计算整个群体的最优值及相应的最优位置，得到最优微粒子对应的位置坐标，从而计算得到最优位置对应的多项式插值曲线，得到机械臂目标函数的当前最优路径。

所述运行路径使得所述空间机械臂关节力矩得到优化。

由以上技术方案可以看出，本发明实施例具有以下有益效果：

本发明实施例的技术方案中，利用七次多项式插值法对机械臂关节变量进行参数化处理，获得粒子群算法的优化控制参数，将机械臂各关节力矩均值的和值最小作为粒子群算法求解过程中的目标函数，依据所述优化控制参数以及所述目标函数，利用粒子群算法的寻优原理对空间机械臂运行轨迹进行优化求解，从而获得使空间机械臂各关节力矩均值的和值得以优化的运行路径，因此能够使空间机械臂在完成任务的前提下使得各关节力矩均值的和值得以降低，从而降低机械臂任务执行所需的能耗，同时可以减轻机械臂的机构磨损，延缓因关节应力过大导致的机械结构性能衰减速率。

【附图说明】

为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其它的附图。

图1是本发明实施例所提出的基于粒子群算法的冗余度空间机械臂关节力矩优化方法的流程示意图；

图2是基于空间算子描述的多自由度空间机械臂模型示意图；

图3是基于空间算子代数的逆动力学计算流程图；

图4是利用本发明实施例所提出的基于粒子群算法的机械臂关节力矩优化算法的流程图；

图5是本发明实施例中七自由度空间机械臂DH坐标系示意图；

图6是本发明实施例迭代寻优过程中种群目标均值的变化曲线示意图；

图7是本发明实施例迭代寻优过程中机械臂关节力矩均值和的变化曲线示意图。

【具体实施方式】

为了更好的理解本发明的技术方案，下面结合附图对本发明实施例进行详细描述。

应当明确，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。

基于粒子群算法的冗余度空间机械臂关节力矩优化方法主要包括三个部分：首先将笛卡尔空间的规划路径向关节空间转换，针对机械臂关节空间展开数值仿真研究，利用多项式插值法对机械臂关节变量进行参数化处理，获得粒子群算法的优化控制参数；再将机械臂各关节力矩均值的和值最小作为粒子群算法求解过程中的目标函数；最终以多项式系数作为优化控制参数，以机械臂各关节力矩均值的和值最小目标函数，利用粒子群算法的寻优原理对空间机械臂运行轨迹进行优化求解，所求运行路径即可使得所述空间机械臂关节力矩得以优化。

本发明实施例给出一种基于粒子群算法的冗余度空间机械臂关节力矩优化方法，请参考图1，其为本发明实施例所提出的基于粒子群算法的冗余度空间机械臂关节力矩优化方法的流程示意图，如图1所示，该方法包括以下步骤：

步骤101，将机械臂操作空间中点到点的转移任务从笛卡尔空间转换至关节空间，利用七次多项式插值法对机械臂关节变量进行参数化处理，获得粒子群算法的优化控制参数。

具体的，首先利用七次多项式插值方法对各个关节进行插值遍历，根据空间机械臂轨迹连续、速度连续等要求，基于多项式插值法建立空间机械臂关节角表达式如下：

$\mathrm{\&theta;}\left(t\right)=\underset{j=0}{\overset{7}{\mathrm{\&Sigma;}}}{a}_{ij}{t}^j=a_{i7}{t}^7+a_{i6}{t}^6+a_{i5}{t}^5+a_{i4}{t}^4+a_{i3}{t}^3+a_{i2}{t}^2+a_{i1}t+a_{i0}$

根据机械臂关节角的多项式表达式，求导可以得到机械臂各关节的速度、加速度多项式表达式为：

$\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&theta;}}\left(t\right)=\underset{j=2}{\overset{7}{\mathrm{\&Sigma;}}}j*\left(j-1\right)a_{ij}{t}^{j-2}=42a_{i7}{t}^5+30a_{i6}{t}^4+20a_{i5}{t}^3+12a_{i4}{t}^2+6a_{i3}t+2a_{i2}$

利用机械臂初始和终止的关节角度、角速度、角加速度约束条件q_int＝θ_int,q_des＝θ_des, ${\stackrel{\&CenterDot;}{q}}_{\mathrm{int}}={\stackrel{\&CenterDot;\&CenterDot;}{q}}_{\mathrm{int}}=0,{\stackrel{\&CenterDot;}{q}}_{des}={\stackrel{\&CenterDot;\&CenterDot;}{q}}_{des}=0$ 可以建立6个关节角约束方程，而在空间机械臂关节角表达式中，多项式的未知系数为8个，因此选择其中的两个参数a_i6和a_i7作为粒子群算法的控制参数，即a＝[a_i6,a_i7],i＝1,2,...,n作为待定参数，其中n为机械臂的自由度数目。将空间机械臂关节角于其速度、加速度表达式代入机械臂约束方程可以将其余六个多项式系数用a_i6和a_i7两个参数表示，具体表示如下：

a_i0＝θ_int

a_i1＝a_i2＝0

$a_{i3}=\frac{10({\mathrm{\&theta;}}_{des}-{\mathrm{\&theta;}}_{\mathrm{int}})-a_{i6}{t}^6-3a_{i7}{t}^7}{t^3}$

$a_{i4}=\frac{-15({\mathrm{\&theta;}}_{des}-{\mathrm{\&theta;}}_{\mathrm{int}})+3a_{i6}{t}^6+8a_{i7}{t}^7}{t^4}$

$x_5=\frac{6({\mathrm{\&theta;}}_{des}-{\mathrm{\&theta;}}_{\mathrm{int}})-3a_{i6}{t}^6-6a_{i7}{t}^7}{t^5}$

其中，θ_int与θ_des是任务给定的机械臂初始及终止的关节角度；至此空间机械臂关节角的表达式可以由a_i6和a_i7两个参数确定，本实施例中同样选取这两个参数作为优化控制参数。

步骤102，将机械臂各关节力矩均值的和值最小作为粒子群算法优化求解过程中的目标函数。

具体的，将机械臂各关节力矩均值的和值最小认为是机械臂各关节力矩最优，从而将机械臂各关节力矩均值的和值最小作为粒子群算法求解过程中的目标函数，对冗余度空间机械臂关节力矩进行优化求解。

所述粒子群算法求解过程中的目标函数为：

$Z=\underset{i=1}{\overset{n}{\&Sigma;}}{\widetilde{\mathrm{\&tau;}}}_i$

其中，i＝1,2,...,n表示机械臂的第i个关节，Z为机械臂在任务执行过程中所有关节力矩的均值和，为机械臂的第i个关节在任务执行过程中力矩的均值；

在给定任务的约束条件下，空间机械臂的力矩均值和最小的优化问题可以表示为：

$\left\{\begin{array}{c}find:\\ \begin{array}{cc}\min & \underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{\&Integral;}_0^{t_f}\frac{\left|{\mathrm{\&tau;}}_i\right|}{t_f}dt\end{array}\\ subjectto:\\ \left|q\right|\&le;q_{\max }\\ \left|\stackrel{\&CenterDot;}{q}\right|\&le;{\stackrel{\&CenterDot;}{q}}_{\max }\\ \left|\stackrel{\&CenterDot;\&CenterDot;}{q}\right|\&le;{\stackrel{\&CenterDot;\&CenterDot;}{q}}_{\max }\end{array}\right.$

所述关节力矩矢量τ可由空间机械臂在关节空间的动力学基本方程求得，所述空间机械臂在关节空间的动力学基本方程如下：

$D\left(\mathrm{\&theta;}\right)\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&theta;}}+H(\mathrm{\&theta;},\stackrel{\&CenterDot;}{\mathrm{\&theta;}})+G\left(\mathrm{\&theta;}\right)=\mathrm{\&tau;}$

式中，θ表示关节角序列，为n维列向量；D(θ)∈R^n×n为其关节空间中惯性矩阵；为其哥氏力和离心力矢量矩阵；G(θ)∈R^n×1为其重力项；τ＝(τ₁,τ₂,…,τ_n)^T为其关节力矩矢量。

图2是基于空间算子描述的多自由度空间机械臂模型示意图，本实施例采用的动力学基本方程就是基于该模型建立的。如图2可以得到本实施例所依据的空间机械臂系统的各符号表示如下：∑_I为惯性坐标系，机械臂所有递推运算均是相对于惯性坐标系进行的；∑_k为第k杆坐标系，定义在关节处；J_k为关节k，J_n为机械臂底座，J₀为机械臂末端与外界相连处；C_k为第k杆质心位置；a_k为关节J_k到k杆质心C_k的向量；b_k为k杆质心C_k到关节J_k+1的向量；p_k为关节k在惯性系下的向量；m(k)为第k杆质量；P_c(k)为点J_k到C_k的向量。

图3是基于空间算子代数的逆动力学计算流程图，由图3可以得出空间机械臂在关节空间的动力学基本方程，具体步骤如下：

首先利用旋量描述方法来表示机械臂的速度、加速度以及力和力矩：

$V\left(k\right)=\left(\begin{array}{c}{\mathrm{\&omega;}}_k\\ v_k\end{array}\right)$

$a\left(k\right)=\left(\begin{array}{c}{\stackrel{\&CenterDot;}{\mathrm{\&omega;}}}_k\\ {\stackrel{\&CenterDot;}{v}}_k\end{array}\right)$

$f\left(k\right)=\left(\begin{array}{c}{N}_k\\ F_k\end{array}\right)$

其中，ω_k,v_k,N_k,F_k分别代表第k个关节的角速度、速度、角加速度、加速度、力矩以及力。

定义空间第k杆的惯性质量矩阵为：

$M\left(k\right)=\left(\begin{array}{cc}{I}_k& m_k{\tilde{p}}_c\left(k\right)\\ -m_k{\tilde{p}}_c\left(k\right)& m_{k}E\end{array}\right)$

其中I_k为连杆k相对于第k关节坐标系的惯性张量矩阵，m_k为连杆k的质量，为连杆k的质心向量的反对称矩阵，E是三维单位矩阵。定义关节k的状态转换矩阵为H(k)＝[h^T(k)000],其中h(k)为关节的转动轴向量，是三维列向量，当第k个自由度为移动副时，H(k)＝[000h^T(k)]，当有固连基座时，由于基座与惯性系为虚铰链接，因此有H(n+1)＝diag[1,1,1,1,1,1]。

关节速度、加速度递推式如下，其中k＝n,n-1,…,1

$V\left(k\right)={\mathrm{\&phi;}}^T(k+1,k)V(k+1)+H^T\left(k\right)\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(k\right)$

$\mathrm{\&alpha;}\left(k\right)={\mathrm{\&phi;}}^T(k+1,k)\mathrm{\&alpha;}(k+1)+H^T\left(k\right)\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&theta;}}\left(k\right)+a\left(k\right)$

关节力、力矩递推式如下，其中k＝1,2,…,n

f(k)＝φ(k,k-1)f(k-1)+M(k)α(k)+b(k)

T(k)＝H(k)f(k)

其中，a(k)，b(k)分别表示机械臂的科氏力和离心力，φ(k+1,k)为力和力矩递推算子，φ^T(k+1,k)为速度和加速度递推算子如以下表达式表示：

$\mathrm{\&phi;}(k+1,k)=\left(\begin{array}{cc}E& \tilde{l}\left(k+1,k\right)\\ 0& E\end{array}\right)$

${\mathrm{\&phi;}}^T(k+1,k)=\left(\begin{array}{cc}E& 0\\ -\tilde{l}\left(k+1,k\right)& E\end{array}\right)$

是相邻关节k+1到关节k的向量，其中定义H(k)为关节k的状态转换矩阵。

a(k)，b(k)分别表示机械臂的科氏力和离心力，当关节为旋转铰时：

$a\left(k\right)=\left(\begin{array}{c}\mathrm{\&omega;}(k+1)\&times;k\left(k\right)\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(k\right)\\ \mathrm{\&omega;}(k+1)\&times;\&lsqb;\mathrm{\&omega;}(k+1)\&times;l(k+1,k)\&rsqb;\end{array}\right)$

$b\left(k\right)=\left(\begin{array}{c}\mathrm{\&omega;}\left(k\right)\&times;\&lsqb;I\left(k\right)\mathrm{\&omega;}\left(k\right)\&rsqb;\\ m\left(k\right)\mathrm{\&omega;}\left(k\right)\&times;\&lsqb;\mathrm{\&omega;}\left(k\right)\&times;P_c\left(k\right)\&rsqb;\end{array}\right)$

当关节为移动铰时，a(k)表示如下：

$a\left(k\right)=\left(\begin{array}{c}0\\ \mathrm{\&omega;}(k+1)\&times;\&lsqb;\mathrm{\&omega;}(k+1)\&times;l(k+1,k)+\mathrm{\&omega;}\left(k\right)\&rsqb;\end{array}\right)$

定义多自由度机械臂的系统速度算子为V＝[V(1),...V(n-1),V(n)]^T，并以同样的形式表示机械臂的加速度算子、科氏力算子、离心力算子、力算子、力矩算子分别为α,a,b,f,T，则可以得到：

$V=H^T{\mathrm{\&phi;}}^T\stackrel{\&CenterDot;}{\mathrm{\&theta;}}$

$\mathrm{\&alpha;}={\mathrm{\&phi;}}^T{H}^T\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&theta;}}+{\mathrm{\&phi;}}^{T}a$

f＝φ(Mα+b)

T＝Hf

其中M是机械臂质量矩阵算子，H是状态投影矩阵算子，φ是空间转移算子。

最终可推导得到机械臂力矩计算公式为：

$T=M_G\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&theta;}}+C$

其中：

M_G＝HφMφ^TH^T

C＝Hφ(Mφ^Ta+b)

M_G表示机械臂的广义质量矩阵，C表示机械臂的非线性力矩阵。空间机械臂在关节空间的动力学基本方程中，D(θ)即为此处的M_G，即等同于此计算公式中的C，由于在空间中重力极小，故重力项G(θ)可忽略不计。

步骤103，依据所述优化控制参数以及所述目标函数，利用粒子群算法的寻优原理对空间机械臂运行轨迹进行优化求解，规划得出空间机械臂的运行路径；所述运行路径即可使得所述空间机械臂关节力矩得以优化。

具体的，图4是利用本发明实施例所提出的基于粒子群算法的机械臂关节力矩优化算法的流程图。如图4所示，粒子群算法求解机械臂最小关节力矩均值和的具体步骤如下：

利用所述粒子群算法求解机械臂最小关节力矩均值和的具体步骤如下：

1)利用七次多项式插值法得到的控制参数a＝[a_i6,a_i7],i＝1,2,...,7，将关节空间的关节角数据描述为插值多项式函数值的离散序列；

2)设定微粒群算法中的初始变量，包括微粒数popsize、认知系数c₁、社会系数c₂、迭代次数max_length、微粒子速度上限V_max、下限V_min以及问题域中的问题值的上下限等；

3)微粒参数初始化。结合所设定的初始参数和微粒速度优化方程，利用随机函数得到微粒群中各粒子的初始位置、初始速度，并将其记录到当前位置表中；

粒子k的当前位置表示为：

X_k(T)＝(a₁₆(T),a₁₇(T),a₂₆(T),a₂₇(T),a₃₆(T)…a₇₆(T),a₇₇(T))

其中，k表示第k个粒子，k＝1,2,...,popsize；X_k(T)为14维行向量，表示粒子k在14维空间中的当前位置；T为迭代次数，T＝1,2,...,max_length；第k个粒子初始位置表示为X_k(1)；a_i6(T)与a_i7(T)为第T次迭代后的控制参数，i＝1,2,...,7；

粒子k的当前速度表示为：

V_k(T)＝(v₁₆(T),v₁₇(T),v₂₆(T),v₂₇(T),v₃₆(T)…v₇₆(T),v₇₇(T))

V_k(T)为14维行向量，表示粒子k在14维空间中的当前速度；其中v₁₆(T)表示与a₁₆(T)相对应维度的速度分量，其余维度类似，第k个粒子初始速度表示为V_k(1)；

4)根据粒子当前位置计算出当前的七次多项式插值曲线，从而得到目标函数相对应的适应度值，记为f(X)，f(X)是关于粒子在空间中位置的函数，用于反映粒子在空间中的位置与目标函数间的关系，记录当前个体最优位置和历史最优位置到当前个体最优表和历史最优表中；

5)计算当前迭代次数下的惯性因子ω的值，通过微粒子速度变异方程迭代计算微粒子新的当前速度，所述微粒子速度变异方程为：

V_k(T+1)＝ω·V_k(T)+c₁·r₁·(pBest_k(T)-X_k(T))+c₂·r₂·(gBest(T)-X_k(T))

其中，V_k(T+1)为微粒子k第T+1次迭代时速度，V_k(T)为微粒子k第T次迭代时速度，ω为当前迭代次数下的惯性因子，r₁和r₂为[0,1]的随机数，pBest_k(T)为微粒子k迭代T次后的个体最优位置，gBest_k(T)为迭代T次后整个粒子群的全局最优位置；

惯性因子ω的值由下式确定：

$\mathrm{\&omega;}=({\mathrm{\&omega;}}_1-{\mathrm{\&omega;}}_2)\&times;\frac{(\max \_length-T)}{\max \_length}+{\mathrm{\&omega;}}_2$

式中，ω₁和ω₂分别是惯性权重的初始值和终端值；max_length和T分别为最大迭代次数和当前迭代次数；

6)通过更新微粒子当前的速度值，进而更新微粒子的当前位置，并修改当前位置表中的位置参数；

微粒子位置更新方程为：

X_k(T+1)＝X_k(T)+V_k(T+1)

其中，X_k(T+1)为微粒子k更新后的当前位置；

7)通过当前得到的控制参数(即粒子的当前位置X_k(T))，通过七次多项式方程计算得到基于当前多项式参数的关节角度、角速度和角加速度，然后利用SOA逆动力学方程计算出机械臂当前各关节力矩的均值和，并以此作为目标函数的当前适应度值；

8)通过当前个体适应度值修改粒子个体历史最优值表和最优位置表中的最优值和最优位置参数；

粒子k的当前最优位置由下式确定：

${\mathrm{pbest}}_k(T+1)=\left\{\begin{array}{cc}{\mathrm{pbest}}_k\left(T\right)& f\left(X_k\right(T+1))\&GreaterEqual;f\left({\mathrm{pbest}}_k\right(T))\\ X_k(T+1)& f\left(X_k\right(T+1))<f\left({\mathrm{pbest}}_k\right(T))\end{array}\right.$

9)修改群体最优值表和最优位置表中的最优值和最优位置，并转到步骤(5)，进行循环优化计算，直至迭代次数max_length达到设定值；

所述全局最优位置gBest(T)由下式确定：

gBest(T)＝min{f(pBest₁(T)),f(pBest₂(T)),…,f(pBest_popsize(T))}

10)统计最优值数据，并计算整个群体的最优值及相应的最优位置，得到最优微粒子对应的位置坐标，从而计算得到最优位置对应的七次多项式插值曲线，得到机械臂目标函数的当前最优路径。

所述运行路径使得所述空间机械臂的关节力矩得以优化。

用于具体实施例中，以一个七自由度的空间机械臂为具体研究对象进行了数值仿真研究，该空间机械臂由七个旋转关节和两根长直臂杆连接组成，机械臂的结构具有对称性，其中本发明实施例中七自由度空间机械臂DH坐标系示意图如图5所示。图5中机械臂各杆件的长度为d₁＝1.2m；d₂＝0.53m；d₃＝0.53m；d₄＝0.52m；d₅＝0.53m；d₆＝0.53m；d₇＝1.2m；a₃＝5.8m；a₄＝5.8m；表一为空间机械臂D-H参数表，如下所示：

表一

连杆i	θi(°)	di(m)	ai-1(m)	ai-1(°)
					1	θ1(0)	d1	0	90
2	θ2(90)	d2	0	90
					3	θ3(0)	0	a3	0
4	θ4(0)	d3+d4+d5	a4	0
					5	θ5(0)	0	0	90
6	θ6(-90)	d6	0	-90
					7	θ7(0)	d7	0	0

由表一可以得到空间机械臂的DH参数。此外，机械臂第一个关节坐标系位置相对于基座坐标系中心的坐标为rca₀＝{0.2,0,2}，姿态偏差为{0,0,0}。

表二为空间机械臂质量特性参数表，由表二可得到机械臂系统的惯性质量参数以及质心坐标向量参数，如下所示：

表二

所述空间机械臂在关节空间中点到点的转移任务设定如下：设定机械臂在运行过程中的一组初始关节角度为θ_int＝[-20°,0,-10°,-100°,120°,180°,70°]，一组期望的终止关节角度为θ_des＝[0,15°,-30°,-110°,140°,165°,90°]，规划时间为t_f＝20s。

利用本发明的方法，以机械臂处在固定基座模式为例，以多项式系数为控制参数，结合动力学模型中计算方程，以任务中机械臂各关节力矩均值的和值最小为优化目标，利用粒子群算法对给定机械臂关节空间轨迹进行优化计算，并通过Matlab软件进行了仿真实验。给定机械臂负载参数以及粒子群算法中相关参数为：质量m_load＝400kg；惯性张量：I_load＝{[16,0,0],[0,137,0],[0,0,146]}；粒子的速度约束V_max＝100，V_min＝0，最大迭代次数max_length＝100，粒子个数popsize＝30，惯性因子ω从0.8到0.2逐渐衰减，认知系数c₁＝0.2，社会系数c₂＝0.2。任务执行过程中的约束条件为：关节角度q的范围为[-180°,180°]，关节角速度 $\stackrel{\&CenterDot;}{q}\&le;1rad/s,$ 关节角加速度 $\stackrel{\&CenterDot;\&CenterDot;}{q}\&le;1rad/s^2.$

图6是本发明实施例迭代寻优过程中种群目标均值的变化曲线示意图，如图6所示，在种群迭代更新100次以后，获得种群目标均值的变化曲线，其中横坐标轴是种群更新迭代次数，纵坐标轴是对应种群当前迭代次数的种群目标均值，由该图可以看出在种群迭代寻优过程中，种群目标均值以较快速度下降，并在迭代到30次时已经从开始时的6.9×10^4收敛到2.6×10^3，且收敛过程迅速没有波动。

图7是本发明实施例迭代寻优过程中空间机械臂关节力矩均值和的变化曲线示意图，如图7所示，在种群迭代更新100次以后，获得迭代寻优过程中机械臂关节力矩均值和值的变化曲线，其中横坐标轴是种群更新迭代次数，纵坐标轴是对应种群当前迭代次数的空间机械臂关节力矩均值和，由该图可以看出在种群迭代寻优过程中，空间机械臂各关节力矩均值的和值以较快速度下降，并在迭代到30次时已经从开始时的1.1×10^3收敛到4.4×10^2，且收敛过程迅速没有波动，由该图可以说明利用本方法使得所述空间机械臂的关节力矩达到了比较好的优化效果。

综上所述，本发明实施例具有以下有益效果：

本发明实施例的技术方案中，利用七次多项式插值法对机械臂关节变量进行参数化处理，获得粒子群算法的优化控制参数，将机械臂各关节力矩均值的和值最小作为粒子群算法求解过程中的目标函数，依据所述优化控制参数以及所述目标函数，利用粒子群算法的寻优原理对空间机械臂运行轨迹进行优化求解，从而获得使空间机械臂各关节力矩均值的和值最小的运行路径，因此能够使空间机械臂在完成任务的前提下使得各关节力矩均值的和值得以降低，从而降低机械臂任务执行所需的能耗，同时可以减轻机械臂的机构磨损，延缓因关节应力过大导致的机械结构性能衰减速率。

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

Claims (4)

1.一种基于粒子群算法的冗余度空间机械臂关节力矩优化方法，其特征在于，所述方法步骤包括：

(1)将机械臂操作空间中点到点的转移任务从笛卡尔空间转换至关节空间，利用七次多项式插值法对机械臂关节变量进行参数化处理，获得粒子群算法的优化控制参数；

(2)将机械臂各关节力矩均值的和值最小作为粒子群算法优化求解过程中的目标函数；

(3)依据所述优化控制参数以及所述目标函数，利用粒子群算法的寻优原理对空间机械臂运行轨迹进行优化求解，规划得出基于本方法空间机械臂当前所能达到的关节力矩均值和最小的运行路径；所述运行路径即可使得所述空间机械臂关节力矩的均值和得以减小。

2.根据权利要求1所述的方法，其特征在于，利用七次多项式插值法对空间机械臂关节变量进行参数化处理，获得粒子群算法的优化控制参数，包括：利用七次多项式插值方法对各个关节进行插值遍历，根据空间机械臂轨迹连续、速度连续等要求，基于七次多项式插值法得出空间机械臂关节角表达式，再利用机械臂执行点到点的转移任务时初始、终止状态的关节角度、角速度、角加速度约束条件将七次多项式系数用某几个参数表示，所获得的参数即为粒子群算法的优化控制参数；

基于七次多项式插值法获得的空间机械臂关节角的表达式为：

θ(t)＝a_i7t⁷+a_i6t⁶+a_i5t⁵+a_i4t⁴+a_i3t³+a_i2t²+a_i1t+a_i0

其中，θ表示关节角序列，为n维列向量，a_i0,a_i1,...a_i7为七次多项式系数，i＝1,2,...,n表示第i个关节；

利用机械臂执行点到点的转移任务时的初始、终止关节角度、角速度、角加速度约束条件建立关节角约束方程为：

q_int＝θ_int

q_des＝θ_des

其中q_int 分别为空间机械臂关节角表达式所描述的所述空间机械臂执行点到点的转移任务时的初始关节角度，角速度以及角加速度；q_des，分别为空间机械臂关节角表达式所描述的所述空间机械臂执行点到点的转移任务时的终止关节角度，角速度以及角加速度；θ_int,θ_des是任务给定的初始及终止的关节角度；

将a_i6和a_i7选为粒子群算法的优化控制参数，对机械臂关节角进行七次多项式插值，所得到的8个多项式系数均可以通过a_i6和a_i7两个未知参数表示，具体表示如下：

a_i0＝θ_int

a_i1＝a_i2＝0

。

3.根据权利要求1所述的方法，其特征在于，将机械臂各关节力矩均值的和值最小作为粒子群算法求解过程中的目标函数，具体说明如下：空间机械臂各关节力矩的数值能够间接反映其在主运动任务执行过程中的辅助性能，本发明以空间机械臂各关节力矩均值的和值作为其操作空间中点到点转移任务下的辅助性能指标，即为本优化方法利用粒子群算法求解过程中的目标函数；

所述粒子群算法求解过程中的目标函数为：

其中，i＝1,2,...,n表示机械臂的第i个关节，Z为机械臂在任务执行过程中所有关节力矩的均值和，为机械臂的第i个关节在任务执行过程中力矩的均值；

所述机械臂第i个关节力矩的均值为：

其中，为空间机械臂的关节力矩矢量τ＝(τ₁,τ₂,…,τ_n)^T中的第i个分量，t_f为规划时间；

所述关节力矩矢量τ可由空间机械臂在关节空间的动力学基本方程求得，所述空间机械臂在关节空间的动力学基本方程如下：

式中，θ表示关节角序列，为n维列向量，可由权利要求2中空间机械臂关节角的表达式求得；D(θ)∈R^n×n为其关节空间中惯性矩阵；为其哥氏力和离心力矢量矩阵；G(θ)∈R^n×1为其重力项；τ＝(τ₁,τ₂,…,τ_n)^T为其关节力矩矢量。

4.根据权利要求1所述的方法，其特征在于，依据所述优化控制参数以及所述目标函数，利用粒子群算法的寻优原理对空间机械臂运行轨迹进行优化求解，获得使空间机械臂各关节力矩得以优化的运行路径，包括：根据机械臂轨迹的任务要求，利用粒子群优化算法，以机械臂各关节力矩均值的和值最小为优化性能指标，以机械臂关节插值多项式的未知系数a_i6和a_i7为控制参数展开优化计算；

利用所述粒子群算法求解机械臂各关节力矩最小的均值和的具体步骤如下：

1)利用七次多项式插值法得到的控制参数a＝[a_i6,a_i7],i＝1,2,...,7，将关节空间的关节角数据描述为插值多项式函数值的离散序列；

2)设定微粒群算法中的初始变量，包括微粒数popsize、认知系数c₁、社会系数c₂、迭代次数max_length、微粒子速度上限V_max、下限V_min以及问题域中的问题值的上下限等；

3)微粒参数初始化。结合所设定的初始参数和微粒速度优化方程，利用随机函数得到微粒群中各粒子的初始位置、初始速度，并将其记录到当前位置表中；

粒子k的当前位置表示为：

X_k(T)＝(a₁₆(T),a₁₇(T),a₂₆(T),a₂₇(T),a₃₆(T)…a₇₆(T),a₇₇(T))

其中，k表示第k个粒子，k＝1,2,...,popsize；X_k(T)为14维行向量，表示粒子k在14维空间中的当前位置；T为迭代次数，T＝1,2,...,max_length；第k个粒子初始位置表示为X_k(1)；a_i6(T)与a_i7(T)为第T次迭代后的控制参数，i＝1,2,...,7；

粒子k的当前速度表示为：

V_k(T)＝(v₁₆(T),v₁₇(T),v₂₆(T),v₂₇(T),v₃₆(T)…v₇₆(T),v₇₇(T))

V_k(T)为14维行向量，表示粒子k在14维空间中的当前速度；其中v₁₆(T)表示与a₁₆(T)相对应维度的速度分量，其余维度类似，第k个粒子初始速度表示为V_k(1)；

4)根据粒子当前位置计算出当前的七次多项式插值曲线，从而得到目标函数相对应的适应度值，记为f(X)，f(X)是关于粒子在空间中位置的函数，用于反映粒子在空间中的位置与目标函数间的关系，记录当前个体最优位置和历史最优位置到当前个体最优表和历史最优表中；

5)计算当前迭代次数下的惯性因子ω的值，通过微粒子速度变异方程迭代计算微粒子新的当前速度，所述微粒子速度变异方程为：

V_k(T+1)＝ω·V_k(T)+c₁·r₁·(pBest_k(T)-X_k(T))+c₂·r₂·(gBest(T)-X_k(T))

其中，V_k(T+1)为微粒子k第T+1次迭代时速度，V_k(T)为微粒子k第T次迭代时速度，ω为当前迭代次数下的惯性因子，r₁和r₂为[0,1]的随机数，pBest_k(T)为微粒子k迭代T次后的个体最优位置，gBest_k(T)为迭代T次后整个粒子群的全局最优位置；

惯性因子ω的值由下式确定：

式中，ω₁和ω₂分别是惯性权重的初始值和终端值；max_length和T分别为最大迭代次数和当前迭代次数；

6)通过更新微粒子当前的速度值，进而更新微粒子的当前位置，并修改当前位置表中的位置参数；

微粒子位置更新方程为：

X_k(T+1)＝X_k(T)+V_k(T+1)

其中，X_k(T+1)为微粒子k更新后的当前位置；

7)通过当前得到的控制参数(即粒子的当前位置X_k(T))，通过七次多项式方程计算得到基于当前多项式参数的关节角度、角速度和角加速度，然后利用SOA逆动力学方程计算出机械臂当前各关节力矩的均值和，并以此作为目标函数的当前适应度值；

8)通过当前个体适应度值修改粒子个体历史最优值表和最优位置表中的最优值和最优位置参数；

粒子k的当前最优位置由下式确定：

9)修改群体最优值表和最优位置表中的最优值和最优位置，并转到步骤(5)，进行循环优化计算，直至迭代次数max_length达到设定值；

所述全局最优位置gBest(T)由下式确定：

gBest(T)＝min{f(pBest₁(T)),f(pBest₂(T)),…,f(pBest_popsize(T))}

10)统计最优值数据，并计算整个群体的最优值及相应的最优位置，得到最优微粒子对应的位置坐标，从而计算得到最优位置对应的七次多项式插值曲线，得到机械臂目标函数的当前最优路径。

所述运行路径即为所述空间机械臂关节力矩经优化后的路径。这能够使空间机械臂在完成任务的前提下使得各关节力矩均值的和值得到降低，从而降低机械臂任务执行所需的能耗，同时可以减轻机械臂的机构磨损，延缓因关节应力过大导致的机械结构性能衰减速率。