CN101847009A - 两足机器人步态能效优化方法 - Google Patents

Abstract

本发明为一种两足机器人步态能效优化方法，包括：(1)规划得到足机器人下肢的行走轨迹；(2)能效优化得到机器人的理想步态；(3)通过单关节控制器来控制机器人；其特征在于所述能效优化得到机器人的理想步态具体包括：模糊ZMP整定、上体能耗评估和基于能耗评估的迭代优化步骤。本发明提出了一种基于模糊逻辑的两足机器人步态能效优化方法，目的是解决两足机器人行走过程中的高能耗问题。该方法能够有效降低机器人能耗并保持其稳定性。

Description

两足机器人步态能效优化方法

技术领域

本发明属于两足机器人步态控制领域。

背景技术

机器人是近几十年来发展起来的综合学科。它集中了机械工程-电子工程、计算机工程、自动控制工程以及人工智能等多种学科的最新科研成果，代表了机电一体化的最高成就，是目前科技发展最活跃的领域之一。

自从70年代工业机器人应用于工业生产以来，机器人对社会的生产，生活产生了深远的影响。双足机器人不同于一般的工业机器人。这种机器人具有灵活的行走系统，具有广阔的工作空间，以便随时走到需要的地方，包括一些对普通人来说不易到达的地方和角落，完成人或智能系统预先设置指定的工作，而且能耗很小。因此，特别适合于日常环境与人类协作完成各种任务，在康复、日常服务、危险环境作业的领域具有广阔的应用潜力。另一方面，由于双足机器人具有多关节，多驱动器，强耦合和多传感器的特点，而且一般多有冗余的自由度，这些特点对其控制问题带来了很大的难度，为各种控制优化刚刚提供了理想的实验平台，使其吸引了许多学者的目光，成为一个瞩目的研究方向。

但能量效率低下已成为两足机器人实用化进程的一大障碍。被动行走(PDW)为实现高效步行提供了一个可能的解决途径，然而当前的被动机器人尚难完成转向、启停等基本动作，且非常容易摔倒，这极大地限制了被动步行机器人的实际应用。传统的主动步行利用跟踪预设关节轨迹的控制方法，虽然可实现类人行走和跑步，但依赖大力矩驱动的高能耗特性成为其广泛应用和进一步发展的瓶颈问题之一。

针对两足机器人步行运动的能效及稳定性问题，已有学者做了一些研究。其中“零力矩点”(Zero moment point，简称ZMP)理论是1968年由南斯拉夫学者M.Vukobratovi提出的步行机器人动态平衡理论。目前，基于ZMP的动态稳定判据仍然广泛应用于步行机器人和移动机械手的动态稳定运动的规划与控制中。

被动机器人方面，Schwab等人选择摆动足刚离地时为庞加莱截面，降维计算得出了被动机器人稳定行走的吸引盆，讨论了所有使得机器人能够稳定运动的角位移与角速度的参数集合。Goswami等人通过在被动机器人基础上增加驱动与控制研制了半被动的步行机器人，通过施加在机器人髋关节和踝关节的控制力矩来调节机器人的能量，扩大了机器人稳定行走的吸引盆。然而仅仅依靠重力为动力源的被动机器人对环境以及机器人初始条件的要求均非常苛刻，半被动机器人的驱动与控制又不能过多地干预机器人的被动特性，否则会丧失被动机器人原有的优点。主动步行机器人方面，Kim DW等人利用基于径向基(RBF)核函数的支持向量回归(SVR)算法实现稳定步行，有效应用于两足步行机器人平地行走和上斜坡运动的建模和控制。Endo G等人提出了一种基于中枢模式发生器(CPG)的在线强化学习算法，该算法能够在200次迭代以内改善两足步行控制器的动态特性。Dip G等人对一个12自由度的两足机器人进行步态最优化分析，采用遗传算法(GA)实现ZMP稳定裕度和步行速度之间的平衡；Vundavilli PR等人针对机器人本身固有的复杂性和环境的不确定性，通过神经网络(NN)和模糊逻辑控制器(FLC)两个模块为步态生成问题建模，并通过软计算方法对系统进行离线优化以获取两足机器人上下斜坡的动态平衡。遗憾的是，以上针对主动步行稳定性的研究均没有考虑能效问题。传统的双足步行机器人以Honda公司的Asimo为代表，虽然在非结构化环境中具有良好的移动能力，但其行走能耗是人类行走能耗的数十倍以上。因此，目前迫切需要一种系统的方法来获取机器人稳定性与能效之间的平衡。

发明内容

本发明的主要目的在于提出一种全新的、系统化的步态能效优化方法来解决高能耗产生的两足机器人实用化障碍，本发明能够有效降低机器人能耗并保持其稳定性。

本发明为一种两足机器人步态能效优化方法，包括：

(1)规划得到足机器人下肢的行走轨迹；

(2)能效优化得到机器人的理想步态；

(3)通过单关节控制器来控制机器人；

其特征在于所述能效优化得到机器人的理想步态具体包括：模糊ZMP整定、上体能耗评估和基于能耗评估的迭代优化步骤。

所述模糊ZMP整定包括模糊逻辑的ZMP轨迹设计和基于ZMP动态的上体轨迹生成；所述模糊逻辑的ZMP轨迹设计是通过模糊逻辑得到机器人行走过程中的ZMP值；所述ZMP动态的上体轨迹生成是通过模糊ZMP解ZMP方程。

所述模糊逻辑的ZMP轨迹设计的模糊规则选取遵循以下条件：摆动腿踝关节的轨迹y_ankle与规划值y′_ankle偏离越多，ZMP往对应方向的调整量越大；支撑腿髋关节的轨迹y_hip与规划值y′_hip偏离越多，ZMP往对应方向的调整量越大；模糊系统的输入为机器人前进方向摆动腿踝关节偏移量e_ankle和髋关节偏移量e_hip，e_ankle＝y′_ankle-y_ankle，e_hip＝y′_hip-y_hip。系统的输出为模糊整定的机器人ZMP轨迹

系统的输入输出都采用三角隶属函数模糊化；模糊系统的第i条规则表述如下：

$\mathrm{Rulei}:\mathrm{If}{e}_{\mathrm{ankle}}\mathrm{is}{A}_{1,i}\mathrm{and}{e}_{\mathrm{hip}}\mathrm{is}{A}_{2,i}\mathrm{Then}{\hat{y}}_{\mathrm{zmp}}\mathrm{is}{B}_i$

其中，e_ankle和e_hip分别是机器人前进方向摆动腿踝关节和支撑腿髋关节的偏移量，

为模糊整定的ZMP轨迹；通过重心模解糊器得到模糊系统的精确输出为机器人ZMP轨迹

所述基于ZMP动态的上体轨迹生成步骤具体为：假设无外力作用，且上体的高度不变，也即

把机器人的每一个连杆看作一个质点，将ZMP轨迹代入到ZMP方程，通过Runge-Kutta方法，解出上体轨迹y_trunk，将此解定义为模糊ZMP整定的，令机器人系统稳定的上体轨迹

所述上体能耗评估步骤具体为：首先通过上体轨迹

预估上体关节角速度和上体关节驱动力矩；然后构造上体关节预估能耗的计算式，分析上体关节角速度、上体关节驱动力矩与预估能耗之间的函数关系。

所述的基于能耗评估的迭代优化步骤针对两足机器人实用化过程中稳定性和低能耗的综合需求，参照前两个步骤中分别依据ZMP稳定判据和能耗最小原则推出的两种上体轨迹，讨论上体轨迹的稳定约束条件，提出了此约束条件下的能效优化迭代算法，将此目标描述为一个单变量约束的优化问题：

$\left\{\begin{array}{cc}\min & {\hat{E}}_{\mathrm{trunk}}=h\left({\hat{y}}_{\mathrm{trunk}}\right)\\ s.t& {\hat{y}}_{\mathrm{trunk}}\∈({\underset{\‾}{\overset{^}{y}}}_{\mathrm{trunk}},{\stackrel{\‾}{\hat{y}}}_{\mathrm{trunk}})\end{array}\right.$

也即求能耗函数

在机器人上体轨迹区间

上的极小点，此极小点

即为稳定条件下最小能耗的上体位置；

是能耗最小表达式，

是稳定性约束条件；表示上体预估位置

的下界，表示上体预估位置

的上界，上体位置下界

和上界

均可依据机器人结构参数得出。

本发明与现有技术相比，是基于ZMP稳定判据与两足机器人能耗分析，为机器人步态的能效优化提供了一种全新的系统化方法，该方法能够有效降低机器人能耗并保持其稳定性。与传统的机器人控制系统不同，该方法增加了基于能效优化的外环控制部分。能效优化控制功能由模糊ZMP整定、上体能耗评估和基于能耗评估的迭代优化等三个模块合作完成。其中，模糊ZMP整定模块针对两足机器人步行过程中腿部轨迹发生变化的情况，提出基于ZMP稳定性判据的模糊策略以调整上体轨迹，使机器人保持稳定步行。上体能耗评估模块引入了机器人步行过程中能量消耗的评估办法，为合理设计机器人步态从而达到能效最优的步行提供了理论依据。基于能耗评估的迭代优化模块针对两足机器人实用化过程中稳定性和低能耗的综合需求，提出了稳定约束条件下以能耗最小为优化目标的迭代优化算法。

附图说明

图1控制系统框图

图2模糊ZMP整定模块示意图

图3上体能耗评估模块示意图

图4基于能耗评估的迭代优化示意图

图5M-ZMP时的步态图(例一)

图6F-ZMP时的步态图(例一)

图7EE-ZMP时的步态图(例一)

图8M-ZMP，F-ZMP和EE-ZMP的功率图(例一)

图9M-ZMP时的步态图(例二)

图10F-ZMP时的步态图(例二)

图11EE-ZMP时的步态图(例二)

图12M-ZMP，F-ZMP和EE-ZMP的功率图(例二)

具体实施方式

本发明提出了一种基于模糊逻辑的两足机器人步态能效优化方法，目的是解决两足机器人行走过程中的高能耗问题。该方法能够有效降低机器人能耗并保持其稳定性。

下面对本发明的详细过程进行说明。

图1为本发明所述的能效优化控制系统总体框图。在图1中我们首先离线规划得到足机器人下肢的行走轨迹。在这基础上，我们通过在线能效优化在线得到机器人的理想的步态，再通过单关节控制器来控制机器人。其中，能效优化环节包括模糊ZMP整定、上体能耗评估和基于能耗评估的迭代优化三个环节。

图2为模糊ZMP整定模块示意图，它包括基于模糊逻辑的ZMP轨迹设计和基于ZMP动态的上体轨迹生成两部分。基于模糊逻辑的ZMP轨迹设计主要讨论机器人前进方向的稳定性问题，其模糊系统的输入为机器人前进方向摆动腿踝关节偏移量e_ankle和髋关节偏移量e_hip，e_ankle＝y′_ankle-y_ankle，e_hip＝y′_hip-y_hip。系统的输出为模糊整定的机器人ZMP轨迹

基于ZMP动态的上体轨迹生成要解决的是通过理想ZMP轨迹在线的到上身的轨迹

其过程如下：

将ZMP轨迹代入下式：

$y_{\mathrm{zmp}}=\frac{\mathrm{\Σ}{m}_i[({\stackrel{\·\·}{z}}_i+g)y_i-{\stackrel{\·\·}{y}}_i{z}_i]}{\mathrm{\Σ}{m}_i({\stackrel{\·\·}{z}}_i+g)}---\left(4\right)$

得：

$m_{\mathrm{trunk}}{\stackrel{\·\·}{y}}_{\mathrm{trunk}}{z}_{\mathrm{trunk}}-m_{\mathrm{trunk}}{\mathrm{gy}}_{\mathrm{trunk}}$

$=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{m}_i({\stackrel{\·\·}{z}}_i+g)y_i-\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{m}_i{\stackrel{\·\·}{y}}_i{z}_i+y_{\mathrm{zmp}}\{-m_{\mathrm{trunk}}g-\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{m}_i({\stackrel{\·\·}{z}}_i+g)\}---\left(5\right)$

其中，y_zmp为ZMP点在y轴方向的位置，这里取

m_trunk为上体质量，y_trunk和z_trunk分别表示上体在y轴方向和z轴方向的位置，为上体在y轴方向的加速度，m_i、y_i、

(i＝1，…，n)分别表示机器人其它n个杆件的质量、位置和加速度，取重力加速度g＝9.8m/s²。假设无外力作用，且上体的高度不变，也即把机器人的每一个连杆看作一个质点，通过Runge-Kutta方法(龙格-库塔法Runge-Kutta是用于模拟常微分方程的解的重要的一类隐式或显式迭代法)，由式(5)可解出上体轨迹y_trunk，将此解定义为模糊ZMP整定的、令机器人系统稳定的上体轨迹

考虑到能耗问题，这里不直接将上体轨迹

输出至逆运动学计算环节，而是将

作为上体预估位置送入关节能耗评估模块，以便求出能效优化的理想上体位置

进一步的处理由下一个模块，也即上体能耗评估模块来完成。

图3为上体能耗评估模块示意图，它包括上体状态预估和上体能量预估。其评估步骤为：首先预估上体关节角速度和上体关节驱动力矩；然后构造上体关节预估能耗的计算式，分析上体关节角速度、上体关节驱动力矩与预估能耗之间的函数关系。

1)、上体关节角速度预估

取杆状上体的几何中点为质心m_trunk的坐标位置，考虑质心m_trunk当前位置y_trunk与预估位置

的时间间隔为T，可得机器人前进方向的上体关节角速度预估值

${\widehat{\stackrel{\·}{\mathrm{\θ}}}}_{\mathrm{trunk}}=2({\hat{y}}_{\mathrm{trunk}}-y_{\mathrm{trunk}})/l_{\mathrm{trunk}}T---\left(6\right)$

其中，l_trunk为杆状上体的长度。

2)、上体关节驱动力矩预估

由模糊ZMP整定模块输出的预估位置

通过逆运动学求解和计算力矩两个步骤可得上体关节驱动力矩

逆运动学解决已知上体位置，求上体关节角的问题：用D-H方法建立机器人杆件的坐标系，由D-H参数确定坐标系间的齐次变换矩阵

机器人的运动学方程为

⁰A₁(θ₁)¹A₂(θ₂)…^n-1A_n(θ_n)＝⁰A_n                                (7)

由式(15)即可解出θ_i(i＝1，…，n)。这里Θ＝[θ₁，…θ_trunk…，θ_n]^T表示各关节角。计算力矩法解决已知关节角，求关节驱动力矩的问题：

${\widehat{\mathrm{\τ}}}_{\mathrm{trunk}}\left({\hat{y}}_{\mathrm{trunk}}\right)=\hat{M}\left({\mathrm{\θ}}_{\mathrm{trunk}}\right)({\stackrel{\·\·}{\mathrm{\θ}}}_{\mathrm{trunk}}^d+K_v{\stackrel{\·}{e}}_{\mathrm{trunk}}+K_p{e}_{\mathrm{trunk}})$

$+\hat{C}({\mathrm{\θ}}_{\mathrm{trunk}},{\stackrel{\·}{\mathrm{\θ}}}_{\mathrm{trunk}}){\stackrel{\·}{\mathrm{\θ}}}_{\mathrm{trunk}}+\hat{G}\left({\mathrm{\θ}}_{\mathrm{trunk}}\right)---\left(8\right)$

其中，“^”表示估计参数，

是机器人上体的期望轨迹，K_p和K_v分别是比例和微分增益矩阵。

3)、构造能耗指标表达式

考虑到两足机器人系统能效评价的综合性和复杂性，构造的上体关节能耗总评估函数中同时考虑了平均功率

平均功率偏差

和平均力矩损耗等三项指标。其中，平均功率偏差指标是为了准确评估机器人关节瞬时功率在平均绝对功率周围分布的标准偏差的估计值，从而避免平均功率较小而瞬时功率极大的不理想状况。假设驱动力矩不做负功，对所有时刻的功率取绝对值，对于两足机器人的腿i、关节j，衡量关节能量消耗的指标具体如下：

平均功率 $P_{\mathrm{av}}\left(t\right)=\frac{1}{T}\underset{i,j}{\mathrm{\Σ}}{\∫}_0^T\left|{\mathrm{\τ}}_{\mathrm{ij}}\left(t\right){\stackrel{\·}{\mathrm{\θ}}}_{\mathrm{ij}}\left(t\right)\right|\mathrm{dt}---\left(9\right)$

平均功率偏差 $D_{\mathrm{av}}\left(t\right)=\sqrt{\frac{1}{T}{\∫}_0^T{(P_i-P_{\mathrm{av}})}^2\mathrm{dt}}---\left(10\right)$

平均力矩损耗 $L_e\left(t\right)=\frac{1}{T}{\∫}_0^T{\left[\mathrm{\τ}\left(t\right)\right]}^2\mathrm{dt}---\left(11\right)$

其中，τ_ij(t)为关节驱动力矩，

为关节角速度，瞬时功率

$P_i\left(t\right)=\underset{i,j}{\mathrm{\Σ}}{\mathrm{\τ}}_{\mathrm{ij}}\left(t\right){\stackrel{\·}{\mathrm{\θ}}}_{\mathrm{ij}}\left(t\right)---\left(12\right)$

综合考虑以上三项指标，构造的预估上体关节能耗总评估函数

${\hat{E}}_{\mathrm{trunk}}=P_{\mathrm{av}}^{\mathrm{trunk}}\left(t\right)+D_{\mathrm{av}}^{\mathrm{trunk}}\left(t\right)+L_e^{\mathrm{trunk}}\left(t\right)---\left(13\right)$

其中，

分别表示两足机器人上体关节驱动器的平均功率、平均功率偏差和平均力矩损耗。将式(9)、(10)、(11)、(12)代入式(13)得预估上体关节能耗总评估函数

${\hat{E}}_{\mathrm{trunk}}=P_{\mathrm{av}}^{\mathrm{trunk}}\left(t\right)+D_{\mathrm{av}}^{\mathrm{trunk}}\left(t\right)+L_e^{\mathrm{trunk}}\left(t\right)$

$=\frac{1}{T}{\∫}_0^T\left|{\widehat{\mathrm{\τ}}}_{\mathrm{trunk}}\left(t\right){\widehat{\stackrel{\·}{\mathrm{\θ}}}}_{\mathrm{trunk}}\left(t\right)\right|\mathrm{dt}$

$+\sqrt{\frac{1}{T}{\∫}_0^T{[{\widehat{\mathrm{\τ}}}_{\mathrm{trunk}}\left(t\right){\widehat{\stackrel{\·}{\mathrm{\θ}}}}_{\mathrm{trunk}}\left(t\right)-\frac{1}{T}{\∫}_0^T|{\widehat{\mathrm{\τ}}}_{\mathrm{trunk}}\left(t\right){\widehat{\stackrel{\·}{\mathrm{\θ}}}}_{\mathrm{trunk}}\left(t\right)\left|\mathrm{dt}\right]}^2\mathrm{dt}}$

$+\frac{1}{T}{\∫}_0^T{\left[{\widehat{\mathrm{\τ}}}_{\mathrm{trunk}}\left(t\right)\right]}^2\mathrm{dt}---\left(14\right)$

其中，预估的关节速度

由当前上体位置y_trunk与模糊预估的上体位置

取T时间内的平均速度得到，预估的关节驱动力矩

由预估位置

通过经典的逆运动学及计算力矩方法得到。

为使预估能耗

与各项参数的关系更加明确，以便获得能耗最小的上体轨迹，参照图4将能耗评估模块各变量之间的关系直观地表示为：

${\hat{E}}_{\mathrm{trunk}}=f({\widehat{\stackrel{\·}{\mathrm{\θ}}}}_{\mathrm{trunk}},{\widehat{\mathrm{\τ}}}_{\mathrm{trunk}})=g({\hat{y}}_{\mathrm{trunk}},y_{\mathrm{trunk}})---\left(15\right)$

其中，f(·)和g(·)分别表示

与相应变量之间的函数关系。f(·)和g(·)的具体表达式取决于角速度预估、逆运动学计算、计算力矩法和能耗函数等各项因素。预估能耗

的取值与上体预估位置和当前位置y_trunk有关。记

其中

表示不带约束条件的、令能耗

最小的上体轨迹。显然，

不一定满足ZMP稳定性判据。因此，如何得到既满足稳定性判据、又满足能耗最小的步态，成为下一步要解决的问题。

图4为基于能耗评估的迭代优化示意图。本模块针对两足机器人实用化过程中稳定性和低能耗的综合需求，参照前两个模块中分别依据ZMP稳定判据和能耗最小原则推出的两种上体轨迹，讨论了上体轨迹的稳定约束条件，提出了此约束条件下的能效优化迭代算法。其优化过程如下：

1)、迭代优化性能指标的定义

基于能耗评估的迭代优化算法以能效最优为目标推导理想的低能耗稳态上体轨迹。将此目标描述为一个单变量约束的优化问题：

$\left\{\begin{array}{cc}\min & {\hat{E}}_{\mathrm{trunk}}=f({\widehat{\stackrel{\·}{\mathrm{\θ}}}}_{\mathrm{trunk}},{\widehat{\mathrm{\τ}}}_{\mathrm{trunk}})=g({\hat{y}}_{\mathrm{trunk}},y_{\mathrm{trunk}})\\ s.t& {\hat{y}}_{\mathrm{trunk}}\∈({\underset{\‾}{\overset{^}{y}}}_{\mathrm{trunk}},{\stackrel{\‾}{\hat{y}}}_{\mathrm{trunk}})\end{array}\right.---\left(16\right)$

其中，

是能耗最小表达式，

是稳定性约束条件。

表示上体预估位置

的下界，

表示上体预估位置

的上界，上体位置下界

和上界

均可依据机器人结构参数得出。考虑到上体的当前位置y_trunk为可测参数，故问题简化为已知y_trunk，应如何围绕模糊生成的

值，在其邻域内搜索一个最优值以使能耗

最小。于是目标函数简化为

$\left\{\begin{array}{cc}\min & {\hat{E}}_{\mathrm{trunk}}=h\left({\hat{y}}_{\mathrm{trunk}}\right)\\ s.t& {\hat{y}}_{\mathrm{trunk}}\∈({\underset{\‾}{\overset{^}{y}}}_{\mathrm{trunk}},{\stackrel{\‾}{\hat{y}}}_{\mathrm{trunk}})\end{array}\right.---\left(17\right)$

也即求能耗函数

在机器人上体轨迹区间

上的极小点，此极小点

即为稳定条件下最小能耗的上体位置。

2)、迭代优化算法描述

针对以上单变量约束优化问题提出了一种基于能耗评估的迭代优化算法，其总体思路是：围绕模糊定性的上体预估位置，在其邻域内搜索一个最优值以使能耗最小。具体分两步完成：首先考虑稳定性约束条件。依据机器人脚掌的几何尺寸，给出符合稳定性判据的ZMP轨迹范围，并通过ZMP方程求出相应的上体位置范围。然后搜索令能耗最小的上体轨迹。在ZMP稳定性判据的约束条件下，将上体轨迹离散化，采用梯度下降算法比较相邻采样点的能耗值，以能耗最小为优化目标，使相应的上体轨迹收敛到理想值附近。

A.能效优化的稳定性约束

机器人右脚踝关节在地面的投影为坐标原点，令机器人脚跟至坐标原点的径向距离为d_ankle-heel，机器人脚尖至坐标原点的径向距离为d_ankle-toe，依照人类前脚掌长度大于后脚掌长度的事实，取d_ankle-toe＞d_ankle-heel。由ZMP稳定性判据，单腿支撑期的稳定条件是

${\hat{y}}_{\mathrm{zmp}}\∈({\underset{\‾}{\overset{^}{y}}}_{\mathrm{zmp}},{\stackrel{\‾}{\hat{y}}}_{\mathrm{zmp}})=(-d_{\mathrm{ankle}-\mathrm{heel}},d_{\mathrm{ankle}-\mathrm{toe}})---\left(18\right)$

其中，

表示稳定条件下ZMP点在y轴方向的坐标下界，表示稳定条件下ZMP点在y轴方向的坐标上界。

也即单腿支撑期的ZMP点始终落在支撑脚构成的支撑多边形内。由式(18)得出的

取值区间及式(4)所示的ZMP方程，可得上体预估位置

的下界

和上界也即得到了优化问题的稳定性约束条件

B.迭代优化算法的具体步骤

步骤1)：上体轨迹离散化。对轨迹按a(n)步长间隔采样，得到N+1个离散值

n＝0，…，k，…，N。其中，N为依据控制精度和实时性要求而确定的任意非负整数。显然的，N越大，控制精度越高，但计算量增大，实时性减弱；

${\hat{y}}_{\mathrm{trunk}}(n+1)={\hat{y}}_{\mathrm{trunk}}\left(n\right)+a\left(n\right)$

$a\left(n\right)=n\frac{{\stackrel{\‾}{\hat{y}}}_{\mathrm{trunk}}-{\underset{\‾}{\overset{^}{y}}}_{\mathrm{trunk}}}{N},n=0,\·\·\·,k,\·\·\·,N$

步骤2)：能耗差分运算。令n＝1，代入

依据

计算

步骤3)：能耗变化趋势判断。若ΔE＜0，执行下一步；若ΔE＝0，输出

作为理想的上体轨迹

算法结束。否则若ΔE＞0，取

依据

计算

验证ΔE＜0，否则提示出错，算法结束。否则执行下一步；

步骤4)：稳定性约束。检测

是否满足

步骤5)：稳定约束下的最小能耗搜索。倍增步长使能耗递减，直至

不满足

时为止。也即若n＝1时满足

则取n＝2，3，…k，…，N，直至n＝k时不满足

步骤6)：输出稳定条件下能耗最小的上体轨迹。返回n＝k-1状态，输出其

值作为理想的上体轨迹

也即

至此已实现了本发明所述的基于模糊逻辑的两足机器人步态的能效优化。以下我们用仿真实验来验证本发明所提出的能优化算法的性能。

实验

采用具体应用实例并通过Matlab仿真来观察实验结果。

实验内容

本实验的目的在于比较本发明所提出的能效ZMP(EE-ZMP)方法与固定ZMP(F-ZMP)和移动ZMP(M-ZMP)性能，验证本发明所述低能耗性能。其中F-ZMP为机器人在行走过程中ZMP的位置为固定不动的，也就是在图1中没有能效优化这一模块。M-ZMP是指机器人在行走过程中ZMP是移动的，也就是在图1中能效优化模块只有模糊ZMP整定部分而没有上体能耗评估和基于上体能耗评估的迭代算法两部分。

我们做了两个例子的仿真。第一个例子我们第一个例子我们假设机器人在每个步行周期包括一个单脚支撑期和一个瞬时双脚支撑期。在单脚支撑期，支撑脚和地面完全接触，摆动腿脚掌与地面平行。机器人的步行周期是T_c(T_c＝1s)，采样周期为T(T＝0.05s)。机器人的步长为0.5m，步长为0.6m，步高为0.1m。其结果如图5到图8和下表(表1)。

表1机器人在EE-ZMP、F-ZMP和M-ZMP时的能耗表(例一)

	F-ZMP	M-ZMP	EE-ZMP
				E(J)	2.071	0.967	0.481

第二个例子我们假设机器人在每个步行周期包括一个单脚支撑期和一个双脚支撑期，其中双脚支撑期占整个步行周期的20％。在单脚支撑期，支撑脚和地面完全接触，而在双脚支撑期双脚绕其与地面的接触点转动。机器人的步行周期是T_c(T_c＝1s)，采样周期为T(T＝0.05s)。机器人的步长为0.5m，步长为0.6m，步高为0.1m。其结果如图9到图12和下表(表2)。

表2机器人在EE-ZMP、F-ZMP和M-ZMP时的能耗表(例二)

	F-ZMP	M-ZMP	EE-ZMP
				E(J)	2.64	1.256	0.628

图5，6，7和图9，10，11，为机器人的步态图。对比我们可以看出看到移动ZMP和能效ZMP上体摆动的幅度明显小于固定ZMP。图8和图12为功率图，其说明移动ZMP和能效ZMP明显优于固定ZMP；能效ZMP比移动ZMP也有较大的改善。表1，2明显的反应出每步消耗的能量固定ZMP最大能效ZMP最小。通过对比可知，在加了能效优化环节后，机器人的能耗大大减低了，同时也没失去机器人的稳定性。

Claims (6)

1.一种两足机器人步态能效优化方法，包括：

(1)规划得到足机器人下肢的行走轨迹；

(2)能效优化得到机器人的理想步态；

(3)通过单关节控制器来控制机器人；

其特征在于所述能效优化得到机器人的理想步态具体包括：模糊ZMP整定、上体能耗评估和基于能耗评估的迭代优化步骤。

2.根据权利要求1所述的两足机器人步态能效优化方法，其特征在于所述模糊ZMP整定包括模糊逻辑的ZMP轨迹设计和基于ZMP动态的上体轨迹生成；所述模糊逻辑的ZMP轨迹设计是通过模糊逻辑得到机器人行走过程中的ZMP值；所述ZMP动态的上体轨迹生成是通过模糊ZMP解ZMP方程。

3.根据权利要求2所述的两足机器人步态能效优化方法，其特征在于所述模糊逻辑的ZMP轨迹设计的模糊规则选取遵循以下条件：摆动腿踝关节的轨迹y_ankle与规划值y′_ankle偏离越多，ZMP往对应方向的调整量越大；支撑腿髋关节的轨迹y_hip与规划值y′_hip偏离越多，ZMP往对应方向的调整量越大；模糊系统的输入为机器人前进方向摆动腿踝关节偏移量e_ankle和髋关节偏移量e_hip，e_ankle＝y′_ankle-y_ankle，e_hip＝y′_hip-y_hip；系统的输出为模糊整定的机器人ZMP轨迹

系统的输入输出都采用三角隶属函数模糊化；模糊系统的第i条规则表述如下：

$\mathrm{Rulei}:\mathrm{If}{e}_{\mathrm{ankle}}\mathrm{is}{A}_{1,i}\mathrm{and}{e}_{\mathrm{hip}}\mathrm{is}{A}_{2,i}\mathrm{Then}{\hat{y}}_{\mathrm{zmp}}\mathrm{is}{B}_i$

其中，e_ankle和e_hip分别是机器人前进方向摆动腿踝关节和支撑腿髋关节的偏移量，

为模糊整定的ZMP轨迹；通过重心模解糊器得到模糊系统的精确输出为机器人ZMP轨迹

4.根据权利要求2所述的两足机器人步态能效优化方法，其特征在于所述基于ZMP动态的上体轨迹生成步骤具体为：假设无外力作用，且上体的高度不变，也即

把机器人的每一个连杆看作一个质点，将ZMP轨迹代入到ZMP方程，通过Runge-Kutta方法，解出上体轨迹y_trunk，将此解定义为模糊ZMP整定的，令机器人系统稳定的上体轨迹

5.根据权利要求1所述的两足机器人步态能效优化方法，其特征在于所述上体能耗评估步骤具体为：首先通过上体轨迹预估上体关节角速度和上体关节驱动力矩；然后构造上体关节预估能耗的计算式，分析上体关节角速度、上体关节驱动力矩与预估能耗之间的函数关系。

6.根据权利要求1所述的两足机器人步态能效优化方法，其特征在于所述的基于能耗评估的迭代优化步骤针对两足机器人实用化过程中稳定性和低能耗的综合需求，参照前两个步骤中分别依据ZMP稳定判据和能耗最小原则推出的两种上体轨迹，讨论上体轨迹的稳定约束条件，提出了此约束条件下的能效优化迭代算法，将此目标描述为一个单变量约束的优化问题：

$\left\{\begin{array}{cc}\min & {\hat{E}}_{\mathrm{trunk}}=h\left({\hat{y}}_{\mathrm{trunk}}\right)\\ s.t& {\hat{y}}_{\mathrm{trunk}}\∈({\underset{\‾}{\overset{^}{y}}}_{\mathrm{trunk}},{\stackrel{\‾}{\hat{y}}}_{\mathrm{trunk}})\end{array}\right.$

也即求能耗函数

在机器人上体轨迹区间

上的极小点，此极小点

即为稳定条件下最小能耗的上体位置；

是能耗最小表达式，是稳定性约束条件；

表示上体预估位置

的下界，

表示上体预估位置

的上界，上体位置下界

和上界均可依据机器人结构参数得出。

Images