李娟娟

（南京郵電大學(xué)自動(dòng)化學(xué)院，南京，210003）

0 引言

研制與人類外觀特征類似，具有人類智能，靈活性，并能夠與人交流，不斷適應(yīng)環(huán)境的仿人機(jī)器人一直是人類的夢(mèng)想之一。在仿人機(jī)器人的研究過(guò)程中，機(jī)器人的穩(wěn)定性尤其是步態(tài)穩(wěn)定性的研究已成為新一代機(jī)器人的研究重點(diǎn)。仿人機(jī)器人系統(tǒng)本身的高度非線性及運(yùn)動(dòng)過(guò)程中存在大量的隨機(jī)干擾和各種時(shí)變的外力的作用，使得常規(guī)控制方法在實(shí)現(xiàn)高精確度運(yùn)動(dòng)控制時(shí)顯得力不從心。眾所周知，ZMP(Zero Moment Point)即零力矩點(diǎn)是機(jī)器人穩(wěn)定性的一個(gè)重要評(píng)價(jià)標(biāo)準(zhǔn)，只有當(dāng)ZMP位于支撐多邊形內(nèi)部時(shí)，仿人機(jī)器人才能保持穩(wěn)定。本文就是基于該理論，在Tomoya Sato等人的補(bǔ)償算法的基礎(chǔ)上，提出了一種新的ZMP補(bǔ)償控制方法，使得仿人機(jī)器人在整個(gè)步行過(guò)程中保持ZMP位于支撐多邊形之內(nèi)。

目前，比較成熟的基于ZMP的步態(tài)穩(wěn)定性控制方法包括通過(guò)踝關(guān)節(jié)力矩進(jìn)行控制、通過(guò)質(zhì)心加速度控制ZMP[1,6,8]以及通過(guò)腿的擺動(dòng)進(jìn)行控制等等[1,8]。對(duì)于踝關(guān)節(jié)力矩控制方法而言，機(jī)器人的踝關(guān)節(jié)力矩可能會(huì)與實(shí)際值之間存在很大誤差，而控制腿的擺動(dòng)這種方法只適用于單足支撐的階段。相反，通過(guò)質(zhì)心加速度控制ZMP的方法[8]卻可以適用于整個(gè)步行階段[5]。另外，Tomoya Sato等人已經(jīng)證實(shí)當(dāng)ZMP的誤差為常量或者是變化很慢時(shí)，位置補(bǔ)償可以起到很好的補(bǔ)償效果，使得機(jī)器人保持穩(wěn)定；當(dāng)ZMP的誤差變化很快時(shí)，位置補(bǔ)償?shù)淖饔貌辉儆行?，而加速度補(bǔ)償?shù)男Ч黠@，使機(jī)器人保持穩(wěn)定。因此Tomoya Sato等人利用低通濾波器將ZMP誤差分為高頻分量和低頻分量，對(duì)于高低頻分量分別做加速度補(bǔ)償和位置補(bǔ)償[5,7]。本文就是在以上補(bǔ)償方法的基礎(chǔ)上，考慮到機(jī)器人的運(yùn)動(dòng)是一個(gè)連續(xù)的時(shí)變系統(tǒng)，因此ZMP誤差也是高低頻混雜非常嚴(yán)重的，要通過(guò)截止頻率有限的低通濾波器將ZMP誤差按照高低頻明顯的區(qū)分是無(wú)法實(shí)現(xiàn)的。因此本文提出一種新的分配方法，通過(guò)FFT[3]將ZMP誤差的頻率信息提取出來(lái)，將對(duì)機(jī)器人的補(bǔ)償量利用加速度補(bǔ)償和位置補(bǔ)償?shù)臋?quán)值和來(lái)表述，其中權(quán)值的大小隨著ZMP誤差頻率的變化不斷變化。

在本文中，機(jī)器人的穩(wěn)定行走是通過(guò)調(diào)整ZMP，使其盡量接近期望的ZMP值來(lái)實(shí)現(xiàn)的。如果ZMP可以調(diào)整到期望的ZMP值，那么就可以使機(jī)器人保持穩(wěn)定，從而避免機(jī)器人摔倒。ZMP的誤差是由ZMP干擾造成的，因此有必要對(duì)ZMP干擾進(jìn)行預(yù)測(cè)。本文中的ZMP干擾預(yù)測(cè)器首先對(duì)ZMP的值進(jìn)行預(yù)測(cè)，再根據(jù)頻率的不同進(jìn)行劃分，從而對(duì)機(jī)器人進(jìn)行位置補(bǔ)償和加速度補(bǔ)償。

1 Robocup3D機(jī)器人模型

本文中運(yùn)用的機(jī)器人是Robocup3D仿真足球機(jī)器人，機(jī)器人的關(guān)節(jié)示意圖如圖1所示。該機(jī)器人是以法國(guó)的NAO機(jī)器人為實(shí)際模型，NAO類人機(jī)器人由法國(guó)Aldebaran機(jī)器人公司制造。它的高度約57cm，重量大概為4.5kg[4]。他全身共有22個(gè)自由度，這使其有很強(qiáng)的移動(dòng)性能。

圖1 機(jī)器人關(guān)節(jié)示意圖

本文在研究過(guò)程中將機(jī)器人做了一定的簡(jiǎn)化，(a)假設(shè)機(jī)器人整個(gè)身體質(zhì)量集中于一個(gè)點(diǎn)即機(jī)器人的重心；(b)假設(shè)機(jī)器人在行走過(guò)程中的高度保持不變。

2 ZMP補(bǔ)償算法

假定機(jī)器人的高度是保持恒定的，其ZMP可按下式求得：

該式即ZMP方程式。式中x表示COG（機(jī)器人重心）在前進(jìn)方向的坐標(biāo)值；.. x表示機(jī)器人在前進(jìn)方向的加速度；px表示機(jī)器人的ZMP在前進(jìn)方向上的坐標(biāo)值；zc表示COG的高度，在本文中我們假定是一個(gè)常量；g代表重力加速度。

ZMP的實(shí)際值與期望值之間的關(guān)系由式（2）和式（3）描述

式中 perr表示ZMP的誤差， pref表示ZMP的期望值， pres表示ZMP的實(shí)際值。式（3）中既有x又有因此對(duì)機(jī)器人重心軌跡的補(bǔ)償可以從位置和加速度兩個(gè)方面進(jìn)行考慮[5]。

若只考慮通過(guò)補(bǔ)償機(jī)器人的位置來(lái)實(shí)現(xiàn)對(duì)重心的補(bǔ)償，則補(bǔ)償量Δx通過(guò)式（4）和式（5）求得：

若只考慮加速度補(bǔ)償，則補(bǔ)償量Δx可通過(guò)式（6）和式（7）求得。

3 基于頻率加權(quán)的補(bǔ)償算法

位置補(bǔ)償只適用于ZMP誤差為常量或者變化很慢的情況下，而加速度補(bǔ)償只適用于ZMP誤差快速變化的情況下。然而在機(jī)器人實(shí)際行走過(guò)程中，ZMP誤差的變化是不固定的，有可能在一段時(shí)間內(nèi)保持不變，也有可能不斷變化，而且變化頻率不是固定的。因此很難確定ZMP誤差在什么時(shí)候保持恒定什么時(shí)候不斷變化。

針對(duì)以上這種實(shí)際情況，我們采用式（8）和式（9）求得補(bǔ)償量Δx：

其中，m的大小與ZMP誤差頻率f成反比；n的大小與ZMP誤差頻率 f成正比。m ≤1， n ≤1并且滿足m+n= 1；m表示位置補(bǔ)償量表示加速度補(bǔ)償量。

當(dāng)ZMP誤差的頻率f很小時(shí)，表示ZMP的誤差變化很慢，此時(shí)m的值相應(yīng)的比較大，n值相應(yīng)的比較小，因此在補(bǔ)償過(guò)程中位置補(bǔ)償占的比重大于加速度補(bǔ)償?shù)谋戎兀幌喾慈鬦MP誤差的頻率 f很大時(shí)，表示ZMP的誤差變化很快，此時(shí)m的值相應(yīng)的變小，n值相應(yīng)的變大，因此在補(bǔ)償過(guò)程中加速度補(bǔ)償占的比重大于位置補(bǔ)償?shù)谋戎亍?/p>

由于本算法是根據(jù)頻率的變化實(shí)時(shí)變化加權(quán)系數(shù)的大小，因此位置補(bǔ)償與加速度補(bǔ)償?shù)谋戎卦谡麄€(gè)補(bǔ)償過(guò)程中是根據(jù)頻率的變化相應(yīng)的變化的。在本算法中，對(duì)于高頻ZMP誤差和低頻ZMP誤差采取相同的處理方法，不存在頻率分割的問(wèn)題，因此不必考慮由于截止頻率有限而造成的頻率分配不當(dāng)?shù)恼`差。

4 ZMP補(bǔ)償控制系統(tǒng)

對(duì)于ZMP實(shí)際值造成干擾的物理量稱為ZMP干擾。ZMP干擾是造成ZMP誤差的主要原因，因此對(duì)ZMP的干擾值進(jìn)行補(bǔ)償可以實(shí)現(xiàn)ZMP的實(shí)際值盡量接近ZMP的期望值。

Tomoya Sato等人提出的ZMP補(bǔ)償控制系統(tǒng)是利用兩個(gè)截止頻率不同的低通濾波器將ZMP的誤差分為高頻和低頻兩部分，高頻部分采用加速度補(bǔ)償，低頻部分采用位置補(bǔ)償。這種方法的正確性也得到了證實(shí)，但是從準(zhǔn)確性方面來(lái)說(shuō)，由于高頻和低頻信號(hào)之間沒(méi)有明顯的界限，因此我們采用了FFT的方法獲得ZMP誤差信號(hào)的頻率信息，根據(jù)頻率的變化不斷的調(diào)節(jié)補(bǔ)償權(quán)值，控制位置補(bǔ)償與加速度補(bǔ)償?shù)谋戎?，從而避免了將一個(gè)高低頻混合信號(hào)進(jìn)行頻率分割的難題。

ZMP補(bǔ)償控制系統(tǒng)的結(jié)構(gòu)如圖2所示。圖中 xn代表機(jī)器人重心在行走方向上的坐標(biāo)， 表示扭矩的期望值，

res表示機(jī)器人的關(guān)節(jié)角度。

圖2 ZMP補(bǔ)償控制系統(tǒng)結(jié)構(gòu)圖

5 仿真測(cè)試

將上述ZMP補(bǔ)償控制系統(tǒng)用于Robocup3D機(jī)器人上，由于在Robocup3D中的足球機(jī)器人只在一個(gè)平面即球場(chǎng)地面上運(yùn)動(dòng)，因此我們只考慮在XOY平面內(nèi)的ZMP的變化。仿真過(guò)程中，用身體的位置來(lái)代替ZMP response（在XOY平面內(nèi)，身體的位置近似于ZMP response的位置）。仿真結(jié)果見(jiàn)圖3~圖7。

圖3 原始的ZMP誤差

圖4 加入補(bǔ)償系統(tǒng)后的ZMP誤差

圖5 ZMP原始誤差的幅頻響應(yīng)

圖6 原始的ZMP期望值與實(shí)際值比較

圖7 加入補(bǔ)償系統(tǒng)的ZMP期望值與實(shí)際值比較

經(jīng)過(guò)比較圖3與圖4可以很明顯的看出加了補(bǔ)償系統(tǒng)之后，ZMP的誤差明顯減小，因此從誤差分析的角度考慮，上述補(bǔ)償系統(tǒng)對(duì)Robocup3D機(jī)器人的穩(wěn)定性是有利的。由圖5可知在步行過(guò)程中，ZMP誤差的頻率是不斷變化的，高頻低頻混雜在一起，因此用截止頻率有限的濾波器區(qū)分信號(hào)勢(shì)必會(huì)存在一定的誤差。然而在本文中采用的根據(jù)頻率加權(quán)的方法，因此不必考慮因?yàn)轭l率混雜造成的誤差。圖6與圖7很形象的將原始的ZMP期望值實(shí)際值的差別與加了補(bǔ)償系統(tǒng)后的ZMP期望值實(shí)際值的差別做了比較，進(jìn)一步在信號(hào)自身角度驗(yàn)證了該補(bǔ)償系統(tǒng)對(duì)Robocup3D機(jī)器人穩(wěn)定行走的作用。

6 結(jié)束語(yǔ)

由于仿人機(jī)器人的內(nèi)在穩(wěn)定性很差，在步行過(guò)程中易受環(huán)境的干擾，因此本文針對(duì)機(jī)器人的穩(wěn)定性控制提出了一種新的補(bǔ)償算法。該算法通過(guò)實(shí)時(shí)檢測(cè)機(jī)器人零力矩點(diǎn)的誤差，利用誤差頻率的變化對(duì)機(jī)器人的零力矩點(diǎn)的位置進(jìn)行調(diào)整，從而實(shí)現(xiàn)機(jī)器人的穩(wěn)定行走。

實(shí)現(xiàn)機(jī)器人的穩(wěn)定行走是機(jī)器人完成各種其他高級(jí)動(dòng)作的基礎(chǔ)，因此也成為發(fā)展機(jī)器人事業(yè)的重中之重。本文所述的補(bǔ)償算法雖然對(duì)保持機(jī)器人的穩(wěn)定性有一定的作用，但仔細(xì)觀察圖7還是存在細(xì)微的誤差，再加上本文只是在Robocup3D的仿真機(jī)器人上進(jìn)行的測(cè)試，對(duì)于實(shí)體機(jī)器人的穩(wěn)定性控制還有待于我們進(jìn)一步的研究。

[1] 梶田秀司（日）. 仿人機(jī)器人[M]. 管貽生 譯. 北京:清華大學(xué)出版社, 2007.

[2] 肖南峰 . 智能機(jī)器人[M]. 廣州: 華南理工大學(xué)出版社，2008.

[3] 周輝，董正宏 . 數(shù)字信號(hào)處理及MATLAB實(shí)現(xiàn)[M].北京 . 中國(guó)林業(yè)出版社，2006.

[4] Robocup3D Server Develop Group. RoboCup Soccer Server 3D Manual. 2009.

[5] Tomoya Sato and Kouhei Ohnishi. ZMP Disturbance Observer for Walking Stabilization of Biped Robot[J]. In IEEE Transactions on Robotics, 2008, 290-295.

[6] T.Sugihara,Y.Nakamura and H.Inoue. Realtime Humanoid Motion Generation through ZMP Manipulation based on Inverted Pendulum Control[J]. Proceedings of the 2002 IEEE International Conference on Robotics and Automation, 2002,2(5): 1404-1409.

[7] Tomoya Sato，Sho Sakaino and Kouhei Ohnishi: Parameter Design for ZMP Disturbance Observer of Biped Robot,2008,1650-1655.

[8] Yu Okumura, Tetsuo Tawara, Ken Endo, Takayuki Furuta and Masaharu Shimizu: Realtime ZMP Compensation for Biped Walking Robot using Adaptive Inertia Force Control, 2003, 335-339.