雙足機(jī)器人NAO爬樓步態(tài)規(guī)劃

趙宸 陳殿生

（北京航空航天大學(xué)機(jī)械工程及自動(dòng)化學(xué)院, 北京， 100191）

雙足機(jī)器人NAO爬樓步態(tài)規(guī)劃

趙宸 陳殿生

（北京航空航天大學(xué)機(jī)械工程及自動(dòng)化學(xué)院, 北京， 100191）

雙足機(jī)器人的爬樓梯性能是衡量其在復(fù)雜環(huán)境下行走能力的一項(xiàng)重要指標(biāo)。本文針對(duì)雙足機(jī)器人NAO爬樓梯步態(tài)規(guī)劃問題，提出一種離線步態(tài)規(guī)劃方法：基于幾何法建立機(jī)器人爬樓梯逆運(yùn)動(dòng)學(xué)模型；將運(yùn)動(dòng)解耦為前向運(yùn)動(dòng)和側(cè)向運(yùn)動(dòng)，對(duì)于起步、中步和止步三個(gè)階段，采用加速度空間法和幾何約束規(guī)劃法，計(jì)算各關(guān)節(jié)運(yùn)動(dòng)軌跡，并基于其逆運(yùn)動(dòng)學(xué)模型，得到各關(guān)節(jié)角序列；分別基于NAOSim和NAO進(jìn)行虛擬樣機(jī)仿真實(shí)驗(yàn)和實(shí)物樣機(jī)驗(yàn)證。實(shí)驗(yàn)結(jié)果表明，步態(tài)規(guī)劃方法合理有效。

機(jī)器人，NAO，樓梯，步態(tài)規(guī)劃

0 引言

雙足機(jī)器人的研制開始于上世紀(jì)60年代末，在五十多年中，雙足機(jī)器人的研究工作取得了舉世矚目的成績，也正在深入人類日常生活的各個(gè)方面。雙足機(jī)器人的步態(tài)規(guī)劃不僅需要考慮系統(tǒng)穩(wěn)定性，有時(shí)還需要建立復(fù)雜的運(yùn)動(dòng)學(xué)或動(dòng)力學(xué)模型，給步態(tài)規(guī)劃帶來了挑戰(zhàn)。

J. Gutmann[1]等基于參考軌跡法和動(dòng)力學(xué)模型法，在高58cm的QRIO上實(shí)現(xiàn)了攀爬高3cm臺(tái)階的實(shí)驗(yàn)。日本Honda公司的K. Hirai[2]等基于動(dòng)力學(xué)模型法和ZMP控制技術(shù)，在高130cm的ASIMO上實(shí)現(xiàn)了攀爬高10cm樓梯的實(shí)驗(yàn)。S. Kim[3]等采用基于HMCD系統(tǒng)的步行數(shù)據(jù)法進(jìn)行步態(tài)設(shè)計(jì)，在高150cm的HRP上實(shí)現(xiàn)了攀爬高8cm樓梯仿真實(shí)驗(yàn)。

機(jī)器人的身高與攀爬樓梯的最大高度之比β是衡量機(jī)器人爬樓梯性能的一個(gè)關(guān)鍵指標(biāo)。β表示機(jī)器人攀爬樓梯的能力，β值越小，機(jī)器人爬樓梯性能越好。ASIMO的β值為13，QRIO的β值為19.3，HRP的β值為18.75。目前，雙足機(jī)器人爬樓梯的β都較大，爬樓梯性能有待提高。

圖 1 爬樓梯步態(tài)周期的階段

本文以雙足機(jī)器人NAO為研究對(duì)象，采用加速度空間法和幾何約束規(guī)劃法，求解其關(guān)節(jié)軌跡，并基于逆運(yùn)動(dòng)學(xué)模型，求得其全身關(guān)節(jié)角序列，實(shí)現(xiàn)β=11.6的高性能爬樓梯運(yùn)動(dòng)。

1 雙足機(jī)器人爬樓梯的運(yùn)動(dòng)學(xué)機(jī)理分析與建模

1.1 雙足機(jī)器人爬樓梯的運(yùn)動(dòng)學(xué)機(jī)理分析

一般來說，每個(gè)步態(tài)周期可分為2個(gè)相期（單腳擺動(dòng)相SSP和雙腳支撐相DSP）、3個(gè)階段（起步階段、中步階段、止步階段），如圖 1所示。在一個(gè)步行周期運(yùn)動(dòng)中，雙足機(jī)器人雙腿作為支撐腿運(yùn)動(dòng)的時(shí)期，稱為雙腳支撐相(DSP: Double support phase)；雙足機(jī)器人某只腿作為擺動(dòng)腿運(yùn)動(dòng)的時(shí)期，稱為單腳擺動(dòng)相(SSP: Single support phase)。根據(jù)對(duì)人類爬樓梯行為的觀測(cè)[4]，雙腳支撐相比一般大于等于10%。

1.2 雙足機(jī)器人的逆運(yùn)動(dòng)學(xué)建模

由于逆運(yùn)動(dòng)學(xué)存在解的存在性和唯一性問題，本文選擇踝關(guān)節(jié)和機(jī)器人質(zhì)心作為已知的關(guān)節(jié)坐標(biāo)，利用幾何法進(jìn)行逆運(yùn)動(dòng)學(xué)求解。下面分別對(duì)于雙足機(jī)器人的擺動(dòng)腿和支撐腿，建立幾何模型，如圖 2和圖 3所示。圖中，L1為機(jī)器人小腿長度，L2為機(jī)器人大腿長度，L3為支撐腿的踝關(guān)節(jié)中心點(diǎn)A1到同側(cè)的髖關(guān)節(jié)中心點(diǎn)C1的距離，L4為擺動(dòng)腿的踝關(guān)節(jié)中心點(diǎn)A2到同側(cè)的髖關(guān)節(jié)中心點(diǎn)C2的距離，ZCH是機(jī)器人質(zhì)心PCOM和兩髖連線中點(diǎn)D沿Z軸的距離，XCOM和ZCOM是機(jī)器人質(zhì)心在矢狀面XOZ中的坐標(biāo)，XANKLE和ZANKLE是機(jī)器人擺動(dòng)腿的踝關(guān)節(jié)中心點(diǎn)A2在矢狀面XOZ中的坐標(biāo)。

圖 2 幾何法逆運(yùn)動(dòng)學(xué)求解(支撐腿)

圖 3 幾何法逆運(yùn)動(dòng)學(xué)求解(擺動(dòng)腿)

1.2.1 支撐腿的逆運(yùn)動(dòng)學(xué)求解

根據(jù)圖 2中的幾何關(guān)系，可知支撐腿的踝關(guān)節(jié)中心點(diǎn)A1到同側(cè)的髖關(guān)節(jié)中心點(diǎn)C1的距離L3和A1C1與Z軸的夾角α為

由式(1)和式(2)可知，支撐腿的關(guān)節(jié)角序列為

1.2.2 擺動(dòng)腿的逆運(yùn)動(dòng)學(xué)求解

根據(jù)圖 3中幾何關(guān)系可知，

由式(4)和式(5)可知，擺動(dòng)腿的關(guān)節(jié)角序列為

2 人形機(jī)器人爬樓梯的步態(tài)規(guī)劃

2.1 雙足機(jī)器人爬樓梯的起步和止步步態(tài)設(shè)計(jì)

2.1.1 起步和止步運(yùn)動(dòng)軌跡規(guī)劃

由于在起步階段的運(yùn)動(dòng)處于準(zhǔn)靜平衡態(tài)，導(dǎo)致起步階段無法直接應(yīng)用倒立擺模型，因此采用加速度空間規(guī)劃方法[5]。為了降低機(jī)器人從靜止加速到步行速度的能耗，同時(shí)便于由起步階段更快地過渡到單腳支撐相，采用降低機(jī)器人重心的方法，將機(jī)器人的勢(shì)能轉(zhuǎn)換為動(dòng)能，有：

由式(7)以及NAO的技術(shù)說明書[6]，計(jì)算可得完成起步步態(tài)后，點(diǎn)PCOM與地面高度差為195mm，如圖 4所示。為保證運(yùn)動(dòng)的平滑性，采用正弦函數(shù)作為起步運(yùn)動(dòng)軌跡函數(shù)，XCOM(t)和ZCOM(t)表示質(zhì)心沿X軸和Z軸的運(yùn)動(dòng)軌跡，本文設(shè)定起步運(yùn)動(dòng)時(shí)間t=2s，則有

圖 4 起步階段的最終姿態(tài)

止步步態(tài)規(guī)劃是起步步態(tài)規(guī)劃的逆過程，同樣采用加速度空間規(guī)劃法，初始姿態(tài)即為起步步態(tài)規(guī)劃的最終姿態(tài)最終姿態(tài)即為機(jī)器人雙腳并齊的零位移、零速度的初始站立狀態(tài)。因此，采用正弦函數(shù)作為止步運(yùn)動(dòng)軌跡函數(shù)，即

2.1.2 起步和止步運(yùn)動(dòng)關(guān)節(jié)角序列求解

圖 5 起步階段關(guān)節(jié)角序列

圖 6 止步階段關(guān)節(jié)角序列

2.2 雙足機(jī)器人爬樓梯的中步步態(tài)規(guī)劃

2.2.1 前向運(yùn)動(dòng)軌跡規(guī)劃

中步運(yùn)動(dòng)軌跡規(guī)劃是機(jī)器人爬樓梯動(dòng)態(tài)步態(tài)規(guī)劃的核心問題。在一個(gè)步行周期內(nèi)，分別對(duì)其質(zhì)心和擺動(dòng)腿踝關(guān)節(jié)進(jìn)行運(yùn)動(dòng)軌跡規(guī)劃。

1) 質(zhì)心運(yùn)動(dòng)軌跡規(guī)劃

由于機(jī)器人爬樓梯時(shí)質(zhì)心隨著樓梯高度不斷變化，因此采用一階變桿長倒立擺模型對(duì)質(zhì)心運(yùn)動(dòng)軌跡進(jìn)行規(guī)劃。以機(jī)器人質(zhì)心PCOM到擺動(dòng)腿踝關(guān)節(jié)中心點(diǎn)距離l(t )為擺長，機(jī)器人擺動(dòng)腿踝關(guān)節(jié)中心點(diǎn)為擺點(diǎn)，假設(shè)支撐腿和擺動(dòng)腿與臺(tái)階的接觸點(diǎn)處于臺(tái)階中心位置，且中步階段初始和終止時(shí)刻機(jī)器人質(zhì)心PCOM在臺(tái)階豎直邊界的延長線上，建立的模型如圖 7所示。

圖 7 機(jī)器人前向變長倒立擺模型

在圖 7中，Ls為機(jī)器人單步步長或樓梯深度，H為樓梯高度，L1是中步階段初始時(shí)刻機(jī)器人質(zhì)心PCOM到支撐腿踝關(guān)節(jié)中心點(diǎn)距離，L2是中步階段最終時(shí)刻機(jī)器人質(zhì)心PCOM到擺動(dòng)腿踝關(guān)節(jié)中心點(diǎn)距離。

根據(jù)圖 7中的幾何關(guān)系，L1和L2的關(guān)系為

設(shè)θ()t 為質(zhì)心與擺動(dòng)腿踝關(guān)節(jié)中心點(diǎn)連線與Z軸的夾角，即變長倒立擺的擺角，則根據(jù)一階倒立擺運(yùn)動(dòng)方程，有

為簡化計(jì)算，令l(tl=l1，則此二階微分方程的通解為

下面推導(dǎo)此微分方程的兩個(gè)邊界條件。

在初始時(shí)刻t=0時(shí)，有

在終止時(shí)刻t=T時(shí)，有

由式(12)、式(13)和式(14)可知，變長一階倒立擺的擺角θ()t 為

假設(shè)倒立擺的桿長變化率為常數(shù)，則變長一階倒立擺的擺長() lt 為

由式(15) 和式(16) 可知，機(jī)器人前向質(zhì)心軌跡為

2) 擺動(dòng)腿的踝關(guān)節(jié)運(yùn)動(dòng)規(guī)劃為避免爬樓梯過程中機(jī)器人與外部環(huán)境發(fā)生碰撞，本文采用幾何約束法對(duì)擺動(dòng)腿踝關(guān)節(jié)沿X軸和Z 軸的運(yùn)動(dòng)軌跡進(jìn)行規(guī)劃，其幾何約束如圖 8所示。

圖 8 雙足機(jī)器人爬樓梯的踝關(guān)節(jié)幾何約束

根據(jù)圖 8所示的幾何關(guān)系，在一個(gè)步行周期內(nèi)，沿X軸擺動(dòng)腿由0移動(dòng)到LS。同時(shí)，為保證運(yùn)動(dòng)的穩(wěn)定性和平滑性，假設(shè)在起始時(shí)刻t=0和終止時(shí)刻t=T沿X方向位移的一階導(dǎo)數(shù)為0，雙足機(jī)器人的初始位置保證其在爬樓梯過程中不會(huì)與外部環(huán)境在X軸方向發(fā)生碰撞。本文采用三次樣條插值函數(shù)對(duì)擺動(dòng)腿踝關(guān)節(jié)沿X軸的運(yùn)動(dòng)軌跡() ANKLE Xt 進(jìn)行規(guī)劃，其數(shù)據(jù)表如表 1所示。

表 1：()ANKLEXt 插值函數(shù)數(shù)據(jù)表

可求得三次樣條插值函數(shù)為：

為了避免擺動(dòng)腿與外部環(huán)境的碰撞，機(jī)器人的擺動(dòng)腿在A3處(t=t3)沿Z軸的坐標(biāo)應(yīng)大于H，這里假設(shè)其沿Z軸坐標(biāo)為H+0.1，并假設(shè)擺動(dòng)腿在中間時(shí)刻()達(dá)到最高點(diǎn)Hmax，采用三次樣條插值函數(shù)規(guī)劃擺動(dòng)腿踝關(guān)節(jié)的運(yùn)動(dòng)軌跡，其數(shù)據(jù)如表 2所示。

可求得三次樣條插值函數(shù)為：表 2：() ANKLE Zt 插值函數(shù)數(shù)據(jù)表

2.2.2 側(cè)向運(yùn)動(dòng)軌跡規(guī)劃

機(jī)器人在爬樓梯過程中，于起步和止步階段，其質(zhì)心和踝關(guān)節(jié)在冠狀面上無運(yùn)動(dòng)軌跡，因此，這里只需要對(duì)中步階段質(zhì)心進(jìn)行側(cè)向運(yùn)動(dòng)軌跡規(guī)劃，采用定長一階倒立擺模型對(duì)其進(jìn)行運(yùn)動(dòng)軌跡規(guī)劃。

以機(jī)器人支撐腿踝關(guān)節(jié)中心點(diǎn)A1為擺點(diǎn)，機(jī)器人質(zhì)心PCOM到支撐腿踝關(guān)節(jié)中心點(diǎn)A1的距離L5為擺長，擺角為θ'(t)，建立如圖 9所示的一階倒立擺模型。

圖 9 側(cè)向質(zhì)心軌跡規(guī)劃的一階倒立擺模型

根據(jù)一階倒立擺方程(11)可知

微分方程(11)的邊界條件為

由式(20)和式(21)可知，

因此，質(zhì)心側(cè)向運(yùn)動(dòng)軌跡沿Y軸和Z軸的位移為

機(jī)器人正向的七桿模型如圖 10所示，圖中，θ7～θ12為腿部關(guān)節(jié)角序列。由于將側(cè)向運(yùn)動(dòng)視為定長的倒立擺模型，因此有θ7=θ8=…=θ12。

下面推導(dǎo)擺角 與θ9之間的關(guān)系。

由圖 10的幾何關(guān)系可知，

圖 10 機(jī)器人七桿模型(正視圖)

2.3 中步階段關(guān)節(jié)角序列求解

根據(jù)逆運(yùn)動(dòng)學(xué)模型，可得中步階段前向關(guān)節(jié)角序列{θi}(i=1,2,…,6)如圖 11所示，側(cè)向關(guān)節(jié)角序列{θi} (i=7,8,…,12)如圖 12所示。

圖 11 前向關(guān)節(jié)角序列

圖 12 側(cè)向關(guān)節(jié)角序列

3 基于NAOSim的仿真試驗(yàn)

NAOSim是用于在三維虛擬現(xiàn)實(shí)環(huán)境中測(cè)試NAO的行為和算法。在NAOSim中導(dǎo)入機(jī)器人全身關(guān)節(jié)角序列，仿真結(jié)果如圖13和表 3所示。

圖13 基于NAOSim的機(jī)器人爬樓梯仿真試驗(yàn)

表 3：仿真試驗(yàn)的技術(shù)指標(biāo)

4 基于NAO的物理樣機(jī)試驗(yàn)

Choregraphe是一款專用于NAO的用戶友好的行為編輯器，可通過不同方式為機(jī)器人編程。在Choregraphe中導(dǎo)入機(jī)器人全身關(guān)節(jié)角序列，實(shí)驗(yàn)結(jié)果如圖14和表4所示，實(shí)驗(yàn)結(jié)果表明，NAO能夠攀爬5.2cm的臺(tái)階。

圖14 NAO爬樓梯的物理樣機(jī)試驗(yàn)

表 4：物理樣機(jī)試驗(yàn)的技術(shù)指標(biāo)

5 結(jié)論

本文根據(jù)雙足機(jī)器人爬樓梯的運(yùn)動(dòng)機(jī)理，基于幾何法推導(dǎo)了雙足機(jī)器人爬樓梯的逆運(yùn)動(dòng)學(xué)模型。同時(shí)，將爬樓梯的過程分為起步、中步、止步三個(gè)階段，在起、止步階段和中步階段分別采用加速度空間規(guī)劃法和幾何約束法進(jìn)行運(yùn)動(dòng)軌跡規(guī)劃，并基于逆運(yùn)動(dòng)學(xué)模型，求解機(jī)器人爬樓梯過程中全身關(guān)節(jié)角序列。在仿真環(huán)境NAOSim與物理樣機(jī)NAO上，分別進(jìn)行了攀爬高度為5cm的樓梯試驗(yàn)，試驗(yàn)結(jié)果表明，本文所采用的離線步態(tài)規(guī)劃方法合理、可行。

[1] Gutmann J,Fukuchi M, et al. Stair Climbing for Humanoid Robots Using Stereo Vision[C]. Intelligent Robots and Systems IEEE/RSJ International Conference,2004(2)： 1407-1413

[2] Hirai K,Hirose M, Haikawa Y , Takenaka T. The development of Honda humanoid robot. IEEE Int. Conf. Robotics and Automation. Leuven. Belgium, 1998： 1321-1326.

[3] Kim S, Sankai Y. Stair Climbing Task of Humanoid Robot by Phase Composition and Phase Sequence[C]. IEEE International Workshop on Robots and Human Interactive Communication, 2005：531-536

[4] Chen C T. Linear system theory and design [M]. 3rd edition.New York： Oxford Press, 1999： 25

[5] 柯顯信. 仿雙足機(jī)器人雙足動(dòng)態(tài)步行研究[D]. 上海：上海大學(xué). 2005 ：63-65

[6] Aldebaran Robotics. NAO Software documentation[EB/OL]. http：//www.aldebaran-robotics.com/documentation/family/nao_h25/links_h25.html.