劉 軍，王 楠

(沈陽理工大學 自動化與電氣工程學院，沈陽110159)

一個性能優(yōu)良的四足機器人在規(guī)則與不規(guī)則地形中移動時，應能適應環(huán)境變化，在受到干擾時能夠保持或恢復自身的平衡狀態(tài)[1]。在類人機器人領域，通常有三種策略來恢復機器人穩(wěn)定，三種策略分別在機器人受到不同程度的擾動時應用，此時擾動分為較小、適中、較大三種狀態(tài)；在擾動較小時采用踝關節(jié)或髖關節(jié)策略來調(diào)節(jié)主體位置方向維持平衡[2-4]；當擾動加強時采用步進策略[5-6]；擾動較大時，為避免機器人在降速時發(fā)生難以預料的損傷，采用降速控制法[7-8]。

在以往的四足機器人穩(wěn)定方法研究中主要有兩種方法。第一種方法，為解決四足機器人的大量自由度在實際計算中過于復雜的問題，提出了幾種簡化模型，如線性倒立擺模型(Liner Inverted Pendulum Model，LIPM)[9-10]，并在此基礎上提出了零力距點補償方法(Zero-Moment Point，ZMP)和模型預測控制算法(Model Predictive Control，MPC)[11-13]及將四足機器人視為二足模型提出了二連桿倒立擺模型[14]；其中ZMP在早期常用于判定雙足機器人在行走過程中的動態(tài)穩(wěn)定性[15]。在第二種方法中提出了建立機器人的全身動力學模型來解決穩(wěn)定問題。還有一些研究則提出了基于接觸力調(diào)節(jié)的平衡恢復算法，如文獻[16]設計的力-姿態(tài)反饋控制器(Force-Posture FeedbackCompensation，F(xiàn)PFC)，F(xiàn)PFC采用機器人足端的力傳感器和陀螺儀采集到的反饋信號來對機器人姿態(tài)進行調(diào)整。文獻[17-18]提出了基于動量的平衡控制器，通過約束優(yōu)化問題的定義，計算所允許的關節(jié)加速度，以獲得所需的角動量和線性動量。也有通過深度學習對四足機器人進行大量訓練與學習，從而實現(xiàn)無建?？刂芠19]。C Gehring等研究了在平坦地形下基于接觸力的四足機器人平衡控制算法[20]。

本文設計一個基于四足機器人全身動力學的平衡控制器，以每條腿的足端位置向量為入手點，通過實時獲得每條腿的反饋信號，調(diào)節(jié)主體加速度來維持機器人平衡，抵消機器人足端與障礙物產(chǎn)生的干擾；設計的平衡控制器具有一定的魯棒性，能實時調(diào)整補償值，提高四足機器人行走時的穩(wěn)定性，同時采用貝塞爾曲線對足部軌跡進行規(guī)劃。

1 運動學與動力學模型

本文使用的四足機器人由一個主體框架和四條腿組成，圖1為四足機器人模型，圖2為四足機器人簡化模型。本文設計的機器人每條腿都有三個自由度。

圖1 四足機器人模型

圖2 四足機器人簡化模型

圖2中RF、LF、RB和LB分別表示機器人右前腿、左前腿、右后腿和左后腿。

本文采用顯式動力學來推導機器人動力學模型[21]。如圖2所示，其系統(tǒng)運動相對于地面慣性參考系(XYZ)進行分析，取系統(tǒng)勢能為零。該系統(tǒng)的一般拉格朗日公式為

(1)

(2)

(3)

(4)

(5)

(6)

簡化得到

(7)

(8)

(9)

(10)

(11)

注意：T的表達式(7)用身體位置與方向固定的向量表示，必須對每一項使用適當?shù)淖儞Q矩陣來做式(1)中所需的微分。

廣義坐標向量q表示為

(12)

(13)

(14)

(15)

式中：δ0∈R6×1為主體位置和方向的矢量矩陣；θ∈Rn為關節(jié)位置向量；n為各腿關節(jié)個數(shù)。系統(tǒng)引入了代數(shù)約束，利用文獻[23]提出的自然正交補法可得到獨立運動方程組。該機器人運動學方程可表示為

(16)

設

(17)

2 步態(tài)和足部軌跡規(guī)劃

2.1 步態(tài)軌跡規(guī)劃

四足機器人的步態(tài)可分為靜態(tài)和動態(tài)兩種。

靜態(tài)步態(tài)：包含行走步態(tài)和爬行步態(tài)；機器人使用該步態(tài)時，在行走周期的任意時刻都保證有三條腿與地面接觸，其質(zhì)心的垂直投影始終保持在機器人支撐腿形成的多邊形內(nèi)。動態(tài)步態(tài)包括對角小跑、跳躍、踱步和奔跑；動態(tài)步態(tài)運動速度較快，且運動周期中任意時刻最多兩條腿與地面接觸，機器人質(zhì)心垂直投影沒有停留在機器人支撐腿形成的多邊形內(nèi)，但也能保持動態(tài)平衡。

當機器人處在平坦或不規(guī)則地形時，其穩(wěn)定性都非常重要。為驗證本文所設計的平衡控制器的穩(wěn)定性與魯棒性，采用靜態(tài)步態(tài)中的行走步態(tài)在仿真環(huán)境中進行測試。

行走步態(tài)：在一個周期內(nèi)任意時刻僅有一條腿抬起，并以0.25的相位進行輪流邁步；其序列圖如圖3所示。

圖3 行走步態(tài)序列圖

2.2 足部軌跡規(guī)劃

為保證機器人能穩(wěn)定行走，其足部軌跡應與動物行走軌跡類似，且足端落地時足部軌跡不能對機器人產(chǎn)生較大影響。本文采用貝塞爾曲線對足部運動軌跡進行規(guī)劃[24]。貝塞爾曲線方程為

(18)

式中：E為控制點；e為控制點個數(shù)；i為第i個控制點；t為曲線參數(shù)。

四足機器人軌跡規(guī)劃有多種，如3階軌跡規(guī)劃和正弦軌跡規(guī)劃，但由于這些軌跡規(guī)劃沒有考慮加速度，因此本文采用5階貝塞爾曲線方程，為

X(t)=E0(1-t)5+5E1(1-t)4t+10E2(1-t)3t2+10E3(1-t)2t3+5E4(1-t)t4+E5t5

(19)

由于機器人初始狀態(tài)為靜止，根據(jù)文獻[16]定義貝塞爾曲線初始狀態(tài)為

(20)

(21)

(22)

(23)

在式(20)中，X0為機器人腳在X方向上的起始位置；ΔX為腳在X方向上的步距。

將式(20)～(23)初始條件代入式(19)中，得到

X(t)=0.5+3560.41t3-54775.55t4+224720.2t5(0≤t≤0.0975)

(24)

同樣，可得到Z軸方向的曲線方程為

(25)

初始狀態(tài)為

(26)

(27)

(28)

(29)

將初始條件式(26)～(29)代入式(25)中，得到

(30)

在有些塞爾曲線步態(tài)方案上，允許用戶任意組合前進、側向與偏轉(zhuǎn)指令，且隨時間推移給出足端坐標。貝塞爾曲線控制足端運動示意圖如圖4所示。

圖4 足端運動示意圖

圖4中抬起高度表示機器人足端距離地面最遠距離；站立點距離為機器人足端運動一個周期在地面上相距最遠距離；站立高度為機器人中心距離地面最遠距離。

3 四足機器人平衡控制器

本文以機器人全身動力學模型為切入點設計平衡控制器。在以往研究中，通過逆運動學模型推導機器人身體位置，必須獲得機器人的關節(jié)力矩和足端接觸力。傳統(tǒng)的力傳感器噪聲較大，導致測量的接觸力誤差較大，于是通過運動學約束從動力學方程中消除接觸力[25-26]。

當機器人站立在環(huán)境中時，假設機器人保持靜止，初速度為0，運動時與地面存在摩擦力，可得到約束條件為

(31)

式中v為機器人在環(huán)境中保持站立腿的數(shù)量。此時，定義一個命名為β的獨立空間。

(32)

式中θSL為自由腿的關節(jié)角。整體結構與獨立空間β的關系可以表示為

(33)

式中S為映射矩陣。

(34)

其中

(35)

(36)

式中JL，i和Jb，i分別為第i條站立腿和主體雅克比矩陣。

(37)

式中XL，i表示第i條站立腿的足端位置。將式(33)改寫為驅(qū)動與欠驅(qū)動兩部分組合，為

(38)

式中：下標un表示欠驅(qū)動；u表示驅(qū)動。將式(38)進行時間微分，有

(39)

將動力學方程式(16)分解為驅(qū)動與欠驅(qū)動部分組合，則

(40)

(41)

式(40)、式(41)可寫成兩個獨立方程，為

(42)

(43)

(44)

式中：

(45)

(46)

從Sun和Su的定義可以得到

SunJun+SuJu=0

(47)

本文以四足機器人靜止在地面上為例證明式(47)，同樣在其他狀態(tài)下也可通過類似的步驟證明式(47)。

當機器人靜止在地面上時，四條腿都與地面接觸，可得到矩陣J為

(48)

得到Jun與Ju為

(49)

同樣可得Sun、Su為

(50)

將式(49)、(50)代入式(47)中得到

(51)

得到抵消接觸力后的運動學方程為

(52)

式(52)應以獨立空間表示，將式(39)代入式(52)得

(53)

此時的動力學方程已消去與接觸力有關的項，當外部干擾施加在機器人身上時，會使機器人在力作用的方向上有運動的趨勢并產(chǎn)生一個初始加速度，導致機器人偏離預期姿態(tài)，失去平衡。為抵消干擾所產(chǎn)生的加速度，應對機器人施加一個與之相反的加速度。期望加速度通過式(53)獲得，機器人運動時需要較大的位置誤差和誤差率進行動態(tài)調(diào)節(jié)，所以本文采用比例微分(PD)控制器計算所需加速度。

(54)

在遇到外界干擾時，平衡控制器得到的加速度通過逆運動學控制器反饋到機器人的各個關節(jié)力矩中，從而恢復機器人的平衡。其控制過程如圖5所示。

圖5 四足機器人控制平衡圖

4 仿真與結果

為驗證本文提出算法的有效性和優(yōu)越性，本文利用四足機器人分別在平坦(如圖6所示)和不規(guī)則地形(如圖7所示)進行測試。本次實驗的機器人由四條腿和一個主體構成，每條腿有3個自由度，整體自由度為18，機器人可以在環(huán)境中沿著坐標系所有坐標軸自由移動。表1為四足機器人各部位特性及控制器參數(shù)。

表1 四足機器人各部位特性及控制器參數(shù)

平坦地形：

本次實驗讓機器人沿著X軸行走，如圖6所示，機器人在行走中能夠非常穩(wěn)定地從A位置運動到C位置。

圖6 穿越平坦地形

通過圖6可知，本文選擇的步態(tài)和平衡控制器能夠很好滿足機器人在平坦地形下的行走。

復雜地形：

實驗設計了一個凹凸不平的地面，如圖7所示，讓機器人從A位置開始沿X軸行走至C位置，此時機器人未加平衡控制器。

圖7 無平衡控制器穿越不規(guī)則地形

通過圖7可以看到，在未加平衡控制器時，機器人并不能適應復雜地形，在前進到B位置幾欲跌倒，繼續(xù)行走一小段距離后機器人即已無法保持平衡。可見在不規(guī)則地形中，沒有平衡控制器的機器人無法適應地形，整個主體部分失去平衡，無法順利運動到指定地點。

將平衡控制器導入機器人中，重復圖7實驗，如圖8所示。

圖8 有平衡控制器機器人穿越不規(guī)則地形

由圖8可以看出，在加入平衡控制器后，機器人并沒有如圖7所示那樣失去平衡，反而克服了足端所受到的干擾，順利地從A位置運動到C位置。通過圖7、圖8對比可以看出，本文設計的平衡控制器能夠很好地幫助機器人在不均勻地形行走時維持自身平衡。實驗在同一環(huán)境下的不同路段進行多次，實驗過程中即使四足機器人足端受到各種不同方向的障礙干擾，平衡控制器均能通過獲得足端位置向量及時克服環(huán)境所帶來的干擾。實驗表明，該算法在外界干擾下，能及時調(diào)整主體位置恢復平衡，并具有一定程度的魯棒性。

機器人在平坦和不規(guī)則地形行走的策略輸出如圖9所示。

圖9 策略輸出

由圖9a可以看出，當機器人行走在平坦地形時，由于沒有受到外界干擾，其身體各部位的策略輸出呈一種周期不斷循環(huán)的狀態(tài)，所以機器人在行走過程中一直保持平衡狀態(tài)。由圖9b可以看出，當機器人行走在不規(guī)則地形中時，其足端一直受到干擾，機器人無法保持平衡；為使機器人保持平衡，平衡控制器不斷調(diào)節(jié)機器人四條腿的扭矩，導致其策略輸出不斷變化以維持機器人平衡。

5 結論

為實現(xiàn)四足機器人在不規(guī)則地形上保持穩(wěn)定行走，設計了一種基于四足機器人全身動力學的平衡控制器。平衡控制器通過消除機器人足部受到的接觸力，將行走時足端反饋的矢量信息通過動力學方程計算，得到平衡所需要的加速度；將加速度通過逆運動學控制反饋到機器人的關節(jié)力矩上，實現(xiàn)機器人的穩(wěn)定控制。在不同的環(huán)境下多次仿真表明，該平衡控制器能夠有效幫助四足機器人在不規(guī)則地形中行走時保持平衡。