羅 榮,錢樂天,羅 欣

(華中科技大學(xué)機械科學(xué)與工程學(xué)院,湖北 武漢 430074)

0 引言

現(xiàn)有的四足機器人爬階梯方法多為“顯爬”方式,即預(yù)先設(shè)定或采用視覺傳感器獲取階梯地形先驗信息,據(jù)此合理地規(guī)劃機身姿態(tài)和擺動腿足端軌跡[1-3]。但這種方式完全依賴于先驗地形信息,在某些場景下難以發(fā)揮作用,如有煙、霧、霾或是處于山洞、夜間等視覺傳感器難以獲取地形信息的環(huán)境。

“盲爬”作為“顯爬”方式的重要補充,逐漸得到研究者的重視[4-5]?！懊づ馈睕]有先驗地形知識,只能通過機器人內(nèi)部感受器和觸覺信息的融合后驗估計局部地形,據(jù)此調(diào)整機身姿態(tài)?！懊づ馈边^程中擺動腿不可避免地與階梯臺階發(fā)生磕碰。因此,機器人需要具備高動態(tài)穩(wěn)定性和意外磕碰事件處理能力。

在爬階梯運動規(guī)劃方面,現(xiàn)有爬階梯方法大多采用基于零力矩點的靜步態(tài)規(guī)劃方法[6-7],使零力矩點始終位于機器人支撐多邊形范圍內(nèi)。黃博等[8]利用運動空間需求最小、穩(wěn)定裕度最大和步態(tài)協(xié)調(diào)性最好等評價指標(biāo),對四足機器人的基本步態(tài)進(jìn)行了對比分析,提出了基于投影分析法結(jié)合平面靜平衡步態(tài)理論的階梯步態(tài)規(guī)劃方法。Lee等[9]在靜步態(tài)規(guī)劃基礎(chǔ)上,控制足地接觸力對質(zhì)心產(chǎn)生的角動量變化率最小,降低了機器人在階梯上傾斜的可能性。Ye等[10]分析并確定了機器人階梯上不同支撐腿配置情況下的最優(yōu)機身姿態(tài),設(shè)計了最優(yōu)姿態(tài)之間的轉(zhuǎn)換策略,以使機器人能夠安全地攀爬階梯?？傮w上看,由于基于零力矩點的靜態(tài)階梯運動規(guī)劃方法要求時時處處保持靜平衡,攀爬速度慢,動態(tài)穩(wěn)定性較差,尤其難以應(yīng)對“盲爬”方式存在的機器人腿與階梯臺階豎直面磕碰的問題。

本文設(shè)計了一種四足機器人采用對角小跑步態(tài)快速“盲爬”階梯策略。根據(jù)觸地腿實時估計階梯局部信息;基于貝塞爾曲線設(shè)計了針對階梯地形的擺動腿足端軌跡;基于廣義動量法設(shè)計了對于擺動腿足端磕碰階梯臺階豎直面的檢測方法,并對磕碰問題進(jìn)行了處理;基于穩(wěn)定裕度概念以及估計的階梯部分尺寸參數(shù)實時規(guī)劃機身的參考軌跡,采用模型預(yù)測算法對參考軌跡進(jìn)行跟蹤,保證機器人快速穩(wěn)定地“盲爬”階梯。通過實驗驗證了策略的正確性。

1 階梯信息估計

定義階梯每步臺階縱向方向尺寸為臺階深度d,豎直方向尺寸為臺階高度h。由于寬度方向與機器人攀爬方向垂直,故不需要考慮。定義機器人前后支撐腿足端連線與水平面形成的“虛擬坡度”角度為β,并視為階梯的“局部坡度”,如圖1所示。為便于描述,在水平面定義世界坐標(biāo)系{D},其ZD軸豎直向上。于機身質(zhì)心處定義機身坐標(biāo)系{B}原點,其XB軸與機身軀干平行,ZB軸垂直機體向上,YB軸符合右手定則。

設(shè)定機器人攀爬步態(tài)為對角小跑步態(tài),某時刻前后支撐腿分別位于上下級臺階時,β為

(1)

lt為前后支撐腿足端在世界坐標(biāo)系水平面投影距離;ht為豎直方向上的距離,作為估計的臺階高度。其表達(dá)式為:

lt=lcosθ+Δxbk+Δxfj

(2)

ht=Δzbk-Δh

(3)

l為機身長度尺寸;θ可通過慣性測量單元測得;j、k∈{L,R}為左、右;Δh為后支撐腿髖關(guān)節(jié)轉(zhuǎn)軸與前支撐腿足端在世界坐標(biāo)系豎直方向上的距離,即

Δh=Δzfj-lsinθ

(4)

Δxfj和Δxbk分別為前后支撐腿的髖關(guān)節(jié)轉(zhuǎn)軸到足端矢量lg在世界坐標(biāo)系x方向投影;Δzfj和Δzbk分別為相應(yīng)的z方向投影。lg可通過腿部運動學(xué)求出。

2 運動規(guī)劃

2.1 擺動腿規(guī)劃

2.1.1 落足點規(guī)劃

機器人擺動腿i期望落足點在世界坐標(biāo)系X-Y平面投影位置pi,e,x-y∈R2通過線性組合公式確定,即

(5)

pi,h,d,x-y=cx-y+pi,h,c,x-y

(6)

cx-y∈R2,為機身質(zhì)心實際位置在世界坐標(biāo)系X-Y平面的投影;pi,h,c,x-y∈R2,為擺動腿i髖關(guān)節(jié)相對于機身質(zhì)心位置在世界坐標(biāo)系X-Y平面的投影。

擺動腿i足端在世界坐標(biāo)系的期望擺動高度zi為

zi=zi,0+(ht+Δh)

(7)

zi,0為擺動腿i足端在世界坐標(biāo)系的實際高度;Δh為在估計的臺階高度ht基礎(chǔ)上增加的余量。

2.1.2 足端軌跡規(guī)劃

為減小足端與階梯臺階豎直面發(fā)生磕碰的概率,本文基于多控制點的貝塞爾曲線設(shè)計了一種針對階梯地形的足端軌跡,如圖2所示。

圖2 基于貝塞爾曲線的足端軌跡

圖2中x和z向的長度被歸一化處理,虛線為各控制點連線,共7個控制點,實線為生成的足端軌跡。n控制點的貝塞爾曲線P(t)可由以下遞推公式求出,即

(8)

其中,Bi,n(t)為

(9)

為減小擺動腿因提前觸地受到的地面反力對機器人穩(wěn)定性的影響,當(dāng)某腿檢測到足地接觸事件時,控制該腿不再進(jìn)行后續(xù)的擺動。

2.1.3 磕碰檢測與處理

如圖3所示為擺動腿足端磕碰階梯臺階豎直面。由于傳統(tǒng)的力估計方法噪聲大,本文基于廣義動量法[13]檢測足端磕碰事件。

圖3 擺動腿足端磕碰階梯臺階示意

關(guān)節(jié)開鏈機械臂與環(huán)境產(chǎn)生碰撞時,動力學(xué)為

(10)

(11)

(12)

r為觀測的外力矩值;K是增益對角矩陣;pest為滿足式(11)的動量估計值,結(jié)合式(11)、式(12)得

(13)

機器人單腿可視為多關(guān)節(jié)機械臂,由于碰撞沖擊對膝關(guān)節(jié)動量的影響最大,只考慮膝關(guān)節(jié)的動量變化,檢測算法離散形式為

(14)

若抬腳過程發(fā)生磕碰,將足端x向的期望位置pi,e,x設(shè)為當(dāng)前實際位置,當(dāng)足擺到期望的最高位置后,繼續(xù)跟隨原規(guī)劃點擺動;若落腳過程發(fā)生磕碰,將所有擺動腿足端在世界坐標(biāo)系x向的期望位置pe,x設(shè)為

pe,x=px+Δx

(15)

px為擺動腿當(dāng)前實際位置;Δx為調(diào)整值,以減小下一次擺動發(fā)生磕碰的概率。

2.2 機身規(guī)劃

穩(wěn)定裕度越大機器人越穩(wěn)定,穩(wěn)定裕度可以通過機身質(zhì)心位置離零力矩點(ZMP)的距離判定[15]。四足機器人以動步態(tài)如對角小跑步態(tài)運動時,穩(wěn)定裕度可視為機身質(zhì)心與1對支撐腿足端連線的距離。為使穩(wěn)定裕度最大,機身質(zhì)心的期望水平位置應(yīng)盡量靠近連線中點。另一方面,為了快速高效地攀爬階梯,機器人水平速度應(yīng)盡量維持在設(shè)定的恒定速度值附近。綜合以上2個需求,機身水平參考速度的計算描述為

(16)

(17)

|ct,x-y-(pf,e,x-y-pb,e,x-y)/2|≤δc

(18)

pf,e,x-y和pb,e,x-y分別為通過式(5)得出的前后支撐腿足端期望位置水平投影;δc為設(shè)定閾值。

機身質(zhì)心豎直方向期望位置參考前后支撐腿為

(19)

ce,z為質(zhì)心豎直方向期望位置;zc為根據(jù)機器人尺寸預(yù)設(shè)恒定值;pf,z和pb,z為前后支撐腿豎直方向?qū)嶋H位置。

對于臺階高度相比機器人尺寸不高的階梯,機身保持水平更有利于攀爬[16]。依據(jù)階梯“局部坡度”β和臺階高度ht,機身俯仰角期望值θe設(shè)為

(20)

h0為根據(jù)機器人尺寸設(shè)定的閾值。

為保證控制平滑性,豎直方向質(zhì)心位置和機身俯仰角軌跡基于三次多項式,即

x(t)=at3+bt2+ct+d

(21)

x為機身豎直方向位置或俯仰角,約束條件為

x(t0)=cz,0,x(tf)=cz,f

x(t)=x0+(xf-x0)(3-2t)t2

(22)

(23)

機身其余狀態(tài)的期望值設(shè)為定值。

3 運動控制

3.1 擺動腿控制

擺動腿通過帶前饋的PD算法控制,前饋項為

(24)

(25)

τs為各關(guān)節(jié)的期望力矩;Kp,fb和Kd,fb為增益矩陣。

3.2 機身控制

機身狀態(tài)采用模型預(yù)測控制[17]。機身狀態(tài)為

(26)

在坡度小于15°區(qū)域內(nèi)，農(nóng)村居民點斑塊面積與數(shù)量占比高達(dá)91.9%與86.8%。在小于5°區(qū)域農(nóng)村居民點密度、平均面積與標(biāo)準(zhǔn)差高出其他區(qū)域較多，而5°～15°區(qū)域內(nèi)斑塊數(shù)量與景觀形狀指數(shù)大于0°～5°區(qū)域，體現(xiàn)出0°～5°區(qū)域內(nèi)農(nóng)村居民點具有連片聚集大規(guī)模的特征，而5°～15°區(qū)域內(nèi)農(nóng)村居民點斑塊更加碎小、并具有離散分布特性。隨著坡度增大農(nóng)村居民點總面積、面積比例、數(shù)量等各項指數(shù)均減小，且在坡度大于25°時降至極低水平，表明坡度對農(nóng)村居民點分布與規(guī)模均具有極大的影響。農(nóng)村居民點形狀指數(shù)在0°～5°區(qū)域內(nèi)最高，表明區(qū)域內(nèi)農(nóng)村居民點形狀復(fù)雜，而其他區(qū)域差異不明顯。

(27)

(28)

(29)

fi∈R3為支撐腿i受到的地面反作用力;ri∈R3為從機身質(zhì)心到fi作用點的距離;m為機器人的質(zhì)量;g∈R3為重力加速度;I∈R3為機器人的慣性張量;R為從機身坐標(biāo)變換到世界坐標(biāo)的旋轉(zhuǎn)矩陣。定義[x]×∈R3×3為關(guān)于x的斜對稱矩陣,使得對于所有的x、y∈R3,有[x]×y=x×y∈R3。

對于爬階梯運動,橫滾角可視為0,但機身俯仰角為0的假設(shè)不成立。為此,相較文獻(xiàn)[16],本文在機器人動力學(xué)中考慮了機身俯仰角,有

(30)

對于角速度較小的運動,有

(31)

該模型的求解可轉(zhuǎn)化為以下模型預(yù)測控制問題,即:

(32)

xt+1=Atxt+Btutt=0,…,n-1

(33)

ut,min≤Ctut≤ut,maxt=0,…,n-1

(34)

Dtut=0t=0,…,n-1

(35)

xt為時刻t的機身狀態(tài);ut為時刻t的控制輸入(支撐腿足端接觸反力Ft,r);n為預(yù)測的步數(shù);Qt和Rt為權(quán)重的對角半正定矩陣;At和Bt為離散時間系統(tǒng)動力學(xué);ut,min、Ct和ut,max為摩擦錐約束;Dt為1個支撐腿選擇矩陣。力Ft,r經(jīng)坐標(biāo)變換和腿部雅可比計算得到支撐腿各關(guān)節(jié)期望扭矩τe,i。

3.3 整機控制框架

控制器系統(tǒng)框架如圖4所示。

圖4 機器人“盲爬”階梯控制器系統(tǒng)

反饋數(shù)據(jù)模塊處理各傳感器的反饋數(shù)據(jù)。狀態(tài)估計模塊根據(jù)狀態(tài)估計算法估計機身狀態(tài)信息。機身規(guī)劃模塊規(guī)劃機身狀態(tài)的參考軌跡,輸入模型預(yù)測控制模塊進(jìn)行計算。腿部控制器接收相應(yīng)腿足端的期望位置、速度和前饋力,經(jīng)逆運動學(xué)解算及PD控制算法計算各腿關(guān)節(jié)期望力矩τi,最終發(fā)送給關(guān)節(jié)驅(qū)動器。

4 仿真及實驗驗證

4.1 實驗系統(tǒng)搭建

實驗對象為宇樹科技的A1機器人,樣機和模型如圖5所示。機器人每條腿有3個由電機驅(qū)動的關(guān)節(jié)(側(cè)擺、髖、膝),每個足端有1個氣囊,可通過檢測氣壓的變化檢測足地接觸事件。機器人部分參數(shù)如表1所示。

表1 A1機器人基本參數(shù)

圖5 A1機器人模型

如圖6a所示,機身質(zhì)心理想高度hc為0.300 m,陰影部分為足端有效工作空間?！懊づ馈睍r擺動腿最大期望擺動高度z0須大于階梯臺階高度。實驗機器人髖關(guān)節(jié)結(jié)構(gòu)半徑rh為0.040 m,足直徑df為0.040 m,基于關(guān)節(jié)尺寸及關(guān)節(jié)限位,腿部實際全長為0.360 m,如圖6b所示。經(jīng)實際測試,最大安全抬腳高度為0.160 m。

圖6 A1機器人足端運動范圍

物理實驗平臺的硬件結(jié)構(gòu)如圖7所示。虛線框中為機器人內(nèi)部硬件,其中,以太網(wǎng)交換機用于機器人與外部PC交互數(shù)據(jù),底層控制器用于收集IMU反饋數(shù)據(jù)以及與關(guān)節(jié)電機驅(qū)動器通信?？刂扑惴ùa運行于處理器型號為Intel酷睿i7-9750H的筆記本電腦(PC)上的Linux系統(tǒng)。各數(shù)據(jù)通過Ethernet(UDP)以1 kHz的通信頻率在PC和機器人之間傳輸。

圖7 實驗平臺硬件結(jié)構(gòu)組成及關(guān)系

4.2 仿真實驗

仿真實驗平臺為Webots仿真軟件。對如表2所示的規(guī)則階梯Ⅰ和不規(guī)則階梯Ⅱ進(jìn)行了攀爬實驗。不規(guī)則階梯Ⅱ各級臺階尺寸如表3所示。仿真結(jié)果數(shù)據(jù)從機器人在平坦地面加速完成后開始記錄。

表2 仿真實驗的階梯信息

表3 階梯Ⅱ尺寸信息

爬階梯Ⅰ仿真實驗結(jié)果如圖8所示。階梯Ⅰ整體“坡度”近似為24°。機器人分別在第0.767 s(A點)開始踩上第1級臺階水平面,在第4.748 s(B點)爬上整個階梯。攀爬過程中機器人實時估計的階梯“局部坡度”與階梯整體“坡度”一致,且機身俯仰角能跟隨估計“坡度”調(diào)整變化以適應(yīng)階梯地形。攀爬速度設(shè)定值為0.55 m/s。實際平均速度為0.53 m/s。

圖8 爬階梯Ⅰ仿真實驗結(jié)果

爬階梯Ⅱ仿真實驗結(jié)果如圖9所示。機器人分別在第0.667 s(C點)開始踩上第1級臺階水平面,在第4.439 s(D點)爬上整個階梯。由于階梯Ⅱ各級臺階尺寸不規(guī)則,表現(xiàn)為攀爬過程中實時估計的階梯“局部坡度”在較大范圍內(nèi)變化。攀爬過程中速度設(shè)定值為0.52 m/s,實際平均速度值為0.50 m/s。

圖9 爬階梯Ⅱ仿真實驗結(jié)果

爬階梯Ⅰ實驗中,在t=3.298 s機器人右前擺動腿抬起時與階梯臺階豎直面發(fā)生磕碰,如圖8a所示。檢測到該事件后,“盲爬”運動規(guī)劃器將右前腿足端世界坐標(biāo)系x向的期望位置被設(shè)為當(dāng)前實際位置,當(dāng)足擺動到期望的最高位置后,x向繼續(xù)按原規(guī)劃擺動,足端最終落在上一級臺階水平面上,運動過程細(xì)節(jié)快照如圖10a所示。在t=3.626 s機器人左前擺動腿落下時與階梯臺階豎直面發(fā)生磕碰,如圖8a所示。檢測到該事件后,將足端在世界坐標(biāo)系x向的期望位置設(shè)為當(dāng)前位置往后1 cm,直到完全落下,轉(zhuǎn)為支撐相。運動過程細(xì)節(jié)快照如圖10b所示。在爬階梯Ⅱ仿真實驗中也出現(xiàn)了類似的情況,結(jié)果如同前一實驗情形。

圖10 爬階梯Ⅰ擺動腿發(fā)生磕碰后運動快照

4.3 物理平臺實驗

實驗基于表4和圖11所示的階梯Ⅲ和Ⅳ,其臺階高度分別達(dá)到了機器人腿部全長的29.17%和30.56%。

表4 物理平臺實驗的階梯信息

圖11 實驗階梯

實驗結(jié)果如圖12和圖13所示,圖中線纜為用于機器人和PC通信的網(wǎng)線。機器人分別在第1.100 s(E點)和1.000 s(I點)進(jìn)入2個階梯地形,第9.000 s(H點)和4.400 s(J點)爬上階梯頂平面。機器人前腳剛踏上階梯第1個臺階時,機身俯仰角立刻根據(jù)估計出的局部“坡度”進(jìn)行調(diào)整,而后續(xù)攀爬過程中機身俯仰角跟隨估計的“虛擬坡度”角度變化以適應(yīng)階梯地形。機器人爬階梯Ⅲ和Ⅳ的平均速度分別為0.50 m/s和0.60 m/s,相應(yīng)的無量綱速度Fr達(dá)0.27和0.32。

圖12 爬階梯Ⅲ實驗結(jié)果

圖13 爬階梯Ⅳ實驗結(jié)果

對于階梯Ⅲ,由于臺階深度較大,機器人在攀爬過程中存在4只腳都在同一個臺階水平面上的情況,如圖12a中圖片2所示。該時刻處于圖12b中F(左前與右后腿剛切換為支撐腿)和G(左前與右后腿支撐相結(jié)束)點之間,此時“局部坡度”為0,機身俯仰角也為0。當(dāng)遇到臺階時估計“坡度”和機身俯仰角會隨階梯臺階“坡度”相應(yīng)變化,最終表現(xiàn)為角度數(shù)據(jù)呈現(xiàn)周期性。對于階梯Ⅳ,攀爬過程中估計的階梯“坡度”和機身俯仰角能基本維持在階梯整體“坡度”值附近,如圖13b所示。

5 結(jié)束語

本文四足機器人快速“盲爬”階梯策略通過估計階梯局部信息,保證運動規(guī)劃的合理性,對擺動腿與階梯臺階的磕碰事件進(jìn)行了實時檢測及處理,對擺動腿和機身的運動進(jìn)行了規(guī)劃,使用模型預(yù)測算法對機身狀態(tài)進(jìn)行了控制。四足機器人可“盲爬”臺階高度相對腿較長的階梯,且攀爬速度較快。