基于變質心高度策略的雙足機器人變步長步態(tài)規(guī)劃

周江琛，肖曉暉

(武漢大學動力與機械學院，湖北武漢，430072)

雙足步行機器人能適應復雜環(huán)境，具有高度的靈活性和運動能力，在野外探測、家庭服務等諸多領域有著廣闊的應用前景[1]。為提高雙足機器人在復雜環(huán)境下的運動能力，一方面，機器人應具備通過調整步長來跨越地面障礙的能力，在生成步行模式時要考慮機器人步長調節(jié)能力；另一方面，要考慮變步長步行過程中機器人的能量效率。針對變步長步行，眾多學者參考ZMP 軌跡生成步態(tài)，通過調整ZMP 軌跡以實現(xiàn)目標運動[2-3]。KAJITA等[4]提出一種線性倒立擺模型跟蹤控制器，通過調整質心軌跡來減小ZMP誤差；馮帥等[5]通過補償單足相時間，實現(xiàn)機器人質心軌跡平穩(wěn)過渡并實現(xiàn)變步長步行；張世龍等[6]基于時間補償和踝關節(jié)力矩控制調整ZMP，在步長變化時，在線修正機器人狀態(tài)，保證穩(wěn)定步行。與此同時，也有部分研究人員研究人類步行原理，建立更精確的運動模型或者采用仿生學原理，實現(xiàn)雙足步行。AFTAB等[7]采用飛輪倒立擺模型(inverted pendulum plus flywheel model, IPFM)，通過軀干轉動來調整ZMP 以控制落腳點；KUDOH 等[8]通過捕捉人類動作提出邁步策略。另外，一些基于智能控制的方法也得到廣泛運用，如劉飛等[9]通過離線學習，建立最優(yōu)落腳點與軀干姿態(tài)模型，實現(xiàn)落腳點調整；ZHONG 等[10]采用神經(jīng)網(wǎng)絡結合模糊邏輯控制器實現(xiàn)復雜地面下機器人運動控制。上述步長調整方法基于恒定高度的LIPM，認為機器人在步行過程中質心高度保持不變，避免由高度變化帶來的復雜非線性問題。由于運動學約束，質心高度限制了機器人的步長變化范圍，在當步長發(fā)生較大改變時，逆運動學無法得出有效解；同時，這類方法忽略了質心高度對機器人的影響。而基于智能控制的方法需要一定的訓練時間，對環(huán)境適應力較差。研究表明，人類步行過程中軀干高度并不固定且不同環(huán)境下具有多樣步態(tài)，以實現(xiàn)高效穩(wěn)定步行[11]。同時，在ZMP 穩(wěn)定裕度內，通過調整步長和線性倒立擺模型高度可以降低機器人能耗[12]。鑒于此，仿人機器人行走的高度調整策略也引起研究者關注。一方面，高度變化帶來復雜非線性問題，傳統(tǒng)基于參考ZMP 軌跡的步行模式生成器解算困難；另一方面，在實時調整落腳點時，為提高穩(wěn)定性，必須考慮機器人面臨的結構尺寸和穩(wěn)定裕度等多方面的約束。模型預測控制(model predictive control, MPC)能解決非線性多約束優(yōu)化問題。目前，已經(jīng)有學者基于MPC 實現(xiàn)落腳點調整[13]、側向干擾恢復[14]和機器人上下臺階[15]，但這些方法沒有關注到變步長步行中質心高度的調整。為此，本文針對仿人機器人變步長步行需求，鑒于傳統(tǒng)線性倒立擺步態(tài)規(guī)劃方法中質心高度對步長可變范圍的限制，同時考慮倒立擺模型質心高度對步行能量效率的影響，引入高度變化，提出一種基于MPC 的變質心高度的步態(tài)生成方法。該方法基于變質心高度策略生成變步長步態(tài)，避免了由于運動學約束導致的固定質心高度對步長可調范圍的限制；同時，考慮到質心高度對步行能耗的影響，以不同步長下能耗最低的質心高度為參考，通過調整質心高度以提高變步長步行能量效率。

1 雙足機器人節(jié)能步行

1.1 雙足步行步態(tài)生成

在LIPM的雙足步態(tài)生成過程中，將機器人簡化為1個三維倒立擺模型，雙足步行模式由步行參數(shù)生成參考ZMP 軌跡，ZMP 軌跡通過LIPM 生成質心軌跡，再由逆運動學解算各關節(jié)角度，生成雙足步行步態(tài)，其流程如圖1所示。

為避免由質心高度變化帶來的非線性問題，線性倒立擺模型將質心維持在恒定高度[2]。然而，質心垂直狀態(tài)會影響機器人的平衡能力和能量效率，例如當質心高度較低時更容易平衡，具有更高的魯棒性，加速質心向上可以短暫地提高魯棒性[16]，將質心運動維持在恒定高度，導致步態(tài)不自然，同時增加了膝關節(jié)力矩和速度，降低了能量效率[11]。

圖1 基于LIPM生成雙足步行步態(tài)流程Fig.1 Biped walking gait generation process based on LIPM

1.2 能耗最低質心高度

為驗證質心高度對能量效率的影響，同時考慮到機器人變步長步行需求，以Aldebaran Robotics 公司設計的NAOH25 型機器人為實驗平臺，在穩(wěn)定性前提下，研究不同步長步行過程中能效最優(yōu)質心高度。

采用理論計算和實驗驗證相結合的方法，首先，估算步行能耗，確定能耗最低質心高度；然后，通過樣機實驗進行修正。

以NAOH25 仿人機器人為平臺，重點研究機器人下肢運動。圖2 所示為本文采用的桿式模型。由圖2可見：將機器人視為由數(shù)個連桿組成，每個連桿固定1個局部坐標系，將機器人樣機簡化成10個自由度連桿模型，Σw表示固定在地面上的全局坐標系，Σb表示機器人腰部中心的局部坐標系，以機器人向前為全局坐標系x軸方向，向左為全局y軸方向，豎直向上為全局z軸方向。

圖2 機器人連桿模型Fig.2 Linkage model of robots

由于目前沒有能明確描述機器人能量消耗的表達式，所以，通用的評價指標是計算機器人10個關節(jié)能耗求和。最重要的是要計算行走所需功率：

式中：Pi為第i段連桿關節(jié)功率；τi和分別為第i段連桿關節(jié)力矩和角速度。

將單足相ZMP 固定在支撐足中心，由三次多項式規(guī)劃足部擺動軌跡，生成雙足步態(tài)[17]，得出式(1)中各關節(jié)角速度；然后，基于牛頓-歐拉方程，從擺動腿末端向支撐腿依次計算作用在各連桿上的力和力矩，解逆動力學，通過迭代求出各關節(jié)力矩，估算能耗[18]。

由牛頓-歐拉定理，圖2中第i根連桿的動力學方程為

式中：fi和τi分別為連桿和對應關節(jié)所受外力和力矩，初值條件fi+1和τi+1為0；ac,i和ωi分別為連桿質心加速度和關節(jié)角速度；Ii為連桿慣性張量；為旋轉矩陣；ri-1,ci為第i-1坐標系原點到i-1連桿質心位置矢量。

迭代計算各關節(jié)力矩，再由式(1)可理論計算雙足步行能耗，改變倒立擺模型質心高度，即可得出不同高度下步行理論能耗，獲得理論最優(yōu)高度?？紤]模型誤差，圍繞理論最優(yōu)高度選取附近區(qū)域高度進行樣機實驗。由于NAO 機器人不具備關節(jié)扭矩測量傳感器，這里采用電流數(shù)據(jù)來衡量能量消耗[3]，在實驗過程中，保持機器人上肢所有關節(jié)鎖定，即可由下肢運動關節(jié)總平均電流即可評估整體能耗。

以步長取0.060 m為例，理論計算能耗最低質心高度為0.318 m，倒立擺高度分別選取0.310，0.314，0.315，0.316，0.318 和0.319 m 進行步行實驗，步行周期為1.0 s，單足相時間占80%，每次進行10 個周期的定步長步行，保證每次實驗在同樣的地面環(huán)境下完成。機器人關節(jié)平均電流結果如圖3所示，由圖3可見：定步長步行中不同質心高度下能耗不同，樣機實驗中，0.060 m 步長下能耗最低質心高度為0.315 m。

圖3 機器人關節(jié)平均電流Fig.3 Average current of robot joint

通過樣機實驗，得出步行周期為1.0 s時6種不同步長下能耗最低時的最優(yōu)離心高度，結果如表1所示。

表1 不同步長下最優(yōu)質心高度Table 1 Optimal COM height for variable step-length

2 基于MPC的質心軌跡

2.1 質心狀態(tài)方程

如上所述，基于固定高度的雙足步行在復雜環(huán)境中具備較差的變步長步行能力，而機器人在不同步長下具有不同的能耗最低的質心高度。因此，考慮變質心高度情況下的變步長步態(tài)生成。將模型預測控制集中在步行機器人的質心運動上，考慮高度變化，質心的狀態(tài)使用3個變量(xc,yc,zc)來定義，分別表示質心在前向、側向和垂直方向的位置。

基于MPC 理論，以質心前向、側向和豎直方向位置的三階微分為控制輸入并假設在每個采樣時間間隔Δt內的控制輸入保持恒定，以下1個采樣時刻質心位置為輸出，則在第k個采樣時刻，系統(tǒng)狀態(tài)方程如下：

利用上述模型，給定當前狀態(tài)，在預測域Tpre(包含未來Np個采樣時長)內質心位置狀態(tài)序列W(k+1)=[w(k+1),…,w(k+Np)]T，預測時域內參量速度狀態(tài)序列和加速度狀態(tài)序 列可 以 由 下 式表示：

式中：cp=[1 0 0]為位置狀態(tài)選擇矩陣。從質心狀態(tài)矢量中取出位置分量同理，cv=[0 1 0]，ca=[0 0 1]。

2.2 優(yōu)化目標函數(shù)

MPC 的核心問題是通過構建優(yōu)化目標函數(shù)和可行域得到穩(wěn)定狀態(tài)下最優(yōu)輸出。給定參考的足部落腳點位置和步行周期(本文中，各周期時長為定值)，在優(yōu)化目標中引入質心水平位置跟蹤誤差、參考高度跟蹤誤差和足部落腳點位置誤差。此外，考慮機器人驅動能力引入質心速度和輸入懲罰。最終，建立如下優(yōu)化目標函數(shù)Fgoal。

式中：αW，βW，γW和δS分別為位置跟蹤懲罰系數(shù)、速度懲罰系數(shù)、控制輸入懲罰系數(shù)和足部落腳點跟蹤懲罰系數(shù)(為保證Hessian矩陣正定性，以上系數(shù)均設置為大于0)；為預測時域內質心在地面投影位置和高度的參考值；S(k)=[s(k+1),…,s(k+Nf)]T和分別為預測時域所利用的未來Nf個步行周期(除當前支撐周期外)的實際落腳點和參考落腳點位置(s∈{dx,dy})；[Dx,Dy]T為預測時域內未來Nf個步行周期前向和側向足部落腳點位置，如對第k個采樣時刻，有Dx(k)=[dx(k,1),…,dx(k,Nf)]T。

式(8)中，輸入矢量χ為

本文提出的目標函數(shù)關注于位置跟蹤，從而保證機器人能夠在變步長步行中跟蹤預定足部落腳點。與文獻[16]不同，本文考慮到倒立擺模型高度的影響，將質心高度的參考值單獨設置，其參考高度與預測周期步長相關聯(lián)，據(jù)表1中不同步長下能耗最低的質心高度，實現(xiàn)質心高度可調的變步長步行。

2.3 約束模型

為保證穩(wěn)定步行，需要考慮ZMP穩(wěn)定性約束。高度改變時，以前向運動為例，有

式中：px為ZMP 點的x方向(前向運動)分量；g為重力加速度。根據(jù)ZMP 穩(wěn)定性理論，機器人在行走過程中應該保證ZMP在足部支撐多邊形內[2]。由于單足支撐階段支撐多邊形穩(wěn)定區(qū)域最小，并且雙足支撐過程ZMP 穩(wěn)定裕度較大，故本文只考慮單足支撐階段以簡化約束形式。在預測時域內ZMP位置應該滿足以下約束：

式中：[px(k+i),…,px(k+Np)](i∈[1,Np])為采樣時域內所有采樣時刻對應的實際ZMP點在全局坐標系下x軸位置，即前向位置；[py(k+i),…,py(k+Np)]為實際ZMP 點在全局坐標系下y軸位置，即側向位置；[dm(k+i),…,dm(k+Np)](m∈{x,y})為采樣時域內所有采樣時刻對應的足部中心落點(落腳點)；Np為預測時域內采樣點數(shù)；和分別為x方向上ZMP相對于足部落點的下邊界和上邊界，由機器人足部尺寸決定；和分別為y方向上ZMP 相對于足部落點的下邊界和上邊界，由機器人足部尺寸決定。以ZMP 上邊界為例，式(11)中ZMP 約束計算如下：

2.4 參考軌跡

在式(8)中，需要給定預測時域內足部參考落腳點、質心地面投影參考位置和質心參考高度。不同周期的足部落腳點可以通過預先指定參考步長和步寬等參數(shù)給出。對于實際機器人而言，當前已落下的支撐腳應保持不動。因此，這里需要將當前周期足部落腳點和預先指定的未來周期落腳點映射成整個預測時域的參考落腳點。

在滾動優(yōu)化過程中，預測時域開始的采樣時刻落在當前初始周期，落腳點位置不可移動，而預測時域內未來(Nf- 1)個落腳點位置通過優(yōu)化函數(shù)進行調整。預測時域內參考足部落腳點(以前向落腳點為例)可由下式計算：

針對參考質心位置，將其在地面的投影位置關聯(lián)到每個周期的支撐腳中心位置(即各個周期的落腳點)。以前向運動為例，在預測時域內第k個采樣點質心參考位置為

本文關注于變步長過程中質心高度的調整，因此，每個周期，式(8)中質心參考高度與當前周期步長關聯(lián)，以實現(xiàn)高度可調變步長步行。

式中：Href為預測時域內質心參考高度，根據(jù)預測周期的步長，通過查表1得出對應步長下能耗最低質心參考高度。

2.5 二次約束二次規(guī)劃問題

結合式(12)的約束方程，式(8)的優(yōu)化目標函數(shù)，優(yōu)化問題最終可表示成下列QCQP模型：

式中：Fgoal(χ)為優(yōu)化目標函數(shù)；hj(χ)為約束函數(shù)；χ?RNt是由式(9)表示的輸入矢量；Q?RNt×Nt是目標函數(shù)的Hessian 矩陣；Pi?RNt×Nt為約束方程的Hessian 矩陣；q∈RNt×1,pi∈RNt×1為1 次項系數(shù)矩陣；bj∈R 為約束函數(shù)的常數(shù)項；Nt和Nc分別為輸入變量數(shù)目和約束條件數(shù)目。

式(16)中目標函數(shù)Hessian矩陣Q和1次項系數(shù)矩陣q推導如下：

式(17)中diag函數(shù)以各矩陣為對角元素生成對角矩陣，各對角元素矩陣計算如下：

式中：I為單位矩陣。

式(16)中約束方程Hessian矩陣Pi和1次項系數(shù)矩陣pi和常數(shù)項推導如下：

一旦生成式(16)所示的二次約束二次規(guī)劃問題，則可以轉化為序列二次規(guī)劃(sequential quadratic programming,SQP)求解[19]。采用簡單SQP方法，在式(16)中圍繞初始迭代點產(chǎn)生以下二次規(guī)劃問題：

解二次規(guī)劃，通過χ=χ+δ更新狀態(tài)變量χ，用新的χ重復解δ，該步驟重復Ns次(本文中取Ns為2，優(yōu)化問題收斂且計算效率較高)。

為提高優(yōu)化問題的解算效率，取MPC 模型中預測時域采樣間隔大于機器人運動控制采樣間隔，即Δt＞Δt′，同時保證tpre＞1.5tr(其中，tpre為預測時長，tr為步行周期時長)，得到的預測時域內的質心軌跡通過三次Hermite插值，得出采樣間隔為Δt的軌跡。

3 實驗

3.1 實驗平臺與參數(shù)設置

以Aldebaran Robotics 公司設計NAOH25 機器人為實驗平臺進行實驗，其身高為0.574 m，髖關節(jié)高度為0.330 m，雙足中心間距為0.100 m，質量約5.4 kg，下肢具有12 個自由度，可實現(xiàn)三維步行。

在實驗過程中，默認各周期參考步長為0.04 m，參考步寬為0.10 m，擺動腿最大擺動高度為0.02 m。為減小足部與地面的碰撞，擺動腿軌跡采用5次多項式插值。模型參數(shù)如表2所示。

3.2 變步長步行

在復雜地面環(huán)境下，機器人應具備通過調整步長來跨越地面障礙物和缺陷的能力，本文提出高度可調的變步長調整策略。設計如表3所示的變步長序列參數(shù)設置，默認步長為0.040 m，步寬為0.100 m，質心高度為0.320 m。

除表3所列周期步行參數(shù)發(fā)生改變外，其他周期均使用默認參數(shù)。由表3 可見：機器人將從第2個周期開始在前進方向做變步長運動，同時在第12個周期停止。

表2 模型參數(shù)設置Table 2 Model parameters setup

表3 變步長步行參數(shù)設置Table 3 Variant step-length parameter setup for biped walking

3.2.1 變步長步行實驗

采用本文方法，各周期質心參考高度根據(jù)步長選擇合適值，具體見表1，生成的質心參考軌跡如圖4 所示。由圖4 可見：機器人在第4 個周期改變步長，第4周期內質心參考高度也設置為相應能耗最低的高度，而質心在第3 周期內已經(jīng)開始下降，這顯示出MPC 模型的作用，即可根據(jù)預測視界內參考狀態(tài)的變化，提前調整質心軌跡，從而保證約束條件下優(yōu)化目標函數(shù)最優(yōu)。

分別基于恒定高度LIPM和本文所述方法生成質心軌跡，進行變步長行走對比實驗。采用恒定高度LIPM方法生成變步長質心軌跡時，質心高度分別取0.320 m 和0.315 m。這里0.320 m 為默認高度，為步長0.04 m 下能耗最低時的質心高度，而0.315 m 為離線步態(tài)生成時，關節(jié)角度不出現(xiàn)突變的最高質心高度。實際ZMP 軌跡由機器人足底壓力傳感計算得出，樣機實驗結果如圖5所示。由圖5可見：

圖4 變步長質心參考軌跡Fig.4 COM trajectories for bipedal walking with variant step-length

1) 基于固定高度的LIPM 方法生成變步長步態(tài)，在高度取0.320 m 時，機器人在第9 個周期步長增加為0.10 m 時摔倒，無法實現(xiàn)變步長步行，實驗中觀測到機器人膝關節(jié)轉動出現(xiàn)突變。

2)基于固定高度的LIPM方法在高度為0.315 m時，能實現(xiàn)變步長行走，機器人實際ZMP 保持在支持區(qū)域內。

3)采用本文提出的變質心高度策略的步態(tài)規(guī)劃方法，機器人能通過變質心高度以適應步長變化。機器人在行走過程中能較好地跟蹤預定的足部落腳點，機器人實際ZMP 軌跡始終保持在足部支撐區(qū)域內，能實現(xiàn)穩(wěn)定變步長步行。

4)基于變質心高度策略的變步長步態(tài)生成方法，可以在較高的初始高度下完成變步長運動，機器人的步長調節(jié)范圍不受初始高度影響。而在基于恒定高度的LIPM的步態(tài)規(guī)劃方法中，為實現(xiàn)變步長步行，必須選擇較低的質心高度。

3.2.2 步行能量效率

NAO 機器人步行實驗已經(jīng)證明，基于變質心高度的變步長步行策略可實現(xiàn)穩(wěn)定的變步長步行；而對于采用恒定高度的LIPM，為實現(xiàn)變步長運動，由于運動學約束和穩(wěn)定性約束，必須選擇較低的質心高度。

本文通過對比步行實驗中機器人能量消耗，衡量傳統(tǒng)定高度LIPM方法和本文提出的方法在變步長下的能量。由于在0.320 m的固定高度下機器人未能完成本文設置的變步長行走，這里只對比了固定高度為0.315 m和本文提出的變質心高度方法2組實驗結果。實驗結果顯示，固定質心高度時總平均電流為0.336 8 A，變質心高度時總平均電流為0.279 7 A，機器人關節(jié)平均電流如圖6所示。

由圖6可見：在設定的變步長步行實驗中，采用變質心高度策略，機器人單步能耗低于恒定高度的變步長步行能耗，相對于恒定高度，總平均能耗降低約16%。

圖6 機器人關節(jié)平均電流Fig.6 Average current of robot joint

4 結論

1)提出一種基于模型預測控制的高度可調變步長步態(tài)生成方法，通過穩(wěn)定性約束，建立二次約束二次規(guī)劃求解模型，可生成變高度質心軌跡。

2)基于變質心高度策略的變步長步行相對于恒定高度的變步長步行，平均能耗降低約16%。

3)本文中樣機實驗基于離線步態(tài)，無法應對外力干擾，而MPC 可支持在線軌跡生成和優(yōu)化，這有待進一步研究。在下一階段的研究中，將在線生存質心軌跡，同時考慮上身運動，以生成更加仿人的步態(tài)，提高變參數(shù)步行過程中機器人的穩(wěn)定性和魯棒性。