双足机器人节能步态规划算法
卢志强 侯媛彬 孟芸 周福娜
摘 要：為提高双足机器人的步行性能，提出基于五质心倒立摆模型的节能步态规划算法。算法包括步态参数优化算法和步态合成算法。步态参数优化算法允许身体做三维运动，以有限阶傅里叶级数的系数表征特定步长下机器人身体的运动空间。通过离散化这些系数，使运动空间网格化。进而对网格交点进行逆动力学计算，划分出满足允许零力矩点区域要求的种子集合。算法以电机的负荷转矩和角速度的乘积为能耗指标函数，在每个种子的邻域迭代计算。按照最大梯度原则逐次逼近函数极小值，此时的电机角度序列作为对应步长下的解，存入数据库。步态合成算法按照步行距离，规划由起始步、中间步和停止步构成的完整行走轨迹。按照行走步长，从数据库读取腿关节电机的角度序列，并依据双足机器人行走中反馈的零力矩点，对序列进行修改。为验证算法有效性，进行了动态仿真实验和现实环境中双足步行实验。实验结果与固定身体高度或允许身体垂直运动的算法对比，证明步态算法具有明显的节能效果。该算法实现低能耗和高鲁棒性的折中，较好地解决具有高度非线性特征的双足机器人行走问题，为煤矿救援机器人的开发开辟一种新途径。
关键词：双足机器人;五质心模型;步态规划;零力矩点;空间网格化;梯度逼近
中图分类号：TP 242.6
文献标志码：A
文章编号：1672-9315（2021）03-0540-09
DOI：10.13800/j.cnki.xakjdxxb.2021.0320
Abstract：An energy-efficient gait-planning algorithm based on five-centroid inverted pendulum model is presented in the paper for improving the walking performance of biped robot.The gait-planning algorithm is divided into gait parameter optimization（GPO）algorithm and gait synthesis（GSYN）algorithm.GPO algorithm allows the body to perform three-dimensional motion，and represents the motion space of the robot body in a given step length with coefficients of the finite-order Fourier series.By discretizing these coefficients，the motion space is gridded.Then inverse-kinemics calculations of grid intersection points are carried out，and the seed set satisfying the requirement of the allowable zero moment point region is divided.The algorithm takes the product of the load torque and angular velocity of motors as the energy-consumption index function，and calculates iteratively it in the neighborhood of each seed.According to the principle of maximum gradient，the minimum value of the function is approximated successively，the current angle sequences of motors are stored in the database as the solution under the corresponding step length.Given a distance to be traveled，GSYN algorithm plans a complete walking trajectory，i.e.，two starting steps，multiple cyclic steps，and two stopping steps.Based on the walking step length，angle sequences in leg joints is read from the database，and these sequences are modified in lignt of the zero-moment-point feedback during the biped robot walking.In order to identify effectiveness of the proposed algorithm，the dynamic simulation experiment and the biped walking experiment in the real environment were conducted.The experimental results show that the gait algorithm in the paper has a significant energy-saving effect compared with the algorithm of the fixed body height or allowed body vertical motion.The algorithm can optimize the tradeoff between low energy consumption and high robustness and solve the problem of walking in biped robots with highly nonlinear characteristics，which opens up a new way for the development of coal mine rescue robots.Key words：biped robot;five mass model;gait planning;zero moment point;spatial gridding;gradient approximation
0 引 言
双足机器人具有类人的结构和外观，可以很好地适应人类的生活环境，是替代人类从事如煤矿救援等危险性工作的理想机器人[1]。人们期待双足机器人能够以类人的方式在现实环境中稳定可靠地行走[2]，甚至是奔跑[3]。虽然人类可以相对轻松地执行各种运动，但将人类的行为转化为双足机器人动态运动仍然是一项具有挑战性的任务。近40 a来，国内外研究机构与学者在双足机器人领域进行了许多理论[4]和实验研究[5]，并取得显著的发展，如HRP-4可以将车辆行驶在道路中央[6]，ATLAS能跨越障碍、攀爬楼梯等[7]。
双足行走问题是双足机器人运动研究中最重要且最根本的问题，按照研究思想的不同，提出双足行走方法可分为3类：其一是以人类行走特征为参考的步态生成方法。双足机器人设计的原型是人类本身，很多研究人员使用高速光学运动捕捉系统获取人体运动的轨迹，分析人类的运动特征，并将这些特征应用到仿人机器人的运动中。BOUTIN等捕捉人体全身运动轨迹，生成双足机器人步行模式，将该方法成功应用于HRP-2和HOAP-3这2款机器人的行走[8]。吴伟国等采集人体髋、腿、踝的运动样本，构建机器人仿人步行样本库，实现机器人与人跟随步行[9]。其二是基于中枢模式发生器（central pattern generator，CPG）的步态生成方法。人类的双足运动具有周期节律性的特征，CPG是能够产生节律运动的神经环路网络，模仿CPG产生节律信号，可控制机器人步行运动。刘成菊等使用脚底压力和腰部加速度作为CPG的驱动，实现了机器人NAO对不同地形的自适应行走[10]。JUANG等使用连续多目标蚁群优化的全连接递归神经网络构成CPG，解决NAO的多目标步态生成问题[11]。其三是基于双足机器人简化模型的步态生成方法。双足机器人一般有几十个自由度且没有固定的基座，导致其动力学系统非常复杂。考虑到系统复杂度和计算复杂度，利用完整的动力学系统模型难以实现双足机器人的步行运动，简化动力学系统便成为实现双足机器人步行運动很好且容易实现的方案。HONG等将仿人机器人简化成一个单质点和可伸缩不计质量的腿构成的线性倒立摆模型[12]，SHAFII等将双足机器人简化成一个不计质量桌子与将全部质量集中在一辆在桌面上行驶小车构成的桌子-小车模型[13]，SHIMMYO等在单质点倒立摆模型的基础上，进一步考虑了支撑腿和摆动腿质量，构成了三质心倒立摆模型[14]。
目前，已有的双足机器人结构复杂程度还远逊于人类，这决定了以人类特征为参考和基于中枢模式发生器的步态生成方法有其局限性，研究人员更倾向于采用双足机器人简化模型的方法。简化模型的精度决定了控制效果的精度，LUO等在三质心模型中，采用加入角动量的模型预测控制，获得了比单质心模型或一般三质心模型高的零力矩点寻迹精度[15]。另一方面，减少机器人控制中的约束条件能获得更优的控制效果，SHIN等允许机器人身体垂直运动，提高了行走速度、降低了驱动电机的能耗[16]。若允许机器人身体有更多自由度的运动，可以获得更好的控制效果。约束条件的减少，则意味着机器人控制中参数空间的增大，用人工智能的方法寻找最优解将是一个有意义的问题，WRIGHT等综述了机器人运动控制中的常用智能算法[17]。
结合当前双足机器人行走步态规划领域的研究成果和存在的不足，文中提出一种兼顾双足机器人行走稳定性的节能步态规划算法，解决双足机器人行走的步态规划与控制问题。
1 问题陈述
1.1 系统结构
文中使用的双足机器人，具有双臂和双腿，可以模仿人体的行走运动，机器人的每条腿有5个自由度，分别为髋部转角q1/q6，q2/q7，膝盖转角q3/q8，脚踝转角q4/q9和q5/q10，设关节向量q=[q1，q2，…，q10]T。机器人前、侧和垂直方向，分别被定义为X轴、Y轴和Z轴方向。世界坐标系的原点定位于机器人直立时的两脚中点，机器人模型如图1所示。双足机器人每个脚底分别安装4片FSR压力传感器，用来测量脚底作用力的分布，表1列出了机器人身体及右腿的质量及长度，机器人左腿的参数相同。拟议的双足机器人步态行走基于以下假设。
1）上身在任何时候都保持直立的姿态。人的躯干俯仰角度一般在3°以内[18]，多数相关研究表明，这个假设是可以接受的[8-10，12-16]。
2）双脚总是与地面平行。常见的多数仿人机器人没有脚趾，在抬脚和落脚时，不能发挥改善驱动性能的作用[19]。
3）一个步态周期T，包括双支撑（double support phase，DSP）时间TDSP和单支撑（single support phase，SSP）时间TSSP，定义DSP占空比TDSPT。人行走时，σ区间约15%～25%[18]，文中算法选择σ=25%。
1.2 ZMP方程
在诸多双足机器人的动态稳定性指标中，使用最为广泛的是零力矩点（zero moment point，ZMP）概念，ZMP是地面上由重力和惯性产生的总力矩水平分量为零的点[20]。设双足机器人的5个质量点分别为身体b，左腿lu，左足ld，右腿ru，右足rd，组成集合Pc={b，lu，ld，ru，rd}，位置为{ri|i∈Pc}，其中b与髋中心点的距离为hb。同时，设左髋关节lh，左膝关节lk，左踝关节lf，右髋关节rh，右膝关节rk，右踝关节rf，组成集合Pr={lh，lk，lf，rh，rk，rf}，位置为{rj|j∈Pr}。机器人的位置示意如图2所示。
在ZMP位置rZMP=[xZMP，yZMP，0]T处，地面作用力的合力F产生的力矩MZMP，与由于各质心的重力和惯性施加于机器人身体的合力矩M，满足
式中 n=[0，0，1]T为地面单位法向量;g=[0，0，-g]T为重力加速度向量;g=9.8 m/s2;Ii和i分别为位置i处的转动惯量和角加速度。KAJITA等证明的影响较小，是可忽略的[21]。将式（2）和（3）代入（1），可得简化的ZMP等式如式（4）所示。
1.3 有效ZMP区域
双足机器人的建模中不可避免的存在建模误差，HONG等将ZMP固定在支撑脚的中心，以实现最稳定的行走[12]，但这并不是高效的方法。在支撑脚的区域内，划出一些边缘区域，用来补偿建模误差[16]，并使ZMP位于支撑脚中部的有效ZMP区域（allowable ZMP region，AZR）方法，是一种在克服误差与高效行走之间权衡的较好方法。设足长lfl、足宽lfw的双足机器人，步行时步幅为s，双足Y轴距离是w，AZR示意如图3所示。文中分别用ηl=lal/lfl和ηw=law/lfw表示脚长方向和脚宽方向的容限，后续步态规划算法中，ηl取固定值，并约定η=ηw。以左脚在前，右脚在后为例，在第1个DSP中，AZR是由点r1，r2，r3，r4，r5，r6构成的六边形区域;在SSP时，AZR是由点r4，r5，r6，r7构成的长方形;在第2个DSP中，AZR是由点r5，r6，r7，r8，r9，r10构成的六边形。
1.4 能耗指标函数
依赖电池提供能源的双足机器人，能耗E包括运动能耗Em和非运动能耗Ea这2部分[22]。Em用于维持电机的运行，是系统能耗的主要部分，其值为电机瞬时电压向量um（t）和电流向量im（t）对时间t的积分。Ea用于传感器、控制器和内部电路运行，其功率Pa在一段时间内是相对稳定的，可以表示为时间t的线性函数。即
在机器人的能耗中，Em可通过步态轨迹优化显著降低[23]。包含众多伺服电机的双足机器人，准确测量um和im（t）并不容易，考虑到uTm（t）im（t）等于电机输出功率P2与铜损、铁损及附加损耗之和，而其中的主要成分P2为电机输出转矩T2（t）与角速度的乘积（t）。电机运行时，T2（t）用于克服各质心对电机转轴形成的负载力矩Tg（t）。在仿真或实测中，依据机器人的姿态即可计算Tg（t）。因此，若双足机器人一个步态周期有N个采样点，评价步态算法的能耗指标函数E定义为
1.5 問题定义
基于前面的分析，文中的节能步态规划问题可表述为
1）给定双足机器人步长和AZR，允许身体在行走中三维运动，满足1.1中假设条件，确定最小化能耗指标函数E的步态轨迹。
2）已知运动距离，确定包含开始步、中间步和停止步的完整步态轨迹，并利用AZR，提高双足机器人行走的鲁棒性。
2 步态规划算法
2.1 算法概述
文中提出的步态规划算法分为步态参数优化（gait parameter optimization，GPO）算法和步态合成（gait synthesis，GSYN）算法2部分，如图4所示。
1）GPO算法从步长集合
S和AZR容限集合H选取元素si和ηi，在si傅里叶级数表示空间中布网运算，获得符合ηi要求的种子集合
Pi，在Pi中元素pi的邻域，依据文中1.4中的算法计算能耗指标函数，按照最优梯度法迭代运算，获得最小能耗指标函数的电机优化角度序列（步态轨迹）
gi={qi（n）|N=1，2，…，N}，存入数据库。
GPO算法运算时间较长，算法宜于离线运行，对S和H完成运算后，将离线数据库中的结果，复制到在线数据库，GSYN计算中可实时调用。
2）GSYN算法根据要求的步行距离
d，规划出步长序列S*和初始AZR容限序列H*，依次从S*和H*中取出每步元素sj和初始j，查询在线数据库，得到步态控制的关节角度序列gj。gj中的数据依次输入电机，可实现机器人的行走。行走中按照脚步压力集合Fj，计算真实的ZMP轨迹，并得到需要修正的Δηj。由Δηj构成步态反馈，将j修正为ηj，可满足机器人运动低能耗和高鲁棒性的折中。
2.2 GPO算法
GPO算法用于生成步态控制的电机角度序列，是文中算法的核心。按照双足机器人行走的精度要求，从最小步长至最大步长离散取值，确定S中的元素si。H从值域[0，1]选取若干等级，对应不同的鲁棒性要求的ηi。GPO算法从S和H依次选取元素si和ηi，进行布网梯度优化计算，以获得满足最小化E的gi。
2.2.1 布网计算方法
所谓布网计算，是将多维参数取值按照一定间隙离散化为网格，使构成的参数空间网格化，进而计算各个网格交点函数值的过程。若机器人步长为si，身体b进行满足1.1中假设1的三维周期运动，位置rb（n）=[xb（n） yb（n） zb（n）]T可用有限阶的傅里叶级数表示，即
另一方面，若设机器人运动起始为左腿在前，右腿在后的双支撑阶段，随后右腿摆动向前，且右脚rf起始位置为[0 0 0]T，并设运动满足1.1中假设3，则位置
为对比算法性能，以机器人髋高度固定[15]和髋高度按余弦波形垂直变化[16]，其他条件与文中算法相同，设置算法二和算法三，经过优化计算后，其E分别为280.4和252.4 mJ，并标识于图5中。设机器人步态以DSP起始，单步周期N=16，DSP比率σ=0.25，3种算法的单步ZMP轨迹如图6所示，其中文中算法采用能耗E=231.5 mJ时的步态数据。
在Y轴方向SSP过程中，若以AZR边线yAZR=1.5为基准，设Dy=∑14n=3|yZMP（n）-yAZR|为算法ZMP轨迹与基准线的距离，
Df表示ZMP轨迹点的标准差，文中算法与算法二、算法三的数据对比见表3。可以看出，采用文中算法时，其ZMP轨迹更接近于AZR边线，并减少了侧向摆动幅度，从而降低机器人的行走过程中的能耗。
若取d=90 cm，sm=12 cm，按照式（9）计算得c=10，s1=2 cm， s2=7 cm，则机器人运动的步长S*={2，7，12，12，12，12，12，12，7，2}。在机器人运动的每步均以ηj=5/6从在线数据库中取得
gj=[q1 q2 … q16]，j=1，2，…，10，控制機器人的关节运动，对应的双足机器人步态运动的动态仿真如图7所示。
依照前述仿真数据，实测机器人行走试验如图8所示。试验中，设定行走距离90 cm，在第1步为右脚支撑，设定SSP右脚yAZR=-1.5 cm，初始η1=1/3，在行走中，检测到yMIN=1.53 cm，经过式（11）PI及逻辑运算，得按式（12）取η2=2/3。其后，经过第2，3步的校正后，η值稳定在5/6。若控制中选用η1=1/3的控制方法，行走sm=12 cm时，E=276.3 mJ。而采用文中的GSYN算法，调整η=5/6后，行走12 cm的E=231.5 mJ，其值下降了16.21%。
4 结 论
1）电机驱动的机器人具有质量分散的特点，建立多质心机器人模型可较好的描述机器人物理特性。在机器人多质心模型的基础上，建立了以负荷转矩为主要参数的能耗函数，能在仿真中精确而方便地计算机器人运动的能耗指标。
2）GPO算法中的对运动空间网格化的过程中，要求单个网孔内不出现多个峰值，选择合适的网孔间隙，此方法适用于多数系统的优化。算法的梯度逼近中按照精度要求，确定逐次逼近的步长，在网孔规模较大时，可采用将网孔空间再次网格化的方法加速逼近过程。GPO算法对系统优化的过程中需要进行大量的运算，可在有限的时间内确定找到最优值，随着计算机科学的进步，其对比随机优化算法，将愈加表现出其优越的性能。
3）GSYN算法按照实际机器人行走的要求，规划含起始步、中间步和停止步的完整运动过程。在机器人的行走过程中，算法根据反馈的ZMP数据，修改机器人运动的AZR，从而克服建模和环境误差的干扰，可实现低能耗和高鲁棒性的折中，较好地解决具有高度非线性特征的双足机器人行走问题。因此，该算法也作为一种新途径，推广到煤矿救援机器人的开发中。
参考文献（References）：
[1] WANG W，DONG W，SU Y，et al.Development of sea-rch-and-rescure robots for underground coal mine applications[J].Journal of Field Robotics，2014，31（3）：386-407.[2]FU C，CHEN K.Gait synthesis and sensory control of stair climbing for a humanoid robot[J].IEEE Transactions on Industrial Electronics，2008，55（5）：2111-2120.[3]UGURLU B，KAWAMURA A.On the backward ho-pping problem of legged robots[J].IEEE Transactions on Industrial Electronics，2013，61（3）：1632-1634.[4]田彦涛，孙中波，利宏扬，等.动态双足机器人的控制与优化研究进展[J].自动化学报，2016，42（8）：1142-1157.
TIAN Yantao，SUN Zhongbo，LI Hongyang，et al.A review of optimal and control strategies for dynamic walking bipedal robots[J].Acta Automatica Sinica，2016，42（8）：1142-1157.[5]颜云辉，徐靖，陆志国，等.仿人服务机器人发展与研究现状[J].机器人，2017，39（4）：551-564.YAN Yunhui，XU Jing，LU Zhiguo，et al.Development and research status of humanoid service robots[J].Robot，2017，39（4）：551-564.[6]PAOLILLO A，CHERUBINI A，KEITH F，et al.Toward autonomous car driving by a humanoid robot：A sensor-based framework[C]//2014 IEEE/RAS International Conference on Humanoid Robots.Madrid，Spain：IEEE，2014：451-456.[7]KOOLEN T，BERTRAND S，THOMAS G，et al.Design of a momentum-based control framework and application to the humanoid robot Atlas[J].International Journal of Humanoid Robotics，2016，13（1）：1650007.
[8]BOUTIN L，EON A，ZEGHLOUL S，et al.From human motion capture to humanoid locomotion imitation application to the robots HRP-2 and HOAP-3[J].Robotica，2011，29（2）：325-334. [9]吴伟国，栗华，高力扬.人体步行捕捉下的双足机器人跟随步行与实验[J].哈尔滨工业大学学报，2017，49（1）：21-29.WU Weiguo，LI Hua，GAO Liyang.Follow-walking motions of a biped robot and its experimental research based on human walking capturing[J].Journal of Harbin Institiute of Technology，2017，49（1）：21-29.[10]LIU C，WANG D，GOODMAN E D，et al.Adaptive w-alking control of biped robots using online trajectory generation method based on neural oscillators[J].Journal of Bionic Engineering，2016，13（4）：572-584.[11]JUANG C F，YEH Y T.Multiobjective evolution of biped robot gaits using advanced continuous ant-colony optimized recurrent neural networks[J].IEEE Transactions on Cybernetics，2018，48（6）：1910-1922.[12]HONG S，OH Y，KIM D，et al.Real-time walking pattern generation method for humanoid robots by combining feedback and feedforward controller[J].IEEE Transactions on Industrial Electronics，2014，61（1）：355-364.[13]SHAFII N，ABDOLMALEKI A，LAU N，et al.Development of an omnidirectional walk engine for soccer humanoid robots[J].International Journal of Advanced Robotic Systems，2015，12（12）：193.
[14]SHIMMYO S，SATO T，OHNISHI K.Biped walking pattern generation by using preview control based on three-mass model[J].IEEE Transactions on Industrial Electronics，2013，60（11）：5137-5147.[15]LUO R C，CHENC C.Biped walking trajectory generator based on three-mass with angular momentum model using model predictive control[J].IEEE Transactions on Industrial Electronics，2016，63（1）：268-276.[16]SHIN H K，KIM B K.Energy-efficient gait planning and control for biped robots utilizing vertical body motion and allowable ZMP region[J].IEEE Transactions on Industrial Electronics，2015，62（4）：2277-2286.[17]WRIGHT J，JORDANOV I.Intelligent approaches in locomotion：a review[J].Journal of Intelligent & Robotic Systems，2015，80（2）：255-277.
[18]BERTRAM J E A.Constrained optimization in human walking：cost minimization and gait plasticity[J].Journal of Experimental Biology，2005，208（6）：979-991.[19]SELLAOUTI R，STASSE O，KAJITA S，et al.Faster and smoother walking of humanoid HRP-2 with passive toe joints[C]//2006 IEEE/RSJ International Conference on Intelligent Robots and Systems.Beijing，China：IEEE，2006：4409-4414.[20]VUKOBRATOVIC M，BOROVAC B.Zero-moment po-int-thirty five years of its life[J].International Journal of Humanoid Robotics，2004，01（1）：157-173.[21]KAJITA S，KANEHIRO F，KANEKO K，et al.The 3D linear inverted pendulum model：A simple modeling for a biped walking pattern generation[C]//Proceedings 2001 IEEE/RSJ International Conference on Intelligent Robots and Systems.Maui，HI，USA：IEEE，2001，1（1）：239-246.[22]高志偉，代学武，郑志达.基于运动控制和频域分析的移动机器人能耗最优轨迹规划[J].自动化学报，2020，46（5）：934-945.
GAO Zhiwei，DAI Xuewu，ZHENG Zhida.Optiaml energy consumption trajectory planning for mobile robot based on motion control and frequency domain analysis[J].ACTA Automatica Sinica，2020，46（5）：934-945.[23]SRINIVASAN M，RUINA A.Computer optimization of a minimal biped model discovers walking and running[J].Nature，2006，439（5）：72-75.
[24]HERZOG A，ROTELLA N，MASON S，et al.Momentum control with hierarchical inverse dynamics on a torque-controlled humanoid[J].Autonomous Robots，2016，40（3）：473-491.[25]LEE B J，STONIER D，KIM Y D，et al.Modifiable walking pattern of a humanoid robot by using allowable ZMP variation[J].IEEE Transactions on Robotics，2008，24（4）：917-925.