周雪峰,孫廣彬,劉曉光,黃丹,蔣曉明,朱富貴

(1.廣東省科學(xué)院廣東省智能制造研究所,510070,廣州;2.南洋理工大學(xué)機(jī)器人研究中心,639798,新加坡)

?

應(yīng)用擴(kuò)展零力矩點(diǎn)預(yù)觀控制和分解動(dòng)量控制的仿人機(jī)器人全身運(yùn)動(dòng)規(guī)劃方法

周雪峰1,孫廣彬2,劉曉光1,黃丹1,蔣曉明1,朱富貴1

(1.廣東省科學(xué)院廣東省智能制造研究所,510070,廣州;2.南洋理工大學(xué)機(jī)器人研究中心,639798,新加坡)

針對簡化動(dòng)力學(xué)模型對運(yùn)動(dòng)規(guī)劃造成較大誤差的問題,提出一種仿人機(jī)器人全身運(yùn)動(dòng)生成方法,它結(jié)合擴(kuò)展零力矩點(diǎn)(extended zero moment point,EZMP)的預(yù)觀控制和分解動(dòng)量控制,實(shí)現(xiàn)了含多種約束的高效運(yùn)動(dòng)規(guī)劃。該方法給出了非線性EZMP方程在三維空間中的封閉形式,使得機(jī)器人穩(wěn)定步態(tài)生成更加方便,同時(shí)將之前文獻(xiàn)中忽略的角動(dòng)量等因素也考慮在內(nèi),另外可以對EZMP、四肢運(yùn)動(dòng)和角動(dòng)量等自由添加約束。仿真結(jié)果表明,該方法將EZMP平均誤差減小到單一預(yù)觀控制方法的2/5,并可以方便地實(shí)現(xiàn)含復(fù)雜約束的運(yùn)動(dòng)規(guī)劃,為仿人機(jī)器人提供了一種形式簡單且靈活、高效的運(yùn)動(dòng)規(guī)劃方法。

仿人機(jī)器人;擴(kuò)展零力矩點(diǎn);全身運(yùn)動(dòng)規(guī)劃;角動(dòng)量

仿人機(jī)器人的一個(gè)基本問題是如何在保持動(dòng)態(tài)平衡的同時(shí)完成其他實(shí)際的任務(wù),很多文獻(xiàn)對足式行走問題進(jìn)行了研究[1-3]。為了處理零力矩點(diǎn)(ZMP)無法用于非水平地面的缺點(diǎn),Sun等提出了擴(kuò)展零力矩點(diǎn)(extended zero moment point,EZMP)[4]的概念,該方法將質(zhì)心和角動(dòng)量進(jìn)行解耦,得到了一個(gè)三維線性模型,并在3個(gè)運(yùn)動(dòng)方向上同時(shí)應(yīng)用預(yù)觀控制[5]生成穩(wěn)定的運(yùn)動(dòng)步態(tài),而不需要單獨(dú)規(guī)劃豎直方向運(yùn)動(dòng),但是該方法仍然存在一些缺點(diǎn):忽略了繞質(zhì)心的轉(zhuǎn)動(dòng)動(dòng)力學(xué)以及難以施加基于任務(wù)的約束,使得其很難用于執(zhí)行除行走之外的實(shí)際任務(wù)。Park等將水平角動(dòng)量的二階微分和加速度變化率作為系統(tǒng)輸入,可以同時(shí)控制角動(dòng)量和ZMP[6],但并沒有考慮豎直方向運(yùn)動(dòng)規(guī)劃,而且文獻(xiàn)并未說明如何實(shí)現(xiàn)目標(biāo)角動(dòng)量。另外,還有文獻(xiàn)使用線性模型預(yù)測控制(linear model predictive control,LMPC)來生成參考運(yùn)動(dòng)[7-8],這些方法可以很方便地添加各種約束,但是基本上用于比較簡單的模型。Feng等應(yīng)用二次規(guī)劃(quadratic programming,QP)方法和微分動(dòng)態(tài)規(guī)劃(differential dynamic programming,DDP)生成三維運(yùn)動(dòng)軌跡[9],但基于非線性最優(yōu)化方法的求解比較耗時(shí)間,且最優(yōu)性難以保證。Kajita等提出了分解動(dòng)量控制(resolved momentum control,RMC)方法,該方法給定雙足及雙手的軌跡,可以計(jì)算實(shí)現(xiàn)目標(biāo)角動(dòng)量的上身運(yùn)動(dòng)軌跡[10]。本文主要基于Sun等關(guān)于EZMP的工作以及Kajita等的分解動(dòng)量控制,給出了EZMP的非線性表達(dá)式,并結(jié)合預(yù)觀控制和分解動(dòng)量控制來生成穩(wěn)定的運(yùn)動(dòng)模式。該方法同時(shí)生成了質(zhì)心三維運(yùn)動(dòng)軌跡并可用于任意路面;考慮了機(jī)器人的完全動(dòng)力學(xué);可以添加角動(dòng)量以及四肢的運(yùn)動(dòng)軌跡等基于任務(wù)的約束。

1 問題描述

本節(jié)主要給出了EZMP的完整表達(dá)式并指出了基于線性模型的運(yùn)動(dòng)規(guī)劃的弊端。

1.1 EZMP的非線性方程

Sun等提出EZMP的概念,將經(jīng)典ZMP推廣到任意路面[4],但沒有給出EZMP的完整表達(dá)式,本文在此給出了EZMP的非線性表達(dá)式。EZMP定義為虛擬接觸平面(virtual contact plane,VCP)上一點(diǎn),在該點(diǎn)處,地面反作用力矩垂直于VCP,如圖1所示。

圖1 EZMP示意圖

令n=(nx,ny,nz)T和τp=(τpx,τpy,τpz)T分別為VCP平面的法矢量以及在EZMP點(diǎn)p處的地面反作用力矩。O表示以O(shè)為原點(diǎn)的世界坐標(biāo)系,根據(jù)EZMP定義,易得n和τp在同一直線上

(1)

相應(yīng)動(dòng)力學(xué)方程表示為

(2)

式中:L=(Lx,Ly,Lz)T為相對于世界坐標(biāo)系原點(diǎn)O的角動(dòng)量,P=(Px,Py,Pz)T為機(jī)器人的動(dòng)量;pc=(x,y,z)T為質(zhì)心坐標(biāo);p為EZMP坐標(biāo);g為重力加速度(下文中g(shù),其值為g)。

令Lc=(Lcx,Lcy,Lcz)T為繞質(zhì)心pc的角動(dòng)量。繞質(zhì)心pc和原點(diǎn)O的角動(dòng)量微分的關(guān)系可表示為

(3)

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

=0

(4)

式中

(5)

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

(6a)

(6b)

式中

(7a)

(7b)

(7c)

1.2 EZMP方程的線性化及預(yù)觀控制

(8a)

(8b)

(8c)

該模型在各方向具有對稱的結(jié)構(gòu)。假設(shè)c為常數(shù),則可應(yīng)用線性控制器來生成三維運(yùn)動(dòng)軌跡。

1.2.2 預(yù)觀控制 以x方向?yàn)槔?方程式(8)經(jīng)狀態(tài)空間化和離散化后可得

(9)

式中

其中T為采樣時(shí)間。

最優(yōu)控制的目標(biāo)函數(shù)為

(10)

式中:Qe、Qx和R為非負(fù)定權(quán)值矩陣。Δx(i)=x(i)-x(i-1)為狀態(tài)變量增量,Δu(i)=u(i)-u(i-1)為控制輸入增量。

根據(jù)預(yù)觀控制理論,可得最優(yōu)控制如下[6,10]

(11)

1.2.3 基于線性模型及預(yù)觀控制進(jìn)行運(yùn)動(dòng)規(guī)劃的缺點(diǎn) 盡管上述方法可以高效地生成運(yùn)動(dòng)質(zhì)心軌跡,但是機(jī)器人的真實(shí)角動(dòng)量變化率并不是我們假設(shè)的恒定值,在執(zhí)行高速或復(fù)雜運(yùn)動(dòng)時(shí),采用恒定的角動(dòng)量變化率會(huì)導(dǎo)致較大EZMP誤差,影響機(jī)器人動(dòng)態(tài)平衡。另外,預(yù)觀控制是基于線性二次控制理論設(shè)計(jì)的,而且難于施加基于任務(wù)的約束條件,由此限制了機(jī)器人執(zhí)行任務(wù)的能力。

2 基于EZMP預(yù)觀控制和分解動(dòng)量控制的全身運(yùn)動(dòng)生成

針對線性模型的預(yù)觀控制方法存在的缺點(diǎn),結(jié)合預(yù)觀控制和分解動(dòng)量控制生成了滿足目標(biāo)ZMP及角動(dòng)量的全身運(yùn)動(dòng)模式,該方法可以保證動(dòng)態(tài)平衡且同時(shí)滿足四肢運(yùn)動(dòng)約束,便于執(zhí)行穩(wěn)定行走之外的其他有用任務(wù)。所提方案主要包括基于預(yù)觀控制的質(zhì)心運(yùn)動(dòng)生成器和基于分解動(dòng)量的控制器。預(yù)觀控制用于EZMP的線性化模型,生成質(zhì)心的速度軌跡;分解動(dòng)量控制根據(jù)質(zhì)心規(guī)劃器生成的線性運(yùn)動(dòng)來計(jì)算滿足運(yùn)動(dòng)約束的基座位姿軌跡,并由雅可比矩陣映射到機(jī)器人關(guān)節(jié)空間。設(shè)計(jì)時(shí),本文將目標(biāo)角動(dòng)量(繞質(zhì)心)設(shè)為零,并且模仿人在站立、行走、奔跑等自然運(yùn)動(dòng)時(shí)對角動(dòng)量的近零調(diào)節(jié)。

2.1 應(yīng)用分解動(dòng)量控制

在EZMP的非線性方程中,質(zhì)心和角動(dòng)量的運(yùn)動(dòng)是互相耦合的。規(guī)劃關(guān)節(jié)角度軌跡時(shí),機(jī)器人的質(zhì)心運(yùn)動(dòng)和繞質(zhì)心的旋轉(zhuǎn)運(yùn)動(dòng)需要同時(shí)考慮。相對基于線性模型的運(yùn)動(dòng)生成方法,考慮了機(jī)器人EZMP的完整動(dòng)力學(xué)方程生成的運(yùn)動(dòng)軌跡具有更小的目標(biāo)跟蹤誤差。

分解角動(dòng)量控制是將機(jī)器人的動(dòng)量/角動(dòng)量以及關(guān)節(jié)空間運(yùn)動(dòng)結(jié)合在一起[10],可以生成滿足目標(biāo)動(dòng)量或角動(dòng)量的全身運(yùn)動(dòng),且考慮了環(huán)境或任務(wù)對機(jī)器人足或手的約束,控制比較靈活。下面將使用該方法來實(shí)現(xiàn)目標(biāo)動(dòng)量和角動(dòng)量的控制。動(dòng)量P和繞質(zhì)心角動(dòng)量Lc可以用機(jī)器人基座和關(guān)節(jié)空間速度表示為

(12)

(13)

式中:vB為機(jī)器人基座的速度;ωB為空間角速度。

給定基座空間速度ξB和肢體末端速度ξi,可以通過雅可比矩陣得出關(guān)節(jié)空間角速度為

(14)

假設(shè)目標(biāo)角動(dòng)量和各肢體末端速度已經(jīng)給定,目標(biāo)動(dòng)量已由預(yù)觀控制計(jì)算得出,將式(14)代入式(12),基座速度ξB可以通過分解動(dòng)量控制得出

(15)

式中

(16)

上標(biāo)“?”表示偽逆操作符。

2.2 設(shè)定動(dòng)量和角動(dòng)量的參考軌跡

設(shè)繞質(zhì)心角動(dòng)量為零,此時(shí)EZMP方程式為式(8)。給定EZMP參考值,質(zhì)心軌跡由預(yù)觀控制計(jì)算得出,故可以計(jì)算機(jī)器人動(dòng)量和角動(dòng)量軌跡如下

(17)

2.3 全身運(yùn)動(dòng)規(guī)劃方法

運(yùn)動(dòng)規(guī)劃方法包含線性動(dòng)量生成和全身運(yùn)動(dòng)生產(chǎn)兩個(gè)部分。在線性動(dòng)量生成部分,應(yīng)用預(yù)觀控制來生成滿足目標(biāo)EZMP的質(zhì)心運(yùn)動(dòng)并計(jì)算對應(yīng)的(線性)動(dòng)量。在全身運(yùn)動(dòng)生成部分,角動(dòng)量設(shè)為零,應(yīng)用分解動(dòng)量控制來實(shí)現(xiàn)目標(biāo)角動(dòng)量以及(由目標(biāo)EZMP得到的)目標(biāo)動(dòng)量,并通過雅可比矩陣將機(jī)器人的集總參數(shù)形式的運(yùn)動(dòng)映射到每個(gè)關(guān)節(jié)空間。給定機(jī)器人的空間行走參數(shù)(步長、步寬、步高、擺腿高度,步高此處定義為落腳點(diǎn)高度變化)以及時(shí)間行走參數(shù)(步行周期、雙足支撐期或擺腿期的占時(shí)比);落腳點(diǎn)位置和時(shí)間可以由相應(yīng)計(jì)算得到;目標(biāo)EZMP通過落腳點(diǎn)和步行參數(shù)進(jìn)行插值計(jì)算得到;質(zhì)心運(yùn)動(dòng)以及(線)動(dòng)量由預(yù)觀控制生成;給定角動(dòng)量和肢體末端運(yùn)動(dòng)目標(biāo)軌跡,通過分解動(dòng)量控制計(jì)算基座運(yùn)動(dòng)軌跡,并由雅可比矩陣將其映射到關(guān)節(jié)空間。這樣,所生成的運(yùn)動(dòng)同時(shí)滿足動(dòng)態(tài)平衡和其他運(yùn)動(dòng)約束。

3 仿真實(shí)驗(yàn)和結(jié)果分析

所提出的方法通過在Webots軟件中的Atlas機(jī)器人模型進(jìn)行仿真驗(yàn)證,如圖2所示?？刂破魍ㄟ^Matlab進(jìn)行設(shè)計(jì),通過軟件之間的接口傳輸控制指令。腳底的壓力傳感器以及各關(guān)節(jié)的力矩和位置傳感器用來感知腳底力的分布(每個(gè)足底4個(gè)傳感器位于四角位置),進(jìn)而可計(jì)算EZMP測量值[4]。運(yùn)動(dòng)學(xué)和慣性參數(shù)按照機(jī)器人的真實(shí)參數(shù)來配置[11]。

圖2 Atlas機(jī)器人模型

圖3 目標(biāo)EZMP軌跡

3.1 仿真實(shí)驗(yàn)準(zhǔn)備

將式(16)寫成下式

(18)

所有算法及控制器設(shè)計(jì)通過Matlab編程實(shí)現(xiàn),硬件配置為:Intel core i7@2.67 GHz,8 GB內(nèi)存。

3.2 仿真結(jié)果

圖4 預(yù)觀控制產(chǎn)生的殘余角動(dòng)量及對EZMP的影響

設(shè)置一個(gè)對照組來驗(yàn)證本文方法的有效性,即忽略角動(dòng)量(預(yù)觀控制)和考慮角動(dòng)量(預(yù)觀結(jié)合分解動(dòng)量控制)兩種情況。預(yù)觀控制基于線性化模型生成質(zhì)心軌跡,由于忽略了角動(dòng)量信息,所以會(huì)造成一定的EZMP誤差。從圖4a可以看出,預(yù)觀控制所導(dǎo)致的殘余(繞質(zhì)心)角動(dòng)量是很大的,所以不應(yīng)該忽略這一部分,尤其是在執(zhí)行快速運(yùn)動(dòng)時(shí),這一點(diǎn)通過圖4b中對EZMP的預(yù)測結(jié)果進(jìn)一步得到了證實(shí),即圖4b中角動(dòng)量的缺失導(dǎo)致線性模型和非線性(精確)模型預(yù)測產(chǎn)生較大誤差。

從圖5a可以看出,(繞質(zhì)心)角動(dòng)量調(diào)控在很小的幅度以內(nèi),這時(shí)預(yù)觀控制所使用的線性模型式(8)等價(jià)于非線性模型式(6)?？紤]到分解動(dòng)量控制使用的是完整的動(dòng)力學(xué)模型,在這種情況下,本文提出的框架是對機(jī)器人動(dòng)力學(xué)行為的精確描述。

(a)動(dòng)量 (b)角動(dòng)量圖5 分解動(dòng)量控制生成的動(dòng)量和角動(dòng)量

(a)預(yù)觀控制 (b)預(yù)觀結(jié)合分解動(dòng)量控制圖6 預(yù)觀控制和預(yù)觀結(jié)合分解動(dòng)量控制的EZMP軌跡跟蹤

從圖6可以看出,通過引入分解動(dòng)量控制,并將目標(biāo)角動(dòng)量設(shè)為零,EZMP的跟蹤誤差(見圖6b)相比單純線性方法(見圖6a)有了較大的改善,平均誤差減小至原來的2/5左右(約0.18 cm)。

圖7展示了機(jī)器人可以很穩(wěn)定地實(shí)現(xiàn)爬樓梯任務(wù),同時(shí)上身像人類一樣自發(fā)產(chǎn)生擺動(dòng)運(yùn)動(dòng)(調(diào)控繞質(zhì)心角動(dòng)量)。

圖7 Atlas機(jī)器人爬樓梯運(yùn)動(dòng)仿真

4 結(jié) 論

本文引入分解動(dòng)量控制,對基于EZMP的運(yùn)動(dòng)規(guī)劃方法進(jìn)行了改進(jìn),它可以同時(shí)規(guī)劃質(zhì)心軌跡和繞質(zhì)心的角動(dòng)量軌跡,并實(shí)現(xiàn)全身運(yùn)動(dòng)模式規(guī)劃,該方法可以很靈活地處理角動(dòng)量及其他基于任務(wù)的運(yùn)動(dòng)約束。通過仿真展示了機(jī)器人爬樓梯過程,機(jī)器人可以調(diào)節(jié)全身運(yùn)動(dòng)以實(shí)現(xiàn)目標(biāo)(零)角動(dòng)量。在下一步工作中,我們將嘗試?yán)蒙眢w的冗余自由度補(bǔ)償外界擾動(dòng)。

[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ī)器人自由步態(tài)規(guī)劃建模與算法實(shí)現(xiàn) [J]. 西安交通大學(xué)學(xué)報(bào), 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—),男,副研究員;孫廣彬(通信作者),男,博士。 基金項(xiàng)目:國家自然科學(xué)基金資助項(xiàng)目(51405091);廣州市珠江科技新星專項(xiàng)資助項(xiàng)目(201504300920029)。

時(shí)間:2016-10-19

10.7652/xjtuxb201612010

TP242

A

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

網(wǎng)絡(luò)出版地址:http: ∥www.cnki.net/kcms/detail/61.1069.T.20161019.1622.008.html