R o b o c u p 3 D仿人機器人中基于頻率加權的Z M P補償算法

李娟娟

（南京郵電大學自動化學院，南京，210003）

0 引言

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

目前，比較成熟的基于ZMP的步態穩定性控制方法包括通過踝關節力矩進行控制、通過質心加速度控制ZMP[1,6,8]以及通過腿的擺動進行控制等等[1,8]。對于踝關節力矩控制方法而言，機器人的踝關節力矩可能會與實際值之間存在很大誤差，而控制腿的擺動這種方法只適用于單足支撐的階段。相反，通過質心加速度控制ZMP的方法[8]卻可以適用于整個步行階段[5]。另外，Tomoya Sato等人已經證實當ZMP的誤差為常量或者是變化很慢時，位置補償可以起到很好的補償效果，使得機器人保持穩定；當ZMP的誤差變化很快時，位置補償的作用不再有效，而加速度補償的效果明顯，使機器人保持穩定。因此Tomoya Sato等人利用低通濾波器將ZMP誤差分為高頻分量和低頻分量，對于高低頻分量分別做加速度補償和位置補償[5,7]。本文就是在以上補償方法的基礎上，考慮到機器人的運動是一個連續的時變系統，因此ZMP誤差也是高低頻混雜非常嚴重的，要通過截止頻率有限的低通濾波器將ZMP誤差按照高低頻明顯的區分是無法實現的。因此本文提出一種新的分配方法，通過FFT[3]將ZMP誤差的頻率信息提取出來，將對機器人的補償量利用加速度補償和位置補償的權值和來表述，其中權值的大小隨著ZMP誤差頻率的變化不斷變化。

在本文中，機器人的穩定行走是通過調整ZMP，使其盡量接近期望的ZMP值來實現的。如果ZMP可以調整到期望的ZMP值，那么就可以使機器人保持穩定，從而避免機器人摔倒。ZMP的誤差是由ZMP干擾造成的，因此有必要對ZMP干擾進行預測。本文中的ZMP干擾預測器首先對ZMP的值進行預測，再根據頻率的不同進行劃分，從而對機器人進行位置補償和加速度補償。

1 Robocup3D機器人模型

本文中運用的機器人是Robocup3D仿真足球機器人，機器人的關節示意圖如圖1所示。該機器人是以法國的NAO機器人為實際模型，NAO類人機器人由法國Aldebaran機器人公司制造。它的高度約57cm，重量大概為4.5kg[4]。他全身共有22個自由度，這使其有很強的移動性能。

圖1 機器人關節示意圖

本文在研究過程中將機器人做了一定的簡化，(a)假設機器人整個身體質量集中于一個點即機器人的重心；(b)假設機器人在行走過程中的高度保持不變。

2 ZMP補償算法

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

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

ZMP的實際值與期望值之間的關系由式（2）和式（3）描述

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

若只考慮通過補償機器人的位置來實現對重心的補償，則補償量Δx通過式（4）和式（5）求得：

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

3 基于頻率加權的補償算法

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

針對以上這種實際情況，我們采用式（8）和式（9）求得補償量Δx：

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

當ZMP誤差的頻率f很小時，表示ZMP的誤差變化很慢，此時m的值相應的比較大，n值相應的比較小，因此在補償過程中位置補償占的比重大于加速度補償的比重；相反若ZMP誤差的頻率 f很大時，表示ZMP的誤差變化很快，此時m的值相應的變小，n值相應的變大，因此在補償過程中加速度補償占的比重大于位置補償的比重。

由于本算法是根據頻率的變化實時變化加權系數的大小，因此位置補償與加速度補償的比重在整個補償過程中是根據頻率的變化相應的變化的。在本算法中，對于高頻ZMP誤差和低頻ZMP誤差采取相同的處理方法，不存在頻率分割的問題，因此不必考慮由于截止頻率有限而造成的頻率分配不當的誤差。

4 ZMP補償控制系統

對于ZMP實際值造成干擾的物理量稱為ZMP干擾。ZMP干擾是造成ZMP誤差的主要原因，因此對ZMP的干擾值進行補償可以實現ZMP的實際值盡量接近ZMP的期望值。

Tomoya Sato等人提出的ZMP補償控制系統是利用兩個截止頻率不同的低通濾波器將ZMP的誤差分為高頻和低頻兩部分，高頻部分采用加速度補償，低頻部分采用位置補償。這種方法的正確性也得到了證實，但是從準確性方面來說，由于高頻和低頻信號之間沒有明顯的界限，因此我們采用了FFT的方法獲得ZMP誤差信號的頻率信息，根據頻率的變化不斷的調節補償權值，控制位置補償與加速度補償的比重，從而避免了將一個高低頻混合信號進行頻率分割的難題。

ZMP補償控制系統的結構如圖2所示。圖中 xn代表機器人重心在行走方向上的坐標， 表示扭矩的期望值，

res表示機器人的關節角度。

圖2 ZMP補償控制系統結構圖

5 仿真測試

將上述ZMP補償控制系統用于Robocup3D機器人上，由于在Robocup3D中的足球機器人只在一個平面即球場地面上運動，因此我們只考慮在XOY平面內的ZMP的變化。仿真過程中，用身體的位置來代替ZMP response（在XOY平面內，身體的位置近似于ZMP response的位置）。仿真結果見圖3~圖7。

圖3 原始的ZMP誤差

圖4 加入補償系統后的ZMP誤差

圖5 ZMP原始誤差的幅頻響應

圖6 原始的ZMP期望值與實際值比較

圖7 加入補償系統的ZMP期望值與實際值比較

經過比較圖3與圖4可以很明顯的看出加了補償系統之后，ZMP的誤差明顯減小，因此從誤差分析的角度考慮，上述補償系統對Robocup3D機器人的穩定性是有利的。由圖5可知在步行過程中，ZMP誤差的頻率是不斷變化的，高頻低頻混雜在一起，因此用截止頻率有限的濾波器區分信號勢必會存在一定的誤差。然而在本文中采用的根據頻率加權的方法，因此不必考慮因為頻率混雜造成的誤差。圖6與圖7很形象的將原始的ZMP期望值實際值的差別與加了補償系統后的ZMP期望值實際值的差別做了比較，進一步在信號自身角度驗證了該補償系統對Robocup3D機器人穩定行走的作用。

6 結束語

由于仿人機器人的內在穩定性很差，在步行過程中易受環境的干擾，因此本文針對機器人的穩定性控制提出了一種新的補償算法。該算法通過實時檢測機器人零力矩點的誤差，利用誤差頻率的變化對機器人的零力矩點的位置進行調整，從而實現機器人的穩定行走。

實現機器人的穩定行走是機器人完成各種其他高級動作的基礎，因此也成為發展機器人事業的重中之重。本文所述的補償算法雖然對保持機器人的穩定性有一定的作用，但仔細觀察圖7還是存在細微的誤差，再加上本文只是在Robocup3D的仿真機器人上進行的測試，對于實體機器人的穩定性控制還有待于我們進一步的研究。

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

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

[3] 周輝，董正宏 . 數字信號處理及MATLAB實現[M].北京 . 中國林業出版社，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.