多足機器人足端支反力求解方法比較

趙士杰 郭祖華

（北京航空航天大學(xué)，北京，100191）

多足機器人足端支反力求解方法比較

趙士杰 郭祖華

（北京航空航天大學(xué)，北京，100191）

多足移動機器人運動過程中足端支反力的求解是一個難題，本文介紹并比較了兩種計算方法，即偽逆法和優(yōu)化方法。偽逆法將等式約束線性化處理并補充到平衡方程中，然后利用偽逆理論求解；優(yōu)化方法則以各個關(guān)節(jié)力矩的平方和最小為目標(biāo)函數(shù)，搜尋最優(yōu)解。本文用偽逆法、基于常規(guī)規(guī)劃理論的優(yōu)化方法和遺傳算法分別實現(xiàn)了某6足機器人足端反力的求解。通過對計算結(jié)果進(jìn)行分析比較，得知：偽逆法效率高，但結(jié)果偏于保守，機器人行走時能耗偏高；優(yōu)化方法得到的結(jié)果雖然使機器人行走時能耗更少，但計算速度慢，無法用于實時控制。

多足機器人，動力學(xué)，足端力分配

0 引言

多足移動機器人是一種特殊的移動機器人，其仿生的移動模式使得人們對該類機器人的研究方興未艾。多足機器人處于準(zhǔn)靜態(tài)狀態(tài)下，至少有三條腿支撐才能保持機體穩(wěn)定，而機器人三足支撐時，將有9足端反力。對于一個移動機器人而言，只能列出與足端反力相關(guān)的6個平衡方程，這就導(dǎo)致多足機器人足端反力的求解是一個多解問題。而實際機器人行走的某時刻只有一組唯一的足端反力（或關(guān)節(jié)力矩）存在，所以在多組解中只有一組解與實際行走狀態(tài)是相符的。本文比較和分析現(xiàn)有方法的特點，為在仿真和控制過程中機器人足端反力和關(guān)節(jié)力矩求解方法的選取提供參考依據(jù)。

國內(nèi)外多位學(xué)者對以上問題進(jìn)行了研究，提出了不同的解決方法。文獻(xiàn)[1]采用螺旋理論列出了多肢體系統(tǒng)的平衡方程，并根據(jù)偽逆方法的思想，推導(dǎo)了足端支反力的求解公式，但該方法沒有考慮摩擦約束。

文獻(xiàn)[2]提出用對偶線性規(guī)劃算法（Compact-Dual Linear-Programming,CDLP）解決力分布問題，對偶線性規(guī)劃算法是線性規(guī)劃算法中的一種，首先求解出線性約束等式的一般解，然后把線性約束等式的一般解代入目標(biāo)函數(shù)和不等式約束中，以消除線性等式約束，減小計算量，再利用對偶原理把約束的個數(shù)轉(zhuǎn)化為變量的個數(shù)，最后，利用單純形法尋找最優(yōu)解，但是該方法容易產(chǎn)生解不連續(xù)的問題。

文獻(xiàn)[3]以電機所需功率總和最小為目標(biāo)函數(shù)，將非線性不等式摩擦約束簡化為線性等式和不等式方程，消除線性約束，減小計算量，最后用優(yōu)化算法求解各足端反力。本文雖然采用非線性規(guī)劃算法克服了解不連續(xù)性問題，但和其他優(yōu)化方法一樣存在計算效率低的問題，不能用于實時控制。

文獻(xiàn)[4]補充了三個與足端反力有關(guān)的線性方程，與力/力矩平衡方程組成一個有唯一解的線性方程組，實現(xiàn)了解析求解，計算速度快，但忽略了一些約束條件，得到的關(guān)節(jié)力矩較大。

文獻(xiàn)[5]中J.F.Gardner也采用了偽逆法，但他根據(jù)實驗的結(jié)果補充了兩個摩擦力約束方程，對摩擦力方程進(jìn)行線性化處理后加入到原來的力平衡方程組中。

文獻(xiàn)[6]中Mustafa Suphi Erden采用了優(yōu)化方法直接求解機器人足端支反力，作者由機器人動力學(xué)方程求解關(guān)節(jié)力矩，考慮足端的摩擦約束，以關(guān)節(jié)力矩平方和最小為目標(biāo)函數(shù)，進(jìn)行優(yōu)化求解，這種方法使得機器人關(guān)節(jié)力矩達(dá)到最小。

綜上所述，求解多足機器人足端反力的方法有兩類，一類是偽逆法，另一類是優(yōu)化方法。本文以六足機器人為例，在給定的機器人運動形式下，分別采用這兩種方法對足端反力和對應(yīng)的關(guān)節(jié)力矩進(jìn)行求解，針對第二種方法，本文采用經(jīng)典的數(shù)學(xué)規(guī)劃算法和遺傳算法對優(yōu)化模型進(jìn)行求解，最后對結(jié)果進(jìn)行了比較分析。

1 機器人模型及動力學(xué)建模

六足機器人是一個復(fù)雜的機械系統(tǒng)，它是由軀體和繞軀體均勻分布的六條腿構(gòu)成。每條腿有3個連桿，通過3個轉(zhuǎn)動關(guān)節(jié)連接起來，有3個自由度，機器人模型中包含19個剛體，共有24個自由度。本文所使用的六足機器人模型如圖1所示，機器人的主要結(jié)構(gòu)和質(zhì)量參數(shù)見表1。

圖1 六足機器人模型

表1 六足機器人軀體及桿件的設(shè)計參數(shù)

采用D-H方法建立各個剛體的坐標(biāo)系，利用Newton-Euler公式建立六足機器人的動力學(xué)模型，消去動力學(xué)方程中的內(nèi)力或直接運用達(dá)朗伯原理可以列出機器人運動過程中某時刻的平衡方程（1）：

以上公式中所有的量均在慣性坐標(biāo)系下表示。

把方程(1)寫成矩陣形式：

其中：

為了便于表達(dá)機器人的打滑條件，定義機器人足端力的切向分量和法向分量的比值(簡稱為切法比系數(shù)）如下：

顯然，為了避免機器人打滑，足端力的各個分量必須滿足如下約束條件：

其中μ為足端與地面的滑動摩擦系數(shù)。

對六足機器人而言，至少有三條腿支撐才能保持穩(wěn)定，即。方程（2）中未知量的個數(shù)大于方程的個數(shù)，足端支反力的可行解不唯一。該類方程可以采用優(yōu)化方法來求解，即求解滿足某一特定目標(biāo)的最優(yōu)解。現(xiàn)主要有兩種思路：一是用偽逆法求解方程（2），二是以關(guān)節(jié)力矩的平方和最小為目標(biāo)函數(shù)，用各種優(yōu)化算法迭代求解。本文考察六足機器人3條腿支撐時的平衡方程，其中僅有6個方程，但有9個未知量。

2 偽逆方法

利用偽逆方法求解足端反力本質(zhì)上是求解一個含有等式約束的二次規(guī)劃問題，顯然偽逆求解方法中沒有辦法計入（3）式所表示的非線性不等式約束。根據(jù)文獻(xiàn)[5]中Gardner研究的結(jié)論，引入兩個等式約束方程。

方程（4）保證了各個足端支反力fF的法向力F0和切向力的比值相等。我們可以這樣認(rèn)為：真實準(zhǔn)確的足端支反力是支反力初始值和偏差FP量的和。故有：

對于非線性約束方程（4），我們利用泰勒公式在支反力初始值F0處進(jìn)行一階近似，得：

利用方程（4）（5）（6）可得：

其中：

把方程（5）和方程（7）代入方程（2）中，形成新的方程組（8）。

簡記作：

3 優(yōu)化方法

足端支反力的求解也可以直接采用非線性優(yōu)化的方法，優(yōu)化時一般以機器人行走過程中關(guān)節(jié)力矩最小為目標(biāo)。機器人在行走時，其運動狀態(tài)和所受的外力必須滿足機器人的動力學(xué)方程，于是，可以采用如下優(yōu)化模型來求解機器人的足端支反力。

4 計算結(jié)果及分析

現(xiàn)只考慮機器人行走一步的情況，機器人的軀體質(zhì)心軌跡和邁步足的足端軌跡按照六次多項式給出，分別采用偽逆方法、遺傳算法和傳統(tǒng)優(yōu)化方法對該問題進(jìn)行求解。編程工具為Matlab，遺傳算法使用Matlab中的函數(shù)GA，傳統(tǒng)的優(yōu)化算法采用Matlab優(yōu)化工具箱的函數(shù)fmincon。在Matlab中仿真了六足機器人邁一步的動作，各種方法實現(xiàn)一次足端支反力的求解所花費的時間分別為：利用偽逆法求解所需時間為0.0241s，利用Matlab工具箱fmincon函數(shù)所需時間為5.1037s,而利用遺傳算法求解所需時間為5282s。

可見，利用偽逆法求解足端支反力所需要的時間很小，可以用于實時控制，而優(yōu)化方法耗時長，無法用于實時控制。在兩種優(yōu)化方法中，傳統(tǒng)優(yōu)化方法（fmincon）需要的時間相對較少，而遺傳算法求解運行時間過長。

計算結(jié)果如圖2-圖5所示，包括六足機器人邁一步過程中足端支反力、關(guān)節(jié)力和力矩。圖2給出了各種不同算法求得的某一足端反力的切法比系數(shù)，其中，圖2（a）是偽逆法求得的結(jié)果，圖2（b）是傳統(tǒng)優(yōu)化方法求得的結(jié)果，圖2（c）是遺傳算法（GA）求得的結(jié)果。結(jié)果顯示，偽逆法求得的支反力其切向力和法向力比率系數(shù)非常小，小于10-3，而兩種優(yōu)化方法求出的支反力比例系數(shù)相當(dāng)，都在0.1左右，但是遺傳算法求得的結(jié)果曲線不光滑，其余兩條支撐腿求得的結(jié)果類似。

圖2 一條支撐腿的切法比系數(shù)

圖3 顯示了三種不同方法求得的三個支撐足的足端支反力大小，結(jié)果顯示，三種方法求得的結(jié)果變化趨勢一致。其中，偽逆法求得的支反力比較小，另外利用優(yōu)化方法fmincon和GA求得的支反力大小幾乎完全一樣，重合在一起，但比偽逆法求得的結(jié)果偏大。

圖4顯示了機器人做單步行走時，3條支撐腿的力矩變化，其中最左側(cè)的一列是各腿關(guān)節(jié)1的力矩，最右側(cè)的一列是各腿關(guān)節(jié)3的力矩。

圖3 三條支撐腿的支反力大小

從圖4中可以看出，由于偽逆法和優(yōu)化方法追求的目標(biāo)不一樣，所以得到的各個關(guān)節(jié)力矩有較大區(qū)別，用偽逆法求得的結(jié)果顯示各條腿關(guān)節(jié)1和關(guān)節(jié)3的力矩很小，而關(guān)節(jié)2的力矩較大；兩種優(yōu)化算法得到的結(jié)果也是關(guān)節(jié)1和關(guān)節(jié)3的力矩小，關(guān)節(jié)2的力矩大，但差別沒有這么顯著。另外，利用優(yōu)化算法fmincon和GA得到的各關(guān)節(jié)力矩曲線幾近重合；利用遺傳算法優(yōu)化得到的力矩有小幅度的波動，調(diào)整遺傳算法的種群大小、遺傳代數(shù)等參數(shù)后，仍得不到滿意的結(jié)果，且運行時間很長，效率低下。

將偽逆法求得的足端反力代入動力學(xué)方程，可以求得機器人各個關(guān)節(jié)的力矩，然后，將機器人行走每一時刻的各個關(guān)節(jié)的力矩取平方再求和；將優(yōu)化方法求得的關(guān)節(jié)力矩做同樣的處理，并將數(shù)據(jù)畫在圖5上，可以看出，偽逆法求得的關(guān)節(jié)力矩要明顯大于優(yōu)化法求得的關(guān)節(jié)力矩，前者大約是后者的1.3倍。雖然兩種優(yōu)化算法得到的力矩平方和大小曲線已經(jīng)重合，但利用遺傳算法求解耗時非常長。

結(jié)果表明，采用優(yōu)化法求得的足端反力能使機器人在行走過程中關(guān)節(jié)力矩明顯小。結(jié)合圖2的結(jié)果分析可知，偽逆法得到的結(jié)果保證各個支撐足的切法比系數(shù)相等，使得機器人足端反力的切法比系數(shù)很小，這表明該算法是一種保守的足端力方案，使得機器人行走時遠(yuǎn)離打滑的狀態(tài)，而優(yōu)化法本質(zhì)上是放松了這一約束，得到了關(guān)節(jié)力矩更優(yōu)的方案。

圖4 三個支撐腿上的9個關(guān)節(jié)力矩的大小

圖5 三條支撐腿的關(guān)節(jié)力矩平方和

5 結(jié)論

本文介紹了關(guān)于六足機器人足端支反力分布問題的兩類求解方法，即支反力求解的偽逆方法和優(yōu)化方法。偽逆法本質(zhì)上是以九個足端支反力分量的平方和最小為目標(biāo)的算法，優(yōu)化方法則以各個關(guān)節(jié)的力矩平方和最小為目標(biāo)。為了比較各種方法求得結(jié)果的差異，文中用偽逆法、基于常規(guī)規(guī)劃理論的優(yōu)化方法和遺傳算法分別實現(xiàn)了某六足機器人足端支反力的求解。從計算時間上看，偽逆法計算速度遠(yuǎn)遠(yuǎn)快于優(yōu)化方法，有望用在控制算法中實現(xiàn)實時計算；從計算結(jié)果來看，偽逆法得到的結(jié)果偏于保守，能耗較大，而優(yōu)化方法得出的關(guān)節(jié)力矩較小，得到了更優(yōu)的足端力分配方案。針對優(yōu)化方法，本文比較遺傳算法和常規(guī)的優(yōu)化算法求解效果，計算表明，遺傳算法耗時長，而且難以收斂到光滑的關(guān)節(jié)力矩的曲線，并不適合求解這類問題。

為了實現(xiàn)機器人的設(shè)計分析，采用常規(guī)優(yōu)化算法可以得到較滿意的結(jié)果；如果從控制的觀點看，采用偽逆法可以滿足求解速度的要求，但得到的足端力分配方案偏于保守，基于這種方案實施控制，難以發(fā)揮機器人應(yīng)有的機動性能。

[1] Kumar V R，Waldron K J.Force distribution in closed kinematic chains[J].IEEE Journal of Robotics and Automation，1988，4（6）：657-664.

[2] Cheng F T，Orin David E. Efficient Algorithm for Optimal Force Distribution-The Compact-Dual LP Method[J].IEEE Transactions on Robotics and Automation，1990，6（2）：178-187.

[3] 王新杰.多足步行機器人運動及力規(guī)劃研究[D].武漢，華中科技大學(xué)機械電子工程，2005.

[4] Liu H Y，Wen B C. Force Distribution for the Legs of a Quadruped Walking Vehicle[J].Journal of Robotic Systems，1997，14（1）：1-8.

[5] Gardner J F. Efficient computation of force distributions for walking machines on rough terrain[J].Robotica，1992，10（5）：427-433.

[6] Erden M S，Leblebicioglu K. Torque Distribution in a Six-Legged Robot[J].IEEE Transactions On Robotics，2007，23（1）：179-186.

[7] Roy S S，Pratihar D K. Kinematics，Dynamics and Power Consumption Analyses for Turning Motion of a Six-Legged Robot[J].Journal of Intelligent & Robotic Systems，2014，74（3-4）：663-688.