应用扩展零力矩点预观控制和分解动量控制的仿人机器人全身运动规划方法

周雪峰,孙广彬,刘晓光,黄丹,蒋晓明,朱富贵

(1.广东省科学院广东省智能制造研究所,510070,广州;2.南洋理工大学机器人研究中心,639798,新加坡)



应用扩展零力矩点预观控制和分解动量控制的仿人机器人全身运动规划方法

周雪峰1,孙广彬2,刘晓光1,黄丹1,蒋晓明1,朱富贵1

(1.广东省科学院广东省智能制造研究所,510070,广州;2.南洋理工大学机器人研究中心,639798,新加坡)

针对简化动力学模型对运动规划造成较大误差的问题,提出一种仿人机器人全身运动生成方法,它结合扩展零力矩点(extended zero moment point,EZMP)的预观控制和分解动量控制,实现了含多种约束的高效运动规划。该方法给出了非线性EZMP方程在三维空间中的封闭形式,使得机器人稳定步态生成更加方便,同时将之前文献中忽略的角动量等因素也考虑在内,另外可以对EZMP、四肢运动和角动量等自由添加约束。仿真结果表明,该方法将EZMP平均误差减小到单一预观控制方法的2/5,并可以方便地实现含复杂约束的运动规划,为仿人机器人提供了一种形式简单且灵活、高效的运动规划方法。

仿人机器人;扩展零力矩点;全身运动规划;角动量

仿人机器人的一个基本问题是如何在保持动态平衡的同时完成其他实际的任务,很多文献对足式行走问题进行了研究[1-3]。为了处理零力矩点(ZMP)无法用于非水平地面的缺点,Sun等提出了扩展零力矩点(extended zero moment point,EZMP)[4]的概念,该方法将质心和角动量进行解耦,得到了一个三维线性模型,并在3个运动方向上同时应用预观控制[5]生成稳定的运动步态,而不需要单独规划竖直方向运动,但是该方法仍然存在一些缺点:忽略了绕质心的转动动力学以及难以施加基于任务的约束,使得其很难用于执行除行走之外的实际任务。Park等将水平角动量的二阶微分和加速度变化率作为系统输入,可以同时控制角动量和ZMP[6],但并没有考虑竖直方向运动规划,而且文献并未说明如何实现目标角动量。另外,还有文献使用线性模型预测控制(linear model predictive control,LMPC)来生成参考运动[7-8],这些方法可以很方便地添加各种约束,但是基本上用于比较简单的模型。Feng等应用二次规划(quadratic programming,QP)方法和微分动态规划(differential dynamic programming,DDP)生成三维运动轨迹[9],但基于非线性最优化方法的求解比较耗时间,且最优性难以保证。Kajita等提出了分解动量控制(resolved momentum control,RMC)方法,该方法给定双足及双手的轨迹,可以计算实现目标角动量的上身运动轨迹[10]。本文主要基于Sun等关于EZMP的工作以及Kajita等的分解动量控制,给出了EZMP的非线性表达式,并结合预观控制和分解动量控制来生成稳定的运动模式。该方法同时生成了质心三维运动轨迹并可用于任意路面;考虑了机器人的完全动力学;可以添加角动量以及四肢的运动轨迹等基于任务的约束。

1 问题描述

本节主要给出了EZMP的完整表达式并指出了基于线性模型的运动规划的弊端。

1.1 EZMP的非线性方程

Sun等提出EZMP的概念,将经典ZMP推广到任意路面[4],但没有给出EZMP的完整表达式,本文在此给出了EZMP的非线性表达式。EZMP定义为虚拟接触平面(virtual contact plane,VCP)上一点,在该点处,地面反作用力矩垂直于VCP,如图1所示。

图1 EZMP示意图

令n=(nx,ny,nz)T和τp=(τpx,τpy,τpz)T分别为VCP平面的法矢量以及在EZMP点p处的地面反作用力矩。O表示以O为原点的世界坐标系,根据EZMP定义,易得n和τp在同一直线上

(1)

相应动力学方程表示为

(2)

式中:L=(Lx,Ly,Lz)T为相对于世界坐标系原点O的角动量,P=(Px,Py,Pz)T为机器人的动量;pc=(x,y,z)T为质心坐标;p为EZMP坐标;g为重力加速度(下文中g,其值为g)。

令Lc=(Lcx,Lcy,Lcz)T为绕质心pc的角动量。绕质心pc和原点O的角动量微分的关系可表示为

(3)

将式(3)代入式(2),再将式(2)代入式(1),得

=0

(4)

式中

(5)

通过求解方程式(4),可求得水平分量px和py

(6a)

(6b)

式中

(7a)

(7b)

(7c)

1.2 EZMP方程的线性化及预观控制

(8a)

(8b)

(8c)

该模型在各方向具有对称的结构。假设c为常数,则可应用线性控制器来生成三维运动轨迹。

1.2.2 预观控制 以x方向为例,方程式(8)经状态空间化和离散化后可得

(9)

式中

其中T为采样时间。

最优控制的目标函数为

(10)

式中:Qe、Qx和R为非负定权值矩阵。Δx(i)=x(i)-x(i-1)为状态变量增量,Δu(i)=u(i)-u(i-1)为控制输入增量。

根据预观控制理论,可得最优控制如下[6,10]

(11)

1.2.3 基于线性模型及预观控制进行运动规划的缺点 尽管上述方法可以高效地生成运动质心轨迹,但是机器人的真实角动量变化率并不是我们假设的恒定值,在执行高速或复杂运动时,采用恒定的角动量变化率会导致较大EZMP误差,影响机器人动态平衡。另外,预观控制是基于线性二次控制理论设计的,而且难于施加基于任务的约束条件,由此限制了机器人执行任务的能力。

2 基于EZMP预观控制和分解动量控制的全身运动生成

针对线性模型的预观控制方法存在的缺点,结合预观控制和分解动量控制生成了满足目标ZMP及角动量的全身运动模式,该方法可以保证动态平衡且同时满足四肢运动约束,便于执行稳定行走之外的其他有用任务。所提方案主要包括基于预观控制的质心运动生成器和基于分解动量的控制器。预观控制用于EZMP的线性化模型,生成质心的速度轨迹;分解动量控制根据质心规划器生成的线性运动来计算满足运动约束的基座位姿轨迹,并由雅可比矩阵映射到机器人关节空间。设计时,本文将目标角动量(绕质心)设为零,并且模仿人在站立、行走、奔跑等自然运动时对角动量的近零调节。

2.1 应用分解动量控制

在EZMP的非线性方程中,质心和角动量的运动是互相耦合的。规划关节角度轨迹时,机器人的质心运动和绕质心的旋转运动需要同时考虑。相对基于线性模型的运动生成方法,考虑了机器人EZMP的完整动力学方程生成的运动轨迹具有更小的目标跟踪误差。

分解角动量控制是将机器人的动量/角动量以及关节空间运动结合在一起[10],可以生成满足目标动量或角动量的全身运动,且考虑了环境或任务对机器人足或手的约束,控制比较灵活。下面将使用该方法来实现目标动量和角动量的控制。动量P和绕质心角动量Lc可以用机器人基座和关节空间速度表示为

(12)

(13)

式中:vB为机器人基座的速度;ωB为空间角速度。

给定基座空间速度ξB和肢体末端速度ξi,可以通过雅可比矩阵得出关节空间角速度为

(14)

假设目标角动量和各肢体末端速度已经给定,目标动量已由预观控制计算得出,将式(14)代入式(12),基座速度ξB可以通过分解动量控制得出

(15)

式中

(16)

上标“†”表示伪逆操作符。

2.2 设定动量和角动量的参考轨迹

设绕质心角动量为零,此时EZMP方程式为式(8)。给定EZMP参考值,质心轨迹由预观控制计算得出,故可以计算机器人动量和角动量轨迹如下

(17)

2.3 全身运动规划方法

运动规划方法包含线性动量生成和全身运动生产两个部分。在线性动量生成部分,应用预观控制来生成满足目标EZMP的质心运动并计算对应的(线性)动量。在全身运动生成部分,角动量设为零,应用分解动量控制来实现目标角动量以及(由目标EZMP得到的)目标动量,并通过雅可比矩阵将机器人的集总参数形式的运动映射到每个关节空间。给定机器人的空间行走参数(步长、步宽、步高、摆腿高度,步高此处定义为落脚点高度变化)以及时间行走参数(步行周期、双足支撑期或摆腿期的占时比);落脚点位置和时间可以由相应计算得到;目标EZMP通过落脚点和步行参数进行插值计算得到;质心运动以及(线)动量由预观控制生成;给定角动量和肢体末端运动目标轨迹,通过分解动量控制计算基座运动轨迹,并由雅可比矩阵将其映射到关节空间。这样,所生成的运动同时满足动态平衡和其他运动约束。

3 仿真实验和结果分析

所提出的方法通过在Webots软件中的Atlas机器人模型进行仿真验证,如图2所示。控制器通过Matlab进行设计,通过软件之间的接口传输控制指令。脚底的压力传感器以及各关节的力矩和位置传感器用来感知脚底力的分布(每个足底4个传感器位于四角位置),进而可计算EZMP测量值[4]。运动学和惯性参数按照机器人的真实参数来配置[11]。

图2 Atlas机器人模型

图3 目标EZMP轨迹

3.1 仿真实验准备

将式(16)写成下式

(18)

所有算法及控制器设计通过Matlab编程实现,硬件配置为:Intel core i7@2.67 GHz,8 GB内存。

3.2 仿真结果

图4 预观控制产生的残余角动量及对EZMP的影响

设置一个对照组来验证本文方法的有效性,即忽略角动量(预观控制)和考虑角动量(预观结合分解动量控制)两种情况。预观控制基于线性化模型生成质心轨迹,由于忽略了角动量信息,所以会造成一定的EZMP误差。从图4a可以看出,预观控制所导致的残余(绕质心)角动量是很大的,所以不应该忽略这一部分,尤其是在执行快速运动时,这一点通过图4b中对EZMP的预测结果进一步得到了证实,即图4b中角动量的缺失导致线性模型和非线性(精确)模型预测产生较大误差。

从图5a可以看出,(绕质心)角动量调控在很小的幅度以内,这时预观控制所使用的线性模型式(8)等价于非线性模型式(6)。考虑到分解动量控制使用的是完整的动力学模型,在这种情况下,本文提出的框架是对机器人动力学行为的精确描述。

(a)动量 (b)角动量图5 分解动量控制生成的动量和角动量

(a)预观控制 (b)预观结合分解动量控制图6 预观控制和预观结合分解动量控制的EZMP轨迹跟踪

从图6可以看出,通过引入分解动量控制,并将目标角动量设为零,EZMP的跟踪误差(见图6b)相比单纯线性方法(见图6a)有了较大的改善,平均误差减小至原来的2/5左右(约0.18 cm)。

图7展示了机器人可以很稳定地实现爬楼梯任务,同时上身像人类一样自发产生摆动运动(调控绕质心角动量)。

图7 Atlas机器人爬楼梯运动仿真

4 结 论

本文引入分解动量控制,对基于EZMP的运动规划方法进行了改进,它可以同时规划质心轨迹和绕质心的角动量轨迹,并实现全身运动模式规划,该方法可以很灵活地处理角动量及其他基于任务的运动约束。通过仿真展示了机器人爬楼梯过程,机器人可以调节全身运动以实现目标(零)角动量。在下一步工作中,我们将尝试利用身体的冗余自由度补偿外界扰动。

[1] NISHIWAKI K, CHESTNUTT J, KAGAMI S. Planning and control of a humanoid robot for navigation on uneven multi-scale terrain [J]. Experimental Robotics, 2014, 79: 401-415.

[2] 刘冠初, 熊静琪, 乔林, 等. 四足机器人自由步态规划建模与算法实现 [J]. 西安交通大学学报, 2015, 49(6): 84-89. LIU Guanchu, XIONG Jingqi, QIAO Lin, et al. Modelling and algorithm realization of free gait regulation for quadruped robots [J]. Journal of Xi’an Jiaotong University, 2015, 49(6): 84-89.

[3] MIURA K, KANEHIRO F, KANEKO K, et al. Quick slip-turn of HRP-4C on its toes [C]∥Proceedings of the IEEE International Conference on Robotics and Automation. Piscataway, NJ, USA: IEEE, 2012: 3527-3528.

[4] SUN G, WANG H, LU Z. A novel biped pattern generator based on extended ZMP and extended cart-table model [J]. International Journal of Advanced Robotic Systems, 2015, 12: 94.

[5] KAJITA S, KANEHIRO F, KANEKO K, et al. Biped walking pattern generation by using preview control of zero-moment point [C]∥Proceedings of the IEEE International Conference on Robotics and Automation. Piscataway, NJ, USA: IEEE, 2003: 1620-1626.

[6] PARK J, YOUM Y. General ZMP preview control for bipedal walking [C]∥Proceedings of the IEEE International Conference on Robotics and Automation. Piscataway, NJ, USA: IEEE, 2007: 2682-2687.

[7] HERDT A, DIEDAM H, WIEBER P, et al. Online walking motion generation with automatic footstep placement [J]. Advanced Robotics, 2010, 24(5/6): 719-737.

[8] DIMITROV D, WIEBER P, STASSE O, et al. An optimized linear model predictive control solver for online walking motion generation [C]∥Proceedings of the IEEE International Conference on Robotics and Automation. Piscataway, NJ, USA: IEEE, 2009: 1171-1176.

[9] FENG S, XINJILEFU X, HUANG W, et al. 3D walking based on online optimization [C]∥Proceedings of the IEEE-RAS International Conference on Humanoid Robots. Piscataway, NJ, USA: IEEE, 2013: 1-7.

[10]KAJITA S, KANEHIRO F, KANEKO K, et al. Resolved momentum control: humanoid motion planning based on the linear and angular momentum [C]∥Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway, NJ, USA: IEEE, 2003: 1644-1650.

[11]HSU J. Bitbucket repository for DRCSim [EB/OL]. (2013-01-18)[2016-04-01]. https: ∥bitbucket.org/osrf/drcsim/src/c69ecab26a55/ros/atlas_description/urdf/.

(编辑 武红江)

A Whole-Body Motion Planner for Humanoid Robots Using EZMP Preview Control and Resolved Momentum Control

ZHOU Xuefeng1,SUN Guangbin2,LIU Xiaoguang1,HUANG Dan1,JIANG Xiaoming1,ZHU Fugui1

(1.Guangdong Institute of Intelligent Manufacturing, Guangdong Academy of Sciences, Guangzhou 510070, China;2. Robotics Research Centre, Nanyang Technological University, 639798, Singapore)

A whole-body motion planning method for humanoid robots is proposed to tackle motion planning errors caused by simplified dynamic model.The method combines the EZMP (extended zero moment point) control and the resolved momentum control to realize high efficiency motion planning with multiple constraints. A close form of the nonlinear EZMP equation in 3D space is presented to facilitate the stable motion pattern generation of humanoid robots. The angular momentum neglected in most previous literatures is also taken into account. Furthermore, some constraints such as EZMP, limb motion and angular momentum can be added. Simulation results show that the proposed method reduces the average EZMP error to around 2/5 of that using preview control alone and is able to realize motion planning with complex constraints. Therefore, the method provides a compact-formed, flexible and efficient motion planning solution for humanoid robots.

humanoid robot; extended zero moment point; whole-body motion planning; angular momentum

2016-05-01。 作者简介:周雪峰(1982—),男,副研究员;孙广彬(通信作者),男,博士。 基金项目:国家自然科学基金资助项目(51405091);广州市珠江科技新星专项资助项目(201504300920029)。

时间:2016-10-19

10.7652/xjtuxb201612010

TP242

A

0253-987X(2016)12-0058-06

网络出版地址:http: ∥www.cnki.net/kcms/detail/61.1069.T.20161019.1622.008.html