常青, 韓寶玲, 喬志霞， 李茜

(1.天津商業(yè)大學(xué) 機(jī)械工程學(xué)院，天津 300134； 2.北京理工大學(xué) 機(jī)械與車輛學(xué)院 北京 100081)

由于具有較好的運(yùn)動靈活性和環(huán)境適應(yīng)性，四足機(jī)器人一直是仿生機(jī)器人領(lǐng)域的研究熱點(diǎn)[1-3]. 對于四足機(jī)器人的研究而言，運(yùn)動控制策略既是重點(diǎn)又是難點(diǎn)，特別是多足協(xié)調(diào)控制、環(huán)境適應(yīng)性控制和動態(tài)穩(wěn)定性控制[4]. 對于四足機(jī)器人來說，斜坡是一種具有挑戰(zhàn)性的地形. 由于斜坡傾斜角度的存在，機(jī)器人重力沿斜面切線方向的分力開始出現(xiàn)，使四足機(jī)器人更容易滑到. 同時(shí)，機(jī)器人質(zhì)心在斜坡上的投影與支撐足連線之間距離也與在平地上運(yùn)動時(shí)有很大不同，這使得機(jī)器人機(jī)體產(chǎn)生更大的翻轉(zhuǎn)力矩，這也是造成四足機(jī)器人在斜坡運(yùn)動不穩(wěn)定的重要原因之一.

近年來，關(guān)于四足機(jī)器人斜坡運(yùn)動規(guī)劃的研究主要集中在運(yùn)動學(xué)和動力學(xué)規(guī)劃角度. 張秀麗[4]通過調(diào)整前后腿部的伸展長度來控制保持水平位置，從而減小運(yùn)動過程中質(zhì)心投影與支撐足連線之間的位置偏差. 孟健等[5]通過引入NESM(normalized energy stability margin)判據(jù)計(jì)算內(nèi)外傾的穩(wěn)定裕度來對質(zhì)心位置進(jìn)行調(diào)整的. 韓寶玲等[6]通過坡面上機(jī)器人質(zhì)心的投影與支撐線之間的距離通過使用復(fù)合擺線方程對足端軌跡的規(guī)劃來得到對斜坡沖擊較小的運(yùn)動步態(tài)從而提高四足機(jī)器人在斜坡運(yùn)動的穩(wěn)定性. 本文作者在文獻(xiàn)[7]中提出一種改進(jìn)型的對角小跑步態(tài)通過在擺動相和支撐相之間加入四足同時(shí)著地的姿態(tài)調(diào)整相來提高穩(wěn)定性. 馬宗利等[8]采用調(diào)整前腿長度，盡量保證機(jī)體姿態(tài)水平的方法實(shí)現(xiàn)了斜坡上的穩(wěn)定行走. Focchi等[9]通過基于QP力分解控制方法實(shí)現(xiàn)了四足機(jī)器人在斜坡上的行走步態(tài)的規(guī)劃. 在以上四足機(jī)器人斜坡運(yùn)動規(guī)劃方法的研究中，往往需要精確知道坡面角度參數(shù)以及機(jī)器人尺寸參數(shù)，通過幾何建?；蜻\(yùn)動學(xué)建模來實(shí)現(xiàn)對運(yùn)動的精確調(diào)整. 以上方法雖然解決了四足機(jī)器人在斜坡行走的基本問題，但在實(shí)際場景應(yīng)用中具有很大的局限性. 本文中提出了一種四足機(jī)器人對角小跑步態(tài)下的坡面運(yùn)動自適應(yīng)控制算法. 在機(jī)器人質(zhì)心原有運(yùn)動軌跡上加入橫向和縱向偏移補(bǔ)償量，并采用符號微分策略梯度法對偏移補(bǔ)償量進(jìn)行自動調(diào)整，以減少機(jī)體翻轉(zhuǎn)力矩和對角足著地時(shí)間差，從而提高機(jī)器人運(yùn)動穩(wěn)定性. 文中所提方法不依賴于四足機(jī)器人運(yùn)動學(xué)參數(shù)及斜坡角度參數(shù)，能夠自動調(diào)整機(jī)器人質(zhì)心偏移補(bǔ)償量，實(shí)現(xiàn)四足機(jī)器人在斜坡上穩(wěn)定運(yùn)動.

1 四足機(jī)器人運(yùn)動模型

本文的主要研究對象為一款液壓四足機(jī)器人. 該四足機(jī)器人采用前肘后膝式關(guān)節(jié)配置，每條腿都由3個(gè)液壓缸分別驅(qū)動膝關(guān)節(jié)、踝關(guān)節(jié)及髖關(guān)節(jié)，其運(yùn)動學(xué)模型如圖1所示. 本文作者在文獻(xiàn)[10]中對該四足機(jī)器人的運(yùn)動學(xué)參數(shù)進(jìn)行了詳細(xì)定義，并提出將四足機(jī)器人的全向運(yùn)動分解成轉(zhuǎn)向運(yùn)動以及斜向運(yùn)動，并用齊次變換矩陣來描述相鄰運(yùn)動周期四足機(jī)器人足端起始位置的變化情況.

如圖2為四足機(jī)器人一個(gè)斜向運(yùn)動周期中的示意圖，圖中坐標(biāo)系{W}為世界坐標(biāo)系，坐標(biāo)系{B}為機(jī)器人機(jī)體質(zhì)心坐標(biāo)系，坐標(biāo)系{B}’為機(jī)器人斜向運(yùn)動周期結(jié)束時(shí)機(jī)體中心坐標(biāo)系所在位置.

圖1 四足機(jī)器人運(yùn)動學(xué)模型Fig.1 Kinematic model of quadruped robot

圖2 四足機(jī)器人斜向運(yùn)動示意圖Fig.2 Sketch of the oblique motion

斜向運(yùn)動中{B}’相對于{W}的齊次變換參數(shù)化矩陣Gc為

(1)

式中:λ為斜向步長;φ為斜向角;T為一個(gè)運(yùn)動周期的時(shí)間;t為運(yùn)動的時(shí)間. 值得注意的是當(dāng)斜向角φ=0時(shí)則此矩陣表示的是四足機(jī)器人的直線運(yùn)動. 根據(jù)式(1)求出一個(gè)斜向運(yùn)動周期中，機(jī)體坐標(biāo)系{B}相對于世界坐標(biāo)系{W}的位姿，多個(gè)運(yùn)動周期連續(xù)疊加就可以表示四足機(jī)器人的斜向運(yùn)動. 至于運(yùn)動過程的詳細(xì)規(guī)劃方法以及關(guān)節(jié)角的求解方法將直接參考文獻(xiàn)[10]中所用方法.

2 四足機(jī)器人質(zhì)心偏移補(bǔ)償量

自然界中具有脊椎的動物往往具有更為優(yōu)異的運(yùn)動性能，其脊椎的變形對于動物的快速穩(wěn)定運(yùn)動具有重要意義，如圖3所示，大多數(shù)四足動物的脊椎在運(yùn)動過程中會產(chǎn)生周期性的橫向和縱向移動，這種脊椎的運(yùn)動可以改善機(jī)體0力矩點(diǎn)(zero moment point ZMP)與支撐足連線之間的距離，從而降低對機(jī)體運(yùn)動穩(wěn)定性產(chǎn)生的不利影響[11].

圖3 馬在運(yùn)動過程中質(zhì)心的位移變動情況 (改編自文獻(xiàn)[11]的Fig.3)Fig.3 COM displacement of horse (revised from Fig.3 of reference 11)

四足動物的以上運(yùn)動特性為提高四足機(jī)器人運(yùn)動穩(wěn)定性提供了全新的思路. 四足機(jī)器人運(yùn)動規(guī)劃過程中，在機(jī)體質(zhì)心原有位移基礎(chǔ)上加入合理的縱向偏移補(bǔ)償量Xd以及橫向偏移補(bǔ)償量Zd，能夠提高機(jī)器人運(yùn)動的穩(wěn)定性. 由圖3可知，馬在一個(gè)運(yùn)動周期內(nèi)，身體質(zhì)心在3個(gè)方向的位移偏移量呈現(xiàn)出了類似三角函數(shù)的周期性變化. 為此，文中Xd和Zd將采用周期性三角函數(shù)的形式，為

(2)

式中:de1和de2分別為機(jī)體質(zhì)心偏移補(bǔ)償量Xd和Zd所對應(yīng)三角函數(shù)的系數(shù);T為運(yùn)動周期;t為運(yùn)動當(dāng)前運(yùn)動時(shí)間. 將Xd和Zd引入到式(1)中就可以得到含有質(zhì)心偏移補(bǔ)償量的位移變換矩陣，為

0≤t≤T.

(3)

使用上式進(jìn)行運(yùn)動規(guī)劃的難點(diǎn)在于如何確定de1和de2的值. 對于四足動物來說，其偏移量的獲得往往與自身學(xué)習(xí)有很大關(guān)系，而對于不同尺寸結(jié)構(gòu)的動物來說，偏移量的值也不大相同. 四足動物的質(zhì)心偏移量雖然不能直接用于指導(dǎo)機(jī)器人的質(zhì)心偏移補(bǔ)償量，但其學(xué)習(xí)的過程值得借鑒. 在本文中將采用符號微分策略梯度法來對de1和de2進(jìn)行求解.

3 使用PGSD求解質(zhì)心偏移補(bǔ)償量

PGSD是一種用于解決強(qiáng)化學(xué)習(xí)中模型不精確問題的優(yōu)化算法[12]. 馬爾科夫決策過程(Markov decision process，MDP)是強(qiáng)化學(xué)習(xí)最常見的表達(dá)形成，其由包含5個(gè)元素的集合(S,A,{Psa},γ,R)表示. 其中S為狀態(tài)變量集合，A為動作變量集合，Psa為狀態(tài)s的情況下如果進(jìn)行動作a，下一狀態(tài)的概率分布矩陣，γ∈[0,1] 為貼現(xiàn)因子，表示時(shí)間的遞減效應(yīng)，R為獎勵函數(shù). 在狀態(tài)s0∈S采取動作a0∈A，則后續(xù)狀態(tài)s1的概率分布服從概率矩陣Ps0a0. 選擇s1的任一狀態(tài)，并選擇動作a1，則可以得到s2的概率分布服從概率矩陣Ps1a1，此過程一直持續(xù)到所有動作完成，則最終獲得的回報(bào)函數(shù)可以用下式表示為

R(s0,a0)+γR(s1,a1)+γ2R(s2,a2)+….

(4)

策略π：S→A是從狀態(tài)到動作的映射函數(shù). 如式(5)所示為常見的策略函數(shù)：

π(si;θ)=KiΦ(si),

(5)

式中:Φ(si)為特征向量函數(shù);Ki為策略函數(shù)系數(shù);θ=[K1K2…Ki]T為策略參數(shù)系數(shù)矩陣. 在一般的MDP問題中，往往將γ設(shè)定為1，對于一階MDP來說，則有si+1=f(si,ai). 在策略函數(shù)π(si;θ)確定后，回報(bào)函數(shù)的期望所對應(yīng)的值函數(shù)由式(6)確定.

(6)

(7)

(8)

其中θJπ(s)為策略梯度項(xiàng). 由于獎勵函數(shù)R(si,ai)及策略函數(shù)π(si;θ)均為已知，式(8)中R(si,ai)以及π(si;θ)的偏導(dǎo)數(shù)可以直接求出. 對于si+1=f(si,ai)具有明確函數(shù)關(guān)系的模型來說，?si/?ai也比較容易確定，而當(dāng)si+1=f(si,ai)沒有明確的函數(shù)表達(dá)式時(shí)?si/?ai很難直接求得. 為了解決此問題，PGSD法中通過表達(dá)si和ai相互變化趨勢的符號微分矩陣M來近似代替?si/?ai，從而得到近似最優(yōu)解.M中的元素由1、-1和0組成，分別表示si對ai具有促進(jìn)作用、阻礙作用或沒有影響. 對于si∈m，ai∈n，且si+1=f(si,ai)不具有明確函數(shù)表達(dá)式的MDP模型，PGSD中的輸入?yún)?shù)包括符號微分矩陣M∈m×n,策略梯度策略函數(shù)G∈m×n，π∈n×k→m，獎勵函數(shù)R∈n×m→,策略參數(shù)系數(shù)矩陣θ∈k，?π(si;θ)/?θ的和函數(shù)ψi∈m×k優(yōu)化次數(shù)H∈+,以及學(xué)習(xí)率σ∈+. 具體的計(jì)算過程如下(PGSD算法的詳細(xì)推導(dǎo)及分析請查閱參考文獻(xiàn)[12]).

① 初始化，令策略梯度G以及ψi為0.

② 在優(yōu)化次數(shù)H內(nèi)對以下參數(shù)進(jìn)行更新，其中i=0,1,…,H.

? 在狀態(tài)si時(shí)，選擇并執(zhí)行動作si=π(si;θ).

? 更新策略梯度:

更新ψi：

③ 更新策略參數(shù)系數(shù)矩陣:

θ←θ-σGT.

下面將de1和de2的求解問題轉(zhuǎn)化為一階MDP問題，并采用PGSD進(jìn)行求解. 首先確定狀態(tài)變量以及動作變量. 但四足機(jī)器人以對角小跑步態(tài)在斜坡上運(yùn)動時(shí)，對角足著地時(shí)間差越小則機(jī)器人的運(yùn)動過程中越容易保持穩(wěn)定，對角足著地時(shí)間差越大，則機(jī)體保持穩(wěn)定越困難. 令四足機(jī)器人腿部著地時(shí)間分別為td1、td2、td3、td4，四足機(jī)器人對角足著地時(shí)間差可以用tc1=td1-td4以及tc2=td2-td3來表示. 這兩個(gè)量在一定程度上能夠反映四足機(jī)器人運(yùn)動平穩(wěn)狀態(tài)，因而s=[tc1tc2]T可以作為狀態(tài)變量.de1以及de2會對s的變化產(chǎn)生重要影響. 當(dāng)de1增大時(shí)，L1和L2著地時(shí)間提前，L3和L4著地時(shí)間延后，tc1和tc2的值都將變?。划?dāng)de2增大時(shí)，L1和L3會提前著地，L2和L4會延后著地，tc1值將變小，tc2值將變大. 將de1和de2的變動量Δde1和Δde2作為動作變量，即a=[Δde1Δde2]T.

si+1主要受si和ai的影響的，即si+1=f(si,ai). 但是由于機(jī)體姿態(tài)、地形條件等因素也對si+1會產(chǎn)生影響，因而f(si,ai)是具有不確定表達(dá)式的函數(shù). Δde1的變化趨勢與tc1和tc2相反，Δde2的變化趨勢與tc1相反，與tc2相同，因而可以令θ=[de1de1;de2-de2]，式(6)控制策略就可以用下式來表達(dá).

(9)

在斜坡上運(yùn)動時(shí)，對角足著地時(shí)間差值的平方和越小，則四足機(jī)器人運(yùn)動得就越穩(wěn)定. 可以將回報(bào)函數(shù)設(shè)為對角足著地時(shí)間差的平方和，如下所示：

R(s,π(s))=(tc1)2+(tc2)2.

(10)

得到一階MDP中的回報(bào)函數(shù)后，接下來采用PGSD進(jìn)行求解. 微分值?si/?ai的符號可以根據(jù)當(dāng)前動作向量a對狀態(tài)向量的s的作用來決定. Δde1對tc1和tc2有抑制作用，Δde2對tc1和tc2分別有抑制和促進(jìn)的作用，用微分符號矩陣M=[-1 -1; -1 1]來代替?si/?ai.

確定好PGSD中各參數(shù)后. 就可以利用PGSD法進(jìn)行相關(guān)的優(yōu)化求解了. 在優(yōu)化過程中策略參數(shù)所組成的系數(shù)矩陣 [de1de1;de2-de2]向著減少值函數(shù)值的方向收斂，Δde1和Δde2也不斷作用以調(diào)整de1和de2的值，優(yōu)化策略參數(shù)系數(shù)矩陣中所對應(yīng)的de1和de2就是對應(yīng)運(yùn)動規(guī)劃中所需要的機(jī)體質(zhì)心偏移補(bǔ)償量.

4 仿真實(shí)驗(yàn)與分析

在本小節(jié)中，將本文所提算法應(yīng)用于四足機(jī)器人的斜坡運(yùn)動規(guī)劃中. 首先在Webots機(jī)器人專用仿真軟件中搭建仿真環(huán)境，如圖4所示. 仿真環(huán)境中四足機(jī)器人為圖1所示的液壓四足機(jī)器人簡化模型，斜坡共含有4個(gè)連續(xù)坡面，坡面的角度分別為8°，24°，-20°以及8°，坡面在水平面上的投影長度分別為1.5°，0.75，0.5°，2.00 m.

圖4 四足機(jī)器人運(yùn)動仿真環(huán)境Fig.4 Simulation environment of quadruped locomotion

多坡面斜坡的直線運(yùn)動的仿真共分兩組，其中第1組仿真使用本論文所提出的自適應(yīng)規(guī)劃算法，第2組仿真中使用文獻(xiàn)[6]所提的斜坡運(yùn)動算法作為對比. 仿真中四足機(jī)器人按照圖4所示方向進(jìn)行直線運(yùn)動，仿真時(shí)間為20 s. 兩組仿真所使用的算法中四足機(jī)器人的步長均為140 mm,抬腿高度均為80 mm，運(yùn)動周期均為0.5 s，平均規(guī)劃速度為280 mm/s.

第1組仿真中，采用本文所提算法，學(xué)習(xí)率σ設(shè)為20. 由于仿真中地形為具有多個(gè)不同斜度的多坡面斜坡，四足機(jī)器人在運(yùn)動到不同坡面上時(shí)需要不同質(zhì)心偏移補(bǔ)償量進(jìn)行調(diào)整. 根據(jù)時(shí)間將仿真中四足機(jī)器人的運(yùn)動劃分為10個(gè)階段，每個(gè)運(yùn)動階段持續(xù)2 s，包含4個(gè)運(yùn)動周期，對應(yīng)不同的地形特性. 為了提高四足機(jī)器人在不同運(yùn)動階段的地形適應(yīng)性，需要10組質(zhì)心偏移補(bǔ)償量de1和de2. 為了得到這10組de1和de2的值，將算法優(yōu)化次數(shù)H設(shè)為4，那么四足機(jī)器人每隔4個(gè)運(yùn)動周期(2 s)就會根據(jù)上一運(yùn)動階段的運(yùn)動情況，對θ=[de1de1;de2-de2]進(jìn)行更新，得到下一運(yùn)動階段4個(gè)運(yùn)動周期內(nèi)de1和de2所對應(yīng)的值. 在0～2 s運(yùn)動階段內(nèi)由于沒有先前運(yùn)動信息，將de1和de2均設(shè)為0. 真中其它運(yùn)動階段得到的de1和de2值如表1所示. 由表1可知，在第1組仿真中，de1和de2一直在根據(jù)運(yùn)動狀態(tài)進(jìn)行調(diào)整.de2的值比de1稍大證明在仿真中機(jī)器人質(zhì)心橫向的位移調(diào)整對提高機(jī)器人機(jī)體穩(wěn)定性更重要.

表1 第1組仿真中de1和de2的值Tab.1 Values of de1 and de2 in simulation 1

在第2組仿真中，采用文獻(xiàn)[6]中算法進(jìn)行對比，由于算法需要設(shè)定一個(gè)斜坡角度，這里將斜坡角度設(shè)定為面積最大的坡面1及坡面4所對應(yīng)的坡面角度8°，運(yùn)動過程中不再進(jìn)行調(diào)整. 兩組仿真過程的視頻在補(bǔ)充文件中.

如圖5所示為20 s的仿真時(shí)間內(nèi)仿真中四足機(jī)器人質(zhì)心運(yùn)動軌跡. 第1組仿真中機(jī)器人分別在X方向上和Y方向上運(yùn)動了3.92 m以及0.46 m, 總位移為3.95 m. 第2組仿真中機(jī)器人分別在X方向和Y方向上運(yùn)動了2.57 m及0.28 m，總位移為2.68 m. 根據(jù)斜坡各坡面的尺寸參數(shù)以及機(jī)體人機(jī)體結(jié)構(gòu)參數(shù)可知第1組仿真中的機(jī)器人已經(jīng)進(jìn)入了坡面4，而第2組仿真中的機(jī)器人還沒有完全跨過坡面2和坡面3形成的A型坡. 第1組仿真中機(jī)器人平均速度為197.5 mm/s, 為平均規(guī)劃速度的70.5%，而第2組仿真中機(jī)器人平均速度為134 mm/s,僅為平均速度的47.9%. 在代表運(yùn)動偏差的Z方向位移上，四足機(jī)器人在第2組仿真中產(chǎn)生了0.22 m的偏差，而第1組仿真中僅產(chǎn)生0.029 m的偏差. 綜上可知，采用文中所提算法四足機(jī)器人能夠在保持較高速度的情況下穿越含有多個(gè)斜坡的坡面，同時(shí)運(yùn)動偏移位移也很小. 而作為當(dāng)前斜坡運(yùn)動規(guī)劃算法典型代表的文獻(xiàn)[6]則由于無法及時(shí)對與坡面角度相關(guān)的控制算法進(jìn)行及時(shí)的調(diào)整而很難穿越多坡面斜坡，平均速度較低，同時(shí)運(yùn)動偏移很大.

如圖6所示為仿真中四足機(jī)器人機(jī)體姿態(tài)角的變化. 四足機(jī)器人機(jī)體的俯仰角變化能夠體現(xiàn)坡度的變化，第1組仿真中俯仰角的曲線變化基本符合仿真中四個(gè)坡面坡度的變化情況，機(jī)器人完全穿越了四個(gè)坡面. 而第2組仿真中四足機(jī)器人則只經(jīng)過了坡面1和坡面2. 機(jī)器人機(jī)體橫滾角代表的是運(yùn)動的穩(wěn)定情況，從圖中可以看到仿真1中橫滾角在0° 附近做小幅振動，最大角度變動量為6.3°，而仿真2中橫滾角的變化范圍較大，最大角度變動量為13.2°. 航向角代表的是機(jī)器人直線行走的角度偏移情況，從圖中可以看出仿真1中航向角最大偏移角度為1.1°，而仿真2中航向角最大偏移角度為14.1°，因此四足機(jī)器人使用文中所提算法在斜坡上運(yùn)動更加穩(wěn)定.

圖5 仿真中四足機(jī)器人質(zhì)心位移Fig.5 COM displacement of quadruped robot in simulation

圖6 仿真中四足機(jī)器人機(jī)體姿態(tài)角Fig.6 Attitude angle of quadruped robot in simulation

5 樣機(jī)實(shí)驗(yàn)

在樣機(jī)試驗(yàn)中，圖1所示的液壓四足機(jī)器人將在斜坡角度為20°的斜坡上進(jìn)行行走實(shí)驗(yàn). 樣機(jī)中使用的控制算法與仿真實(shí)驗(yàn)中第1組仿真中的算法一致，步長為140 mm，抬腿高度為80 mm，運(yùn)動周期為0.5 s. 采用本文所提算法，四足機(jī)器人樣機(jī)能夠在斜坡上實(shí)現(xiàn)快速穩(wěn)定行走. 需要值得注意的是與以往算法不同，本文所提算法并沒有引入斜坡角度參數(shù)，通過自身調(diào)整完成了斜坡上的直線行走.

6 結(jié) 論

為了實(shí)現(xiàn)四足機(jī)器人以對角小跑步態(tài)在斜坡上穩(wěn)定運(yùn)動，受四足動物的啟發(fā)，本文提出一種基于PGSD的自適應(yīng)控制算法，并通過仿真分析驗(yàn)證所提方法的可行性和有效性. 和傳統(tǒng)的四足機(jī)器人斜坡運(yùn)動規(guī)劃方法相比，本文所提的方法在四足機(jī)器人運(yùn)動規(guī)劃過程中增加了機(jī)體質(zhì)心橫向和縱向偏移補(bǔ)償量，并通過PGSD對偏移補(bǔ)償量進(jìn)行自動調(diào)整來減少機(jī)器人翻轉(zhuǎn)力矩，提高機(jī)器人運(yùn)動穩(wěn)定性.

在本文中，四足機(jī)器人實(shí)物樣機(jī)只在含有單個(gè)坡面的斜坡上進(jìn)行了驗(yàn)證. 在后續(xù)研究中，將會對實(shí)物樣機(jī)在多坡面斜坡上的運(yùn)動進(jìn)行驗(yàn)證. 同時(shí)如何在算法中減小斜坡運(yùn)動過程中足底打滑現(xiàn)象對運(yùn)動的影響將是未來的研究目標(biāo).