CN106363633A - 基于改良粒子群算法的机器人稳定步态规划方法和装置 - Google Patents

Abstract

本发明公开了一种基于改良粒子群算法的机器人稳定步态规划方法和装置。其中，该方法包括：确定适应度函数值，并根据适应度函数值，利用粒子群算法对粒子的速度和位置进行更新，对粒子进行变异，并重新确定适应度函数值，循环上述所有步骤，直至满足预设次数；确定适应度函数值最大时的两髋关节中心参数；根据两髋关节中心参数，利用三次样条插值函数确定机器人踝关节和髋关节的运动轨迹。本发明解决了现有技术中使用粒子群算法进行机器人步行轨迹优化时由于迭代次数的增加造成陷入局部最优解无法跳出的技术问题。

Description

基于改良粒子群算法的机器人稳定步态规划方法和装置

技术领域

本发明涉及机器人控制领域，具体而言，涉及一种基于改良粒子群算法的机器人稳定步态规划方法和装置。

背景技术

机器人是一门集控制、机械、电子、材料等学科于一体的高度交叉的前沿学科，近年来，机器人产业迅速发展，并引起了国家的高度重视，在不久的将来中国将有望成为世界上最大的机器人市场。仿人机器人代表了当今机器人研究领域最为尖端的水平，与其他机器人相比，仿人机器人因具有行走能力更强、上下台阶更方便、工作空间更广阔等等的优势而具有广阔的应用前景。稳定步行是仿人机器人最基本的类人行为,也是仿人机器人研究领域的热点和难点之一。

现有的双足步行轨迹优化方法中最常用的是粒子群算法和遗传算法，标准的粒子群算法通过追踪个体极值和群体极值完成优化过程，操作简单，收敛速度快，但随着迭代次数的增加，可能陷入局部最优解而无法跳出。遗传算法是把问题参数编码为染色体，再利用迭代的方式进行选择、交叉以及变异等运算来交换种群中染色体的信息，最终生成符合优化目标的染色体。但很难处理维度较高的问题，由于仿人机器人计算非常复杂，涉及到大量的个体计算，比较浪费时间。

针对现有技术中使用粒子群算法进行机器人步行轨迹优化时由于迭代次数的增加造成陷入局部最优解无法跳出的问题，目前尚未提出有效的解决方案。

发明内容

本发明实施例提供了一种基于改良粒子群算法的机器人稳定步态规划方法和装置，以至少解决现有技术中使用粒子群算法进行机器人步行轨迹优化时由于迭代次数的增加造成陷入局部最优解无法跳出的技术问题。

根据本发明实施例的一个方面，提供了一种基于改良粒子群算法的机器人稳定步态规划方法，包括：

确定适应度函数值，并根据适应度函数值，利用粒子群算法对粒子的速度和位置进行更新；

对粒子进行变异，并重新确定适应度函数值；

循环上述所有步骤，直至满足预设次数；

确定适应度函数值最大时的两髋关节中心参数；

根据两髋关节中心参数，利用三次样条插值函数确定机器人踝关节和髋关节的运动轨迹。

根据本发明实施例的另一方面，还提供了一种基于改良粒子群算法的机器人稳定步态规划装置，包括：

第一确定模块，用于确定适应度函数值，并根据适应度函数值，利用粒子群算法对粒子的速度和位置进行更新；

变异模块，用于对粒子进行变异，并重新确定适应度函数值；

循环模块，用于循环调用第一确定模块和变异模块，直至满足预设次数；

第二确定模块，用于确定适应度函数值最大时的两髋关节中心参数；

第三确定模块，用于根据两髋关节中心参数，利用三次样条插值函数确定机器人踝关节和髋关节的运动轨迹。

在本发明实施例中，对传统的粒子群算法进行改良，通过确定适应度函数值，并根据适应度函数值，利用粒子群算法对粒子的速度和位置进行更新，之后对粒子进行变异，并重新确定适应度函数值，然后循环上述所有步骤，直至满足预设次数，确定适应度函数值最大时的两髋关节中心参数，然后根据两髋关节中心参数，利用三次样条插值函数确定机器人踝关节和髋关节的运动轨迹，达到了避免陷入局部最优解的目的，从而实现了机器人稳定步态的技术效果，进而解决了现有技术中使用粒子群算法进行机器人步行轨迹优化时由于迭代次数的增加造成陷入局部最优解无法跳出的技术问题。

附图说明

此处所说明的附图用来提供对本发明的进一步理解，构成本申请的一部分，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：

图1是根据本发明实施例1的一种基于改良粒子群算法的机器人稳定步态规划的方法流程图；

图2是根据本发明实施例1的机器人双腿关节结构图；

图3是根据本发明实施例1的机器人髋关节液压机构原理图；

图4是根据本发明实施例1的机器人膝关节液压机构原理图；

图5是根据本发明实施例1的机器人踝关节液压机构原理图；

图6是根据本发明实施例1的利用D-H建模方法建立机器人双腿模型的示意图；

图7是根据本发明实施例1的应用边界条件一和边界条件二时左踝关节在x方向运动曲线对比图；

图8是根据本发明实施例1的应用边界条件一和边界条件二时左踝关节在z方向运动曲线对比图；

图9是根据本发明实施例1的单脚支撑的稳定区域示意图；

图10是根据本发明实施例1的双脚支撑的稳定区域示意图；

图11是根据本发明实施例1的适应度函数的变化曲线示意图；

图12是根据本发明实施例1的ZMP轨迹示意图；

图13是根据本发明实施例1的x-z方向轨迹曲棍图；

图14是根据本发明实施例1的三维步态轨迹曲棍图；以及

图15是根据本发明实施例2的一种基于改良粒子群算法的机器人稳定步态规划的装置结构图。

具体实施方式

为了使本技术领域的人员更好地理解本发明方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分的实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明保护的范围。

需要说明的是，本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本发明的实施例能够以除了在这里图示或描述的那些以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、***、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。

以下为本发明中使用到的部分名词解释：

步态规划，指根据步行要求，给定步态中踝关节及髋关节在各关键时刻径向和侧向的位置及边界条件，求出一条过这些关键点的最优有效路径。

零力矩点(ZMP)，是仿人机器人所受的重力和惯性力的合力延长线与支撑地面的交点,在该点处地面反力的合力矩沿水平面内的两个垂直方向的分量为零。

稳定区域，即满足将机器人足底与地面之间所有接触点都包含在内的最小多边形区域。

稳定裕度，是指ZMP到稳定区域各边界距离的最小值。显然,ZMP稳定裕度越大,步态的稳定性越好,仿人机器人步行时的抗干扰能力也越强。

实施例1

根据本发明实施例，提供了一种基于改良粒子群算法的机器人稳定步态规划的方法实施例，需要说明的是，在附图的流程图示出的步骤可以在诸如一组计算机可执行指令的计算机***中执行，并且，虽然在流程图中示出了逻辑顺序，但是在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤。

图1是根据本发明实施例的基于改良粒子群算法的机器人稳定步态规划方法，如图1所示，该方法包括如下步骤：

步骤S100，确定适应度函数值，并根据适应度函数值，利用粒子群算法对粒子的速度和位置进行更新。

步骤S102，对粒子进行变异，并重新确定适应度函数值。

步骤S104，循环上述所有步骤，直至满足预设次数。

具体的，预设次数可以根据实际情况进行设置，本发明对预设次数的具体数值不做限定，其中，步骤S104中的“循环上述所有步骤”指的是循环步骤S100和步骤S102，即步骤S102中确定了适应度函数值之后，再执行步骤S100时使用的适应度函数值为步骤S102中确定的适应度函数值。

步骤S106，确定适应度函数值最大时的两髋关节中心参数。

步骤S108，根据两髋关节中心参数，利用三次样条插值函数确定机器人踝关节和髋关节的运动轨迹。

通过上述步骤S100-步骤S108，本发明对传统的粒子群算法进行改良，通过确定适应度函数值，并根据适应度函数值，利用粒子群算法对粒子的速度和位置进行更新，之后对粒子进行变异，并重新确定适应度函数值，然后循环上述步骤S100和步骤S102，直至满足预设次数，确定适应度函数值最大时的两髋关节中心参数，然后根据两髋关节中心参数，利用三次样条插值函数确定机器人踝关节和髋关节的运动轨迹，本发明使用的改良的粒子群算法摒弃了标准粒子群算法通过跟踪极值来更新粒子位置的方法，引入遗传算法中的变异操作，通过在速度和位置更新后适当的添加变异操作的方式来搜索最优解，有效的结合了粒子群算法及遗传算法的优势，达到了避免陷入局部最优解的目的，从而实现了机器人稳定步态的技术效果，进而解决了现有技术中使用粒子群算法进行机器人步行轨迹优化时由于迭代次数的增加造成陷入局部最优解无法跳出的技术问题。

在一种可选的实施例中，本发明在步骤S100之前，需要建立机器人的双腿模型，本发明利用D-H建模方法建立机器人的双腿模型，建立了双腿模型后就可以根据腿部正逆运动学解算方法求解零力矩点的坐标，其中，机器人每只腿有6个自由度，髋关节3个自由度且髋关节的3个自由度不交于一点，膝关节1个自由度，踝关节2个自由度且踝关节为2自由度空间并联机构。

具体的，踝关节为2自由度空间并联机构能够有效避免串联踝关节运动的不平衡性。如图所示，图2具体为机器人双腿关节结构图，本发明的机器人可以由液压驱动，每条腿由三种不同的液压结构组成，如图3、图4和图5所示，图3具体为机器人髋关节液压机构原理图，图4具体为机器人膝关节液压机构原理图，图5具体为机器人踝关节液压机构原理图。

在一种可选的实施例中，利用D-H建模方法建立机器人双腿模型，本发明中机器人具体指的是仿人机器人，并在机器人双腿模型上标出相应的连杆参数，如图6所示，图6具体为利用D-H建模方法建立机器人双腿模型，相应的连杆参数如下表所示：

其中，A＝0.37733,B＝0.422,G＝0.089,T＝0.050,F＝0.050，单位为m。

在一种可选的实施例中，在建立了机器人的双腿模型之后，需要根据仿人机器人的步行要求，给定步态中踝关节及髋关节在各关键时刻径向和侧向的位置，由于整个步行阶段涉及到的关键点太多，每个步行周期的关键点相同，因此只选取一个步行周期进行讨论，各关键时刻的位置函数如下：

$h_x\left(t\right)=\left\{\begin{array}{cc}(k-1)D_s+d_{xs}& t={\mathrm{kT}}_c\\ {\mathrm{kD}}_s-d_{xe}& t={\mathrm{kT}}_c+T_d\\ {\mathrm{kD}}_s& t={\mathrm{kT}}_c+T_0\\ {\mathrm{kD}}_s+d_{xs}& t=(k+1)T_c\\ (k+1)D_s-d_{xe}& t=(k+1)T_c+T_d\\ (k+1)D_s& t=(k+1)T_c+T_0\\ (k+1)D_s+d_{xs}& t=(k+2)T_c\end{array}\right.,h_y\left(t\right)=\left\{\begin{array}{cc}-G+d_{ys}& t={\mathrm{kT}}_c\\ G-d_{ym}& t={\mathrm{kT}}_c+T_d\\ G-d_{ye}& t={\mathrm{kT}}_c+T_0\\ G-d_{ys}& t=(k+1)T_c\\ -G+d_{ym}& t=(k+1)T_c+T_d\\ -G+d_{ye}& t=(k+1)T_c+T_0\\ -G+d_{ys}& t=(k+2)T_c\end{array}\right.;$

h_z(t)≡H；

$a_{lx}\left(t\right)=\left\{\begin{array}{cc}{\mathrm{kD}}_s& t=(k+1)T_c+T_d\\ {\mathrm{kD}}_s+L_{a0}& t=(k+1)T_c+T_0\\ (k+2)D_s& t=(k+2)T_c\end{array}\right.,a_{lz}\left(t\right)=\left\{\begin{array}{cc}{l}_{an}& t=(k+1)T_c+T_d\\ H_{a0}& t=(k+1)T_c+T_0\\ l_{an}& t=(k+2)T_c\end{array}\right.,a_{ly}\left(t\right)\≡G;$

$a_{rx}\left(t\right)=\left\{\begin{array}{cc}(k-1)D_s& t={\mathrm{kT}}_c+T_d\\ (k-1)D_s+L_{a0}& t={\mathrm{kT}}_c+T_0\\ (k+1)D_s& t=(k+1)T_c\end{array}\right.,a_{rz}\left(t\right)=\left\{\begin{array}{cc}{l}_{an}& t={\mathrm{kT}}_c+T_d\\ H_{a0}& t={\mathrm{kT}}_c+T_0\\ l_{an}& t=(k+1)T_c\end{array}\right.,a_{ry}\left(t\right)\≡G;$

其中，h_x,h_y,h_z分别为周期步行阶段两髋关节中心在x,y,z三个方向上的位置坐标，a_lx,a_ly,a_lz分别为周期步行阶段左腿踝关节在x,y,z三个方向上的位置坐标，a_rx,a_ry,a_rz分别为周期步行阶段右腿踝关节在x,y,z三个方向上的位置坐标，d_xs,d_xe,d_ys,d_ym,d_ye分别为周期步行各阶段对应的两髋关节中心参数。T_c为半个步行周期，T_d为双脚支撑阶段的时间间隔，D_s代表一步的步长，(L_a0,H_a0)是所抬高脚的最高点坐标，T₀为脚抬到最高时对应的时间。

在一种可选的实施例中，确定适应度函数值的步骤之前包括：

对粒子和速度进行初始化。

利用三次样条插值函数规划机器人髋关节和踝关节的运动轨迹。

利用腿部逆运动学算法，根据机器人髋关节和踝关节的运动轨迹，计算机器人所有关节的关节角度。

利用正运动学算法，计算机器人各连杆质心的坐标。

根据机器人各连杆质心的坐标确定零力矩点的坐标。

具体的，对粒子和速度进行初始化即初始化两髋关节中心参数及粒子速度，其中粒子速度可以在限定范围内随机选取，在一种具体的实施例中，进化次数可以设置为300，种群规模为20，进化次数的选取并非越大越好，因为进化次数越大，所需计算时间越长，只要能满足最终收敛即可。

具体的，利用三次样条插值函数规划机器人髋关节和踝关节的运动轨迹具体为利用三次样条插值函数生成双腿在径向和侧向的运动轨迹，对区间[a,b]，0≤a＝t₀＜t₁＜t₂＜…＜t_n＝b,i＝0,1,…,n。其上的函数s(t)满足如下两个条件：(1)在每一个区间[t_j-1,t_j]上均为不超过三次的多项式；(2)s(t)∈C²[a,b]，则s(t)为[a,b]上的三次样条插值函数。

其中，

$s_j\left(t\right)=\frac{1}{6h_j}\[{(t_j-t)}^3{M}_{j-1}+{(t-t_{j-1})}^3{M}_j\]+\frac{(t_j-t)}{h_j}(f_{j-1}-\frac{1}{6}{M}_{j-1}{h_j}^2)+\frac{(t-t_{j-1})}{h_j}(f_j-\frac{1}{6}{M}_j{h_j}^2)$

其中，t∈[t_j-1,t_j],h_j＝t_j-t_j-1,f_j＝f(t_j),j＝1,2,…,n。

令则边界条件一：s'(t₀)＝f₀',s'(t_n)＝f_n',通常，本发明中令s'(t₀)＝0,s'(t_n)＝0。有：

边界条件二：s”(t₀)＝f₀”,s”(t_n)＝f_n”,通常，本发明中令s”(t₀)＝0,s”(t_n)＝0，称为自然边界条件。有：

加入边界条件，即可求出各个小区间上的插值函数,因为踝关节的运动为前半个步行周期运动，后半周期速度恒为0，为了更好的衔接每个周期的位置轨迹，保证落地时冲击力最小，本发明中使用边界条件一，即令端点处的速度为0。对比了多数速度不为0的情况，接口处曲线明显光滑，尤其是踝关节在x方向和z方向的运动曲线，差异明显，如图7、图8所示，图7具体为应用边界条件一和边界条件二时左踝关节在x方向运动曲线对比图，图8具体为应用边界条件一和边界条件二时左踝关节在z方向运动曲线对图，由图7和图8可以看出，右腿曲线情况与左腿相同，只是相差半个周期。

具体的，利用腿部逆运动学算法，根据机器人髋关节和踝关节的运动轨迹，计算机器人所有关节的关节角度。具体为应用解析方法将各关节角用髋关节位姿和踝关节位姿中的元素表达出来。如上述内容，本发明中的仿人机器人髋关节三轴不交于一点，因此不能应用普遍应用的几何方法进行逆运动学求解，应用了一种解析方法将各关节角用髋关节位姿和踝关节位姿中的元素表达出来。应用在具体实施例中，若初始位姿为T₀，末端位姿为T_m，则变换矩阵T可表示为T＝T₀ ^-1T_m。设

$T=\left(\begin{array}{cccc}{n}_x& o_x& a_x& p_x\\ n_y& o_y& a_y& p_y\\ n_z& o_z& a_z& p_z\\ 0& 0& 0& 1\end{array}\right);$

则各关节角的表达式如下：

${\mathrm{tan\θ}}_1=\frac{a_z{p}_x-a_x{p}_z}{a_y{p}_z-a_z{p}_y};$

(1)当tanθ₁＞0时，

(2)当tanθ₁＜0时，

(3)当tanθ₁＝+∞时，

${\mathrm{tan\θ}}_2=-\frac{a_z}{{\mathrm{cos\θ}}_1{a}_x+{\mathrm{sin\θ}}_1{a}_y};$

(1)当tanθ₂＞0时，

(2)当tanθ₂＜0时，

(3)当tanθ₂＝+∞时，

${\mathrm{\θ}}_{345}=\arctan 2(\frac{a_z}{{\mathrm{sin\θ}}_2},-{\mathrm{sin\θ}}_1{a}_x+{\mathrm{cos\θ}}_1{a}_y);$

θ₆＝arcsin(sinθ₂(-cosθ₁n_x-sinθ₁n_y)-cosθ₂n_z)；

${\mathrm{\θ}}_4=\arccos \left(\frac{1}{2AB}\right({\left(-{\mathrm{sin\θ}}_1{p}_x+{\mathrm{cos\θ}}_1{p}_y+T\right)}^2+{\left(-\frac{p_z}{{\mathrm{sin\θ}}_2}-F\right)}^2-A^2-B^2\left)\right);$

$\mathrm{\φ}=\arctan 2(\frac{B{\mathrm{sin\θ}}_4}{\sqrt{A^2+2AB{\mathrm{cos\θ}}_4+B^2}},\frac{A+B{\mathrm{cos\θ}}_4}{\sqrt{A^2+2AB{\mathrm{cos\θ}}_4+B^2}});$

${\mathrm{\θ}}_3+\mathrm{\φ}=\arctan 2(\frac{-{\mathrm{sin\θ}}_1{p}_x+{\mathrm{cos\θ}}_1{p}_y+T}{\sqrt{A^2+2AB{\mathrm{cos\θ}}_4+B^2}},\frac{-\frac{p_z}{{\mathrm{sin\θ}}_2}-F}{\sqrt{A^2+2AB{\mathrm{cos\θ}}_4+B^2}});$

θ₃＝θ₃+φ-φ；

θ₅＝θ₃₄₅-θ₃-θ₄；

其中，θ＝arctan2(sin(θ),cos(θ))为双变量反正切函数。(角7-12与角1-6的正逆运动学计算相同，这里省去。)

对于本发明的机器人，两髋关节中心初始位姿命名为T₀；左腿初始位姿命名为T_0l，末端位姿命名为T_ml；右腿初始位姿命名为T_0r，末端位姿命名为T_mr，分别为左右腿髋关节及其对应的踝关节位姿，则：

$T_0=\left(\begin{array}{cccc}1& 0& 0& h_x\\ 0& 1& 0& h_y\\ 0& 0& 1& h_z\\ 0& 0& 0& 1\end{array}\right),T_{0l}=\left(\begin{array}{cccc}1& 0& 0& h_x\\ 0& 1& 0& h_y+G\\ 0& 0& 1& h_z\\ 0& 0& 0& 1\end{array}\right),T_{0r}=\left(\begin{array}{cccc}1& 0& 0& h_x\\ 0& 1& 0& h_y-G\\ 0& 0& 1& h_z\\ 0& 0& 0& 1\end{array}\right);$

$T_{mr}=\left(\begin{array}{cccc}0& 0& -1& a_{rx}\\ 0& -1& 0& a_{ry}\\ -1& 0& 0& a_{rz}\\ 0& 0& 0& 1\end{array}\right),T_{ml}=\left(\begin{array}{cccc}0& 0& -1& a_{lx}\\ 0& -1& 0& a_{ly}\\ -1& 0& 0& a_{lz}\\ 0& 0& 0& 1\end{array}\right);$

末端位置分别对应左右脚踝关节的位置坐标。由于运动过程中，脚面一直平行于地面，因此姿态和初始状态相同，即θ₃＝0；θ₄＝0；θ₅＝0；θ₆＝0时，对应的姿态。

具体的，利用正运动学算法中计算机器人各连杆质心的坐标，要应用D-H算法确定各连杆之间的变换矩阵进而得到整个腿部从髋关节到踝关节的变换矩阵。

各连杆变换矩阵如下：

$T_1^0=\left(\begin{array}{cccc}{\mathrm{cos\θ}}_1& -{\mathrm{sin\θ}}_1& 0& 0\\ {\mathrm{sin\θ}}_1& {\mathrm{cos\θ}}_1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right),T_2^1=\left(\begin{array}{cccc}{\mathrm{cos\θ}}_2& -{\mathrm{sin\θ}}_2& 0& 0\\ 0& 0& 1& -T\\ -{\mathrm{sin\θ}}_2& -{\mathrm{cos\θ}}_2& 0& 0\\ 0& 0& 0& 1\end{array}\right);$

$T_3^2=\left(\begin{array}{cccc}{\mathrm{cos\θ}}_3& -{\mathrm{sin\θ}}_3& 0& F\\ 0& 0& -1& 0\\ {\mathrm{sin\θ}}_3& {\mathrm{cos\θ}}_3& 0& 0\\ 0& 0& 0& 1\end{array}\right),T_4^3=\left(\begin{array}{cccc}{\mathrm{cos\θ}}_4& -{\mathrm{sin\θ}}_4& 0& A\\ {\mathrm{sin\θ}}_4& {\mathrm{cos\θ}}_4& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right);$

$T_5^4=\left(\begin{array}{cccc}{\mathrm{cos\θ}}_5& -{\mathrm{sin\θ}}_5& 0& B\\ {\mathrm{sin\θ}}_5& {\mathrm{cos\θ}}_5& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right),T_6^T=\left(\begin{array}{cccc}{\mathrm{cos\θ}}_6& -{\mathrm{sin\θ}}_6& 0& 0\\ 0& 0& 1& 0\\ -{\mathrm{sin\θ}}_6& -{\mathrm{cos\θ}}_6& 0& 0\\ 0& 0& 0& 1\end{array}\right);$

各连杆变换矩阵相乘，得到腿部变换矩阵：

具体的，根据生成轨迹及正逆运动学方程，确定ZMP(零力矩点)坐标，其中各连杆绕其自身质心的惯性张量忽略不计，并假定机器人由多个质点组成。

$p_x=\frac{\underset{i=1}{\overset{N}{\mathrm{\Σ}}}\{({\stackrel{\·\·}{z}}_i+g)x_i-(z_i-p_z){\stackrel{\·\·}{x}}_i\}}{\underset{i=1}{\overset{N}{\mathrm{\Σ}}}({\stackrel{\·\·}{z}}_i+g)};$

$p_y=\frac{\underset{i=1}{\overset{N}{\mathrm{\Σ}}}\{({\stackrel{\·\·}{z}}_i+g)y_i-(z_i-p_z){\stackrel{\·\·}{y}}_i\}}{\underset{i=1}{\overset{N}{\mathrm{\Σ}}}({\stackrel{\·\·}{z}}_i+g)};$

其中，c_i＝(x_i y_i z_i)^T,i＝0,…,6为各连杆在参考坐标系中的质心坐标。p_z是地面高度，在平地上行走时值为0。

令上身在两髋关节中心坐标系中的质心坐标为c₀₁，左大腿在左臀坐标系中的质心坐标为c₁₁，右大腿在右臀坐标系中的质心坐标为c₂₁，左小腿在左膝关节坐标系中的质心坐标为c₃₁，右小腿在右膝关节坐标系中的质心坐标为c₄₁，左脚底在左踝关节坐标系中的质心坐标为c₅₁，右脚底在右踝关节坐标系中的质心坐标为c₆₁。则有

(c₀ 1)＝T₀(c₀₁ 1)^T；

$\left(\begin{array}{cc}{c}_1& 1\end{array}\right)=T_{0l}T_1^{0}T_2^{1}T_3^2{\left(\begin{array}{cc}{c}_{11}& 1\end{array}\right)}^T;$

$\left(\begin{array}{cc}{c}_2& 1\end{array}\right)=T_{0r}T_7^{0}T_8^{7}T_9^8{\left(\begin{array}{cc}{c}_{21}& 1\end{array}\right)}^T;$

$\left(\begin{array}{cc}{c}_3& 1\end{array}\right)=T_{0l}T_1^{0}T_2^{1}T_3^{2}T_4^3{\left(\begin{array}{cc}{c}_{31}& 1\end{array}\right)}^T;$

$\left(\begin{array}{cc}{c}_4& 1\end{array}\right)=T_{0r}T_7^{0}T_8^{7}T_9^{8}T_{10}^9{\left(\begin{array}{cc}{c}_{41}& 1\end{array}\right)}^T;$

$\left(\begin{array}{cc}{c}_5& 1\end{array}\right)=T_{0l}T_1^{0}T_2^{1}T_3^{2}T_4^{3}T_5^{4}T_6^5{\left(\begin{array}{cc}{c}_{51}& 1\end{array}\right)}^T;$

$\left(\begin{array}{cc}{c}_6& 1\end{array}\right)=T_{0r}T_7^{0}T_8^{7}T_9^{8}T_{10}^{9}T_{11}^{10}T_{12}^{11}{\left(\begin{array}{cc}{c}_{61}& 1\end{array}\right)}^T;$

在一种可选的实施例中，确定适度函数值的步骤包括：根据零力矩点的类稳定裕度确定适应度函数值。

在一种可选的实施例中，根据零力矩点的类稳定裕度确定适应度函数值的步骤包括：

确定机器人运动的不同阶段时的目标函数和权重。

根据机器人运动的不同阶段时的目标函数和权重确定适应度函数值。

在一种可选的实施例中，机器人运动的不同阶段包括：双脚支撑左脚在前、左脚支撑、双脚支撑右脚在前和右脚支撑。

具体的，机器人在步行阶段分为单脚支撑阶段和双脚支撑阶段两部分，单脚支撑阶段和双脚支撑阶段又可以进行左右脚的划分，单脚支撑和双脚支撑时都会产生稳定区域，如图9所示，双脚支撑的稳定区域如图10所示。并且各阶段稳定区域中心的坐标可以由左右腿踝关节的坐标表示。

在一种可选的实施例中，虽然采样的关键点在前半周期和后半周期时对称，但插值出的图形在一个步行周期内并不是完全对称的，因此本发明中对一个步行周期的四个阶段均进行讨论，因此根据零力矩点的类稳定裕度确定适应度函数值的步骤中使用如下公式：

双脚支撑左脚在前时的目标函数为：

$f_1=\underset{i=n_1}{\overset{n_2}{\mathrm{\Σ}}}\sqrt{{\left(x_{ZMP}\right(i)-c_x(i\left)\right)}^2+{\left(y_{ZMP}\right(i)-c_y(i\left)\right)}^2};$

左脚支撑时的目标函数为：

$f_2=\mathrm{\λ}\underset{i=n_2}{\overset{n_3}{\mathrm{\Σ}}}|x_{ZMP}\left(i\right)-c_x\left(i\right)|+\mathrm{\μ}\underset{i=n_2}{\overset{n_3}{\mathrm{\Σ}}}|y_{ZMP}\left(i\right)-c_y\left(i\right)|;$

双脚支撑右脚在前时的目标函数为：

$f_3=\underset{i=n_3}{\overset{n_4}{\mathrm{\Σ}}}\sqrt{{\left(x_{ZMP}\right(i)-c_x(i\left)\right)}^2+{\left(y_{ZMP}\right(i)-c_y(i\left)\right)}^2};$

右脚支撑时的目标函数为：

$f_4=\mathrm{\λ}\underset{i=n_4}{\overset{n_5}{\mathrm{\Σ}}}|x_{ZMP}\left(i\right)-c_x\left(i\right)|+\mathrm{\μ}\underset{i=n_4}{\overset{n_5}{\mathrm{\Σ}}}|y_{ZMP}\left(i\right)-c_y\left(i\right)|;$

适应度函数值为：

f＝βf₁+αf₂+βf₃+αf₄；

其中，f₁、f₂、f₃、f₄和f分别表示双脚支撑左脚在前时的目标函数、左脚支撑时的目标函数、双脚支撑右脚在前时的目标函数、右脚支撑时的目标函数和适应度函数值，n₁、n₂、n₃、n₄和n₅分别表示机器人在步行时的五个时刻，i取值为n₁、n₂、n₃、n₄或n₅，(c_x(i),c_y(i))表示不同时刻支撑区域中心坐标，(x_ZMP(i),y_ZMP(i))表示不同时刻的零力矩点坐标，λ和μ分别表示两个参数，且满足λ/μ＝W/L，λ+μ＝1，W表示机器人的脚掌宽度，L表示机器人的脚掌长度，α和β分别表示两个权重值，α/β＝S_d/S_s，α+β＝1，S_d表示双脚支撑阶段稳定区域面积，S_s表示单脚支撑阶段稳定区域面积。

由于单脚支撑和双脚支撑时ZMP落在稳定区域内的概率不同，适应度函数在计算时应有所侧重。设步长为Ds，双脚支撑阶段稳定区域面积为S_d，单脚支撑阶段稳定区域面积为S_s

当L≥Ds时，S_d＝2W·L+2Ds·G+(2G-W)·(L-Ds)；

当L＜Ds时，S_d＝2W·L+W²·Ds/2G+(L+W·Ds/2G)·(2G-W)；

在一种可选的实施例中，利用粒子群算法对粒子的速度和位置进行更新的步骤包括：

寻找个体极值和群体极值。

根据个体极值和群体极值对粒子的速度和位置进行更新。

在一种具体的实施例中，可以将20组初始化的两髋关节中心参数中选出适应度值最大的一组定义为群体极值位置；将初始化的20组两髋关节中心参数定义为个体极值位置。

在一种具体的实施例中，利用公式：

$V_{id}^{k+1}=V_{id}^k+c_1{r}_1(P_{id}^k-X_{id}^k)+c_2{r}_2(P_{gd}^k-X_{id}^k);$

$X_{id}^{k+1}=X_{id}^k+V_{id}^{k+1};$

根据当前迭代选取出的个体最优极值和群体最优极值来进行速度和位置的更新。其中，d＝1,2,…,D；i＝1,2,…,n；k为当前迭代次数；V_id为粒子的速度；c₁和c₂是非负的常数，称为加速度因子；r₁和r₂是分布于[0,1]区间的随机数。

在一种可选的实施例中，对粒子进行变异的步骤包括：随机选择两髋关节参数中的一组参数中的一个参数进行变异。

具体的，变异操作的主要作用是维持种群多样性。变异操作从种群中随机选取一个个体，选择个体中的一点进行变异以产生更优秀的个体，再具体一点，变异操作可以从种群粒子即两髋关节中心参数中随机选取一组参数，再选择这组参数中的一个进行变异以产生更优秀的个体。

具体的，根据上述的变异操作对粒子进行变异得到新的粒子后，需要重新计算适应度值，与之前算得的适应度值进行比较，在一种可选的实施例中，若新计算出的适应度值比前面的适应度值大，则更新个体极值及群体极值。否则保留原来的个体极值和群体极值，若新的粒子计算出的关节角度不在关节角度限定范围内，则直接返回进行速度更新和位置更新，同样保留原来的个体极值和群体极值。

在一种可选的实施例中，在对步骤S100-步骤S102进行循环时，若进化次数设置为300，则需循环300次，从300次循环中得到适应度值最大的一组两髋关节中心参数。如图11所示，图11具体为优化过程中，适应度函数的变化曲线，如果重复以上算法30次，发现得到的适应度值及两髋关节中心参数非常接近。

在一种可选的实施例中，在得到两髋关节中心参数后，将两髋关节中心参数代入ZMP函数，可以得到如图12所示的ZMP轨迹。

需要说明的是，由于在整个步行阶段，仿人机器人要经历起步、多次周期步行以及止步，而起步和止步两个阶段的步态与周期步行时不同。因此需要对其参数进行单独优化，但为了保证步态的连续，起步阶段与周期步行相连接的径向和侧向的参数要与周期步行起始时的参数保持一致，止步阶段起始时与周期步行相连接的径向和侧向的参数要与周期步行结束时的参数保持一致。由于所用到的优化算法与周期步行时的相同，本发明不再累述。其中，机器人经历了起步、第一周期、第二周期、止步后的x-z方向和三维步态轨迹的曲棍图如图13和图14所示。

通过上述实施例，本发明基于改良粒子群算法的机器人稳定步态规划的方法适用于仿人机器人，尤其是高性能仿人机器人，首先，根据机器人的步行要求，给定步态中踝关节及髋关节在各关键时刻径向和侧向的位置及边界条件，利用三次样条插值函数生成双腿在径向和侧向的运动轨迹，利用D-H建模方法建立仿人机器人双腿模型，并应用D-H算法得出双腿的正逆运动学解算算法，应用腿部逆运动学算法，根据髋关节及踝关节的运动轨迹求解出所有关节在各时刻应达到的关节角度。应用正运动学算法，求解出各连杆质心在参考坐标系中的坐标，根据生成的轨迹及正逆运动学方程，确定ZMP(零力矩点)坐标，根据ZMP的稳定裕度确定适应度值，最后，利用改良的粒子群算法得到优化后的两髋关节中心的最优解，从而指导高性能仿人机器人稳定步行。本发明通过粒子速度、位置更新和粒子自身变异的方式搜索最优解，避免了陷入局部最优解的情况，利用该方法可以使优化后的参数满足ZMP落在稳定区域内，并能保证稳定裕度大，稳定性好。本发明以类稳定裕度函数为适应度函数进行优化，类稳定裕度函数综合考虑到各步行阶段的稳定裕度，并对不同步行阶段分配不同的权重。可以使仿人机器人在整个步行阶段具有良好的稳定性。

实施例2

根据本发明实施例，提供了一种基于改良粒子群算法的机器人稳定步态规划的产品实施例，图15是根据本发明实施例的基于改良粒子群算法的机器人稳定步态规划装置，如图15所示，该装置包括第一确定模块101、变异模块103、循环模块105、第二确定模块107和第三确定模块109。

其中，第一确定模块101，用于确定适应度函数值，并根据适应度函数值，利用粒子群算法对粒子的速度和位置进行更新。

变异模块103，用于对粒子进行变异，并重新确定适应度函数值。

循环模块105，用于循环调用第一确定模块101和变异模块103，直至满足预设次数。

第二确定模块107，用于确定适应度函数值最大时的两髋关节中心参数。

第三确定模块109，用于根据两髋关节中心参数，利用三次样条插值函数确定机器人踝关节和髋关节的运动轨迹。

上述方法中已经对各个模块的具体实现过程做了具体的论述，此处不再赘述。

上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。

在本发明的上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述的部分，可以参见其他实施例的相关描述。

在本申请所提供的几个实施例中，应该理解到，所揭露的技术内容，可通过其它的方式实现。其中，以上所描述的装置实施例仅仅是示意性的，例如所述单元的划分，可以为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个***，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，单元或模块的间接耦合或通信连接，可以是电性或其它的形式。

所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可为个人计算机、服务器或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、移动硬盘、磁碟或者光盘等各种可以存储程序代码的介质。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (10)

1.一种基于改良粒子群算法的机器人稳定步态规划方法，其特征在于，包括：

确定适应度函数值，并根据所述适应度函数值，利用粒子群算法对粒子的速度和位置进行更新；

对粒子进行变异，并重新确定所述适应度函数值；

循环上述所有步骤，直至满足预设次数；

确定所述适应度函数值最大时的两髋关节中心参数；

根据所述两髋关节中心参数，利用三次样条插值函数确定所述机器人踝关节和髋关节的运动轨迹。

2.根据权利要求1所述的方法，其特征在于，所述确定适度函数值的步骤包括：

根据零力矩点的类稳定裕度确定所述适应度函数值。

3.根据权利要求2所述的方法，其特征在于，所述根据零力矩点的类稳定裕度确定所述适应度函数值的步骤包括：

确定所述机器人运动的不同阶段时的目标函数和权重；

根据所述机器人运动的不同阶段时的目标函数和权重确定所述适应度函数值。

4.根据权利要求3所述的方法，其特征在于，所述机器人运动的不同阶段包括：双脚支撑左脚在前、左脚支撑、双脚支撑右脚在前和右脚支撑。

5.根据权利要求4所述的方法，其特征在于，所述根据零力矩点的类稳定裕度确定所述适应度函数值的步骤中使用如下公式：

双脚支撑左脚在前时的目标函数为：

$f_1=\underset{i=n_1}{\overset{n_2}{\Σ}}\sqrt{{\left(x_{ZMP}\right(i)-c_x(i\left)\right)}^2+{\left(y_{ZMP}\right(i)-c_y(i\left)\right)}^2};$

左脚支撑时的目标函数为：

$f_2=\mathrm{\λ}\underset{i=n_2}{\overset{n_3}{\Σ}}|x_{ZMP}\left(i\right)-c_x\left(i\right)|+\mathrm{\μ}\underset{i=n_2}{\overset{n_3}{\Σ}}|y_{ZMP}\left(i\right)-c_y\left(i\right)|;$

双脚支撑右脚在前时的目标函数为：

$f_3=\underset{i=n_3}{\overset{n_4}{\Σ}}\sqrt{{\left(x_{ZMP}\right(i)-c_x(i\left)\right)}^2+{\left(y_{ZMP}\right(i)-c_y(i\left)\right)}^2};$

右脚支撑时的目标函数为：

$f_4=\mathrm{\λ}\underset{i=n_4}{\overset{n_5}{\Σ}}|x_{ZMP}\left(i\right)-c_x\left(i\right)|+\mathrm{\μ}\underset{i=n_4}{\overset{n_5}{\Σ}}|y_{ZMP}\left(i\right)-c_y\left(i\right)|;$

适应度函数值为：

f＝βf₁+αf₂+βf₃+αf₄；

其中，f₁、f₂、f₃、f₄和f分别表示双脚支撑左脚在前时的目标函数、左脚支撑时的目标函数、双脚支撑右脚在前时的目标函数、右脚支撑时的目标函数和适应度函数值，n₁、n₂、n₃、n₄和n₅分别表示所述机器人在步行时的五个时刻，i取值为n₁、n₂、n₃、n₄或n₅，(c_x(i),c_y(i))表示不同时刻支撑区域中心坐标，(x_ZMP(i),y_ZMP(i))表示不同时刻的零力矩点坐标，λ和μ分别表示两个参数，且满足λ/μ＝W/L，λ+μ＝1，W表示所述机器人的脚掌宽度，L表示所述机器人的脚掌长度，α和β分别表示两个权重值，且满足α/β＝S_d/S_s，α+β＝1，S_d表示双脚支撑阶段稳定区域面积，S_s表示单脚支撑阶段稳定区域面积。

6.根据权利要求1所述的方法，其特征在于，所述利用粒子群算法对粒子的速度和位置进行更新的步骤包括：

寻找个体极值和群体极值；

根据所述个体极值和所述群体极值对粒子的速度和位置进行更新。

7.根据权利要求1所述的方法，其特征在于，所述确定适应度函数值的步骤之前包括：

对粒子和速度进行初始化；

利用三次样条插值函数规划所述机器人髋关节和踝关节的运动轨迹；

利用腿部逆运动学算法，根据所述机器人髋关节和踝关节的运动轨迹，计算所述机器人所有关节的关节角度；

利用正运动学算法，计算所述机器人各连杆质心的坐标；

根据所述机器人各连杆质心的坐标确定零力矩点的坐标。

8.根据权利要求7所述的方法，其特征在于，所述利用腿部逆运动学算法，根据所述机器人髋关节和踝关节的运动轨迹，计算所述机器人所有关节的关节角度之前包括：

利用D-H建模方法建立所述机器人的双腿模型，其中，所述机器人每只腿有6个自由度，髋关节3个自由度且所述髋关节的3个自由度不交于一点，膝关节1个自由度，踝关节2个自由度且所述踝关节为2自由度空间并联机构。

9.根据权利要求1所述的方法，其特征在于，所述对粒子进行变异的步骤包括：

随机选择两髋关节参数中的一组参数中的一个参数进行变异。

10.一种基于改良粒子群算法的机器人稳定步态规划装置，其特征在于，包括：

第一确定模块，用于确定适应度函数值，并根据所述适应度函数值，利用粒子群算法对粒子的速度和位置进行更新；

变异模块，用于对粒子进行变异，并重新确定所述适应度函数值；

循环模块，用于循环调用所述第一确定模块和所述变异模块，直至满足预设次数；

第二确定模块，用于确定所述适应度函数值最大时的两髋关节中心参数；

第三确定模块，用于根据所述两髋关节中心参数，利用三次样条插值函数确定所述机器人踝关节和髋关节的运动轨迹。