應用擴展零力矩點預觀控制和分解動量控制的仿人機器人全身運動規劃方法

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

(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