并聯(lián)五連桿點(diǎn)足式雙足機(jī)器人穩(wěn)定行走控制方法

孫雙園,宋文杰,劉洧鋮,錢義肇,付夢印

（北京理工大學(xué)自動化學(xué)院,北京 100081）

0 引言

雙足機(jī)器人作為一種典型的地面移動平臺,具有與人類相似的運(yùn)動結(jié)構(gòu),能夠代替人類完成危險任務(wù),其研究近年來受到越來越多學(xué)者的關(guān)注。雙足機(jī)器人最早的研究可追溯到20世紀(jì)80年代,日本早稻田大學(xué)開發(fā)了WABOT 系列機(jī)器人[1]。在此之后,日本本田公司開發(fā)的ASIMO 機(jī)器人[2-3]以及Trade and Industry 公司的HRP[4-5]系列機(jī)器人將雙足機(jī)器人推向更加輕量化、人形化階段。當(dāng)前,最為知名和出色的雙足機(jī)器人當(dāng)屬波士頓動力公司（Boston Dynamics）的Atlas[6]和Agility Robotics 公司的Cassie[7],前者出色的運(yùn)動能力使其能夠執(zhí)行復(fù)雜動作,如后空翻和跑酷;后者則是一款用于服務(wù)的商用機(jī)器人,擁有出色的行走穩(wěn)定性[8]。

由于其自由度需求,機(jī)器人通常需要多個關(guān)節(jié)驅(qū)動器以達(dá)到空間目標(biāo)位姿。根據(jù)驅(qū)動器與關(guān)節(jié)數(shù)量之間的關(guān)系,可將機(jī)器人分為完全驅(qū)動和欠驅(qū)動兩種。在雙足機(jī)器人領(lǐng)域中,傳統(tǒng)機(jī)器人利用穩(wěn)定軌跡約束實(shí)現(xiàn)靜態(tài)行走,行走速度低、能耗大;而欠驅(qū)動機(jī)器人能夠利用自身重力和慣性實(shí)現(xiàn)動態(tài)行走,擁有更高的能量利用效率[9]。欠驅(qū)動雙足機(jī)器人主要有點(diǎn)足式機(jī)器人和被動腳踝式機(jī)器人。Pratt 等[10]使用虛擬模型控制（Virtual Model Control,VMC） 方法在點(diǎn)足式機(jī)器人Spring Turkey 上實(shí)現(xiàn)二維行走,但由于缺少腳踝,行走穩(wěn)定性依賴初始條件及虛擬約束參數(shù)等。Grizzle等[11-13]應(yīng)用虛擬約束（Virtual Constraints,VC） 和混合零動力學(xué)（Hybrid Zero Dynamics,HZD） 方法將Rabbit 機(jī)器人視為五連桿結(jié)構(gòu),行走過程被建模為混合動力系統(tǒng),在實(shí)機(jī)上實(shí)現(xiàn)了二維行走與奔跑。此后,MABEL、MARLO 機(jī)器人先后被研制。其中,MABEL 應(yīng)用HZD 方法可以實(shí)現(xiàn)平面內(nèi)動態(tài)行走[14];而MARLO 腿部為四邊形并聯(lián)結(jié)構(gòu),質(zhì)量集中在機(jī)身部分,可以簡化為線性倒立擺（Linear Inverted Pendulum,LIP）模型[15],通過應(yīng)用步態(tài)庫實(shí)現(xiàn)了三維空間中的穩(wěn)定行走[16]。

欠驅(qū)動點(diǎn)足式雙足機(jī)器人的研究難點(diǎn)在于:1）機(jī)身的大慣性對機(jī)器人行走穩(wěn)定性造成干擾,因此需要避免機(jī)器人機(jī)身在運(yùn)動過程中前后側(cè)翻;2）被動式腳踝無法為身體提供扭矩,因此需要通過持續(xù)切換支撐腿維持機(jī)體穩(wěn)定。本文基于一種并聯(lián)五連桿點(diǎn)足式雙足機(jī)器人,首先給出了五連桿結(jié)構(gòu)的運(yùn)動學(xué)模型。其次,基于LIP 模型設(shè)計了一套穩(wěn)定控制方法,在單腿支撐階段采用PD 反饋控制對機(jī)器人質(zhì)心軌跡進(jìn)行跟蹤,在雙腿切換階段采用軌道能量方法計算捕獲點(diǎn)即穩(wěn)定落腳點(diǎn)位置,對機(jī)器人行走實(shí)現(xiàn)閉環(huán)控制。最后,結(jié)合實(shí)物模型,在Matlab 中實(shí)現(xiàn)了二維空間內(nèi)的穩(wěn)定性行走實(shí)驗(yàn),并展示了行走過程中質(zhì)心和控制關(guān)節(jié)角度的相軌跡圖,證明了控制方法的有效性。

1 五連桿點(diǎn)足式雙足機(jī)器人結(jié)構(gòu)設(shè)計

雙足機(jī)器人腿部結(jié)構(gòu)決定了其運(yùn)動性能,結(jié)構(gòu)的靜態(tài)和動態(tài)性能指標(biāo)對機(jī)器人控制具有決定性作用。現(xiàn)有雙足機(jī)器人如Atlas[6],大都是仿人體設(shè)計的串聯(lián)腿部結(jié)構(gòu),負(fù)載能力較差。本文研究設(shè)計了一種并聯(lián)五連桿腿部結(jié)構(gòu),并在腿部加入彈簧阻尼器以增加系統(tǒng)柔順性,具體結(jié)構(gòu)如圖1所示,主要由機(jī)身、髖關(guān)節(jié)電機(jī)組、大腿桿件組、小腿桿件組等組成。當(dāng)機(jī)器人運(yùn)動時,髖關(guān)節(jié)電機(jī)組中橫滾電機(jī)為機(jī)器人腿部提供橫滾自由度,俯仰電機(jī)組為腿部提供俯仰自由度,五連桿機(jī)構(gòu)實(shí)現(xiàn)力及運(yùn)動的傳遞。

圖1 并聯(lián)五連桿機(jī)器人結(jié)構(gòu)示意圖Fig.1 Structure diagram of parallel five-link robot

該雙足機(jī)器人相較于其他結(jié)構(gòu)具有以下優(yōu)點(diǎn):

1）負(fù)載能力強(qiáng): 五連桿并聯(lián)結(jié)構(gòu)相較于串聯(lián)結(jié)構(gòu)具有高負(fù)載能力,單腿最大載重可達(dá)30kg,整機(jī)最大載重可達(dá)70kg,預(yù)期搭載一名成年人或多種設(shè)備與物資,以滿足伴隨保障、物資運(yùn)輸?shù)刃枨蟆?/p>

2）足端慣量低: 機(jī)器人大部分質(zhì)量集中于上部機(jī)身,足端慣量低,如圖2所示。在行走過程中降低足端對身體姿態(tài)的干擾,并且使實(shí)物更加貼近簡化的數(shù)學(xué)模型,降低控制難度。

3）能量效率高: 采用五連桿對稱結(jié)構(gòu),力矩傳遞指數(shù)高,欠驅(qū)動機(jī)器人能夠利用自身重力和慣性實(shí)現(xiàn)動態(tài)行走,擁有更高的能量利用效率。

2 正逆運(yùn)動學(xué)模型解算

機(jī)器人的運(yùn)動學(xué)分析是進(jìn)行機(jī)器人行走控制的基礎(chǔ)。為了研究足端位置與輸入電機(jī)角度之間的關(guān)系,本節(jié)分析了非同軸五連桿結(jié)構(gòu)的運(yùn)動學(xué)模型。首先給出了簡化串聯(lián)機(jī)構(gòu)模型的變換矩陣,然后通過五桿閉鏈系統(tǒng)推導(dǎo)出角度映射關(guān)系,得到該模型的正逆運(yùn)動學(xué)方程。

2.1 正運(yùn)動學(xué)建模與分析

由于并聯(lián)結(jié)構(gòu)運(yùn)動學(xué)難以分析,且很難找到所有并聯(lián)結(jié)構(gòu)的奇異位形,因此本文將五連桿結(jié)構(gòu)簡化為2 自由度串聯(lián)結(jié)構(gòu),如圖3所示。采用D-H 法則對該串聯(lián)結(jié)構(gòu)進(jìn)行運(yùn)動學(xué)分析。

圖3（a）所示的并聯(lián)結(jié)構(gòu)中,將連桿長度依次設(shè)為l1～l5,五連桿內(nèi)部關(guān)節(jié)角度依次設(shè)為θ1～θ5,髖關(guān)節(jié)側(cè)翻角度設(shè)為θ6,其對應(yīng)的串聯(lián)結(jié)構(gòu)如圖3（b）所示,連桿長度與圖3（a） 相同,對應(yīng)角度設(shè)為φ1、φ2、φ3。選取上部連桿的中心作為基座坐標(biāo)系（定義為0 坐標(biāo)系）,從基坐標(biāo)開始依次給所有關(guān)節(jié)編號,求解坐標(biāo)系之間的變換關(guān)系,最終得到足端坐標(biāo)系到基坐標(biāo)系的變換矩陣

圖3 并聯(lián)五連桿結(jié)構(gòu)及其對應(yīng)簡化串聯(lián)結(jié)構(gòu)Fig.3 Diagram of parallel five-link structure and its corresponding simplified series structure

式（1）中,nmT為從n號坐標(biāo)系到m號坐標(biāo)系的變換矩陣。

2.2 逆運(yùn)動學(xué)建模與求解

逆運(yùn)動學(xué)是指已知末端狀態(tài)位置和姿態(tài),求解各個關(guān)節(jié)角度的過程。對于該五連桿機(jī)器人,主要關(guān)心的問題即為通過足端與基坐標(biāo)系的位置關(guān)系,反解出所需要的髖關(guān)節(jié)兩個電機(jī)的角度即θ2與θ3。

首先,通過2.1 節(jié)串聯(lián)結(jié)構(gòu)下的變換矩陣,當(dāng)足端位置相對于基坐標(biāo)系的坐標(biāo)為（x,y,z）時,有

由此可反解出此時的φ1、φ2、φ3分別為

求得φ1、φ2即圖3（a）五連桿模型中的θ1、θ2后,根據(jù)五邊形約束方程,求取兩個髖關(guān)節(jié)角度θ2、θ3。非同軸五連桿的閉合方程如下

最終得到五連桿模型中的角度如下

式（5）中,

最終,可得到當(dāng)足端想要到達(dá)（x,y,z） 坐標(biāo)位置時的髖關(guān)節(jié)電機(jī)執(zhí)行角度θ2、θ3。

3 雙足機(jī)器人步態(tài)規(guī)劃

3.1 基于LIP 模型的質(zhì)心軌跡生成

本文所研究的機(jī)器人機(jī)身質(zhì)量占總體質(zhì)量的86.5%,故可將其簡化為LIP 模型,LIP 模型動力學(xué)方程如下

式（7） 中,g為重力加速度,x為質(zhì)心水平位置,h為恒定的質(zhì)心高度,p為足端著陸位置（即行走過程中每一步的輸入）。

LIP 模型為線性系統(tǒng),對于連續(xù)行走來說,給定足端位置p,在經(jīng)擺動時間T后,質(zhì)心狀態(tài)可以描述為一個離散系統(tǒng),設(shè)k為機(jī)器人行走過程中第k步,則可將系統(tǒng)描述如下

式（8）～式（10）中,ω=式（8）中的pk對應(yīng)為行走步態(tài)中第k步足端著陸點(diǎn),通過給定pk可以獲得質(zhì)心軌跡x（t）。

3.2 基于貝塞爾插值的擺動腿軌跡生成

在機(jī)器人行走過程中,擺動腿末端軌跡對機(jī)器人行走至關(guān)重要,足端與地面碰撞產(chǎn)生的作用力會對機(jī)器人行走產(chǎn)生擾動,導(dǎo)致機(jī)器人關(guān)節(jié)驅(qū)動器難以有效跟蹤期望位置。本文基于貝塞爾曲線生成擺動腿軌跡,貝塞爾曲線方程如下

式（11）中,t為自變量,M為貝塞爾多項(xiàng)式階數(shù),bi為支撐點(diǎn)的系數(shù)。

由于已知位置點(diǎn)只有足端起始點(diǎn)與足端終點(diǎn),因此簡單地采用了2 階貝塞爾曲線生成擺動腿足端軌跡。通過選取起始點(diǎn)、終點(diǎn)與（pk為邁步長度,zm=0.2m）處三個點(diǎn)進(jìn)行插值,獲得的足端軌跡結(jié)果如圖4所示。

圖4 平面內(nèi)擺動腿軌跡Fig.4 Trajectory of 2D swing leg

4 行走過程的運(yùn)動控制

上述章節(jié)中介紹的雙足機(jī)器人步態(tài)軌跡是在理想環(huán)境下生成的,然而在真實(shí)場景中,機(jī)器人機(jī)身的大慣性與足端落地沖擊會對機(jī)器人行走穩(wěn)定性造成較大影響。因此,為達(dá)到機(jī)器人穩(wěn)定行走的目的,本文在單腿支撐階段采用PD 反饋控制對機(jī)器人的質(zhì)心軌跡進(jìn)行跟蹤,在雙腿切換階段采用軌道能量方法計算捕獲點(diǎn)即穩(wěn)定落腳點(diǎn)位置。

4.1 基于PD 反饋的質(zhì)心軌跡跟蹤控制

在雙足機(jī)器人每一步邁步過程中,由于機(jī)身的大慣量,在重力影響下機(jī)器人會出現(xiàn)前后傾倒現(xiàn)象,從而導(dǎo)致在正確計算各個關(guān)節(jié)位置情況下,機(jī)器人實(shí)際的質(zhì)心軌跡也很難遵循倒立擺模型生成的質(zhì)心軌跡。

為了克服上述問題,本文在此采用PD 反饋控制

式（12） 中,xn、、、、分別為n時刻矢狀面上x方向的實(shí)際位置、期望位置、給定位置、實(shí)際速度和期望速度,zn、zrefn分別為n時刻矢狀面上z方向的實(shí)際位置和期望位置,kp、kd、kz為對應(yīng)的系數(shù)。

如圖5所示,首先依據(jù)LIP 模型生成的質(zhì)心軌跡和貝塞爾插值生成的擺動腿運(yùn)動軌跡,得到該時刻期望位置,根據(jù)逆運(yùn)動學(xué)求解出當(dāng)前狀態(tài)的關(guān)節(jié)角度,控制雙足機(jī)器人做期望運(yùn)動。通過陀螺儀、加速度計等傳感器實(shí)時獲取機(jī)器人機(jī)身位置、速度軌跡即xn、、zn,與期望軌跡做差,乘以相應(yīng)的系數(shù)kp、kd、kz,補(bǔ)償?shù)较乱粫r刻機(jī)身與足端相對位置的輸出上,使得機(jī)身保持水平運(yùn)動,成功跟蹤上節(jié)中生成的質(zhì)心運(yùn)動軌跡,實(shí)現(xiàn)穩(wěn)定的LIP 運(yùn)動。

圖5 邁步過程PD 反饋控制框圖Fig.5 Block diagram of PD feedback control in stepping process

4.2 基于軌道能量的捕獲點(diǎn)計算

在雙足機(jī)器人行走過程中,由于在足端落地瞬間地面會對機(jī)器人產(chǎn)生沖擊,如果離線給定機(jī)器人期望邁步長度,會使其在前進(jìn)過程中摔倒。同時在每兩步之間切換時,擺動腿期望落點(diǎn)位置決定了下一步態(tài)周期機(jī)器人初始狀態(tài)。

軌道能量是一種假想的勢能,用來描述機(jī)器人質(zhì)心的運(yùn)動狀態(tài),定義為

式（13）中,x、z分別為質(zhì)心在矢狀面上相對于支撐點(diǎn)的水平位置和高度,為質(zhì)心水平速度,g為重力加速度。在線性倒立擺運(yùn)動中,軌道能量守恒,因此在雙腿切換過程中,通過控制擺動腿的落點(diǎn)以獲得期望的軌道能量,從而獲得期望周期步態(tài)。

捕獲點(diǎn)[17]最初是基于目標(biāo)軌道能量為0 提出的,使得其在受到外界干擾以后能夠獲取一個動態(tài)平衡的控制過程,從而保證控制倒立擺模型的平衡[18]。但在實(shí)際應(yīng)用過程中,可以通過設(shè)定目標(biāo)軌道能量E來實(shí)現(xiàn)機(jī)器人穩(wěn)定前進(jìn)的效果,其計算方法如下

式（14）中符號定義與式（13）相同,sign（·）為符號函數(shù)。當(dāng)x>0 時,sign（x）=1;當(dāng)x=0 時,sign（x）=0;當(dāng)x<0 時,sign（x）=-1,可根據(jù)當(dāng)前質(zhì)心的速度方向來決定。

結(jié)合上述兩種算法,得到初步的控制框圖如圖6所示。系統(tǒng)輸入為質(zhì)心的目標(biāo)位置和速度（xref、）,反饋輸入為機(jī)器人的整體狀態(tài)（圖中的“機(jī)器人狀態(tài)”）,機(jī)器人狀態(tài)包括四個膝關(guān)節(jié)的關(guān)節(jié)角度、腰部的狀態(tài)（位置和姿態(tài)）、腰部的速度和角速度。根據(jù)這些輸入計算當(dāng)前落足點(diǎn)的優(yōu)化位置,并分別用LIP 和貝塞爾插值法計算支撐腿和擺動腿的軌跡。該控制器對機(jī)器人實(shí)現(xiàn)了一個閉環(huán)控制,改善了離線軌跡生成算法在魯棒性和控制精確度等方面的問題,確保機(jī)器人能夠?qū)崿F(xiàn)穩(wěn)定行走。

圖6 機(jī)器人穩(wěn)定行走控制簡圖Fig.6 Schematic diagram of robot stable walking control

5 仿真實(shí)驗(yàn)

在Matlab 中利用Simscape 模塊搭建仿真系統(tǒng):首先通過solidworks 設(shè)計裝配體,構(gòu)建與真實(shí)機(jī)身結(jié)構(gòu)相同的雙足機(jī)器人模型,生成整機(jī)的urdf 文件;其次將urdf 模型導(dǎo)入至Simulink 中,通過使用關(guān)節(jié)等模塊建立機(jī)械部件之間配合、約束關(guān)系的運(yùn)動學(xué)約束,并確定各個關(guān)節(jié)參考系;最后利用Multibody Contact Forces Library 庫調(diào)整參數(shù)模擬重力與接觸力碰撞,以達(dá)到真實(shí)的物理環(huán)境。

在此過程中,通過為機(jī)身裝載慣性傳感器模塊（即陀螺儀與加速度計）,為關(guān)節(jié)裝載編碼器模塊,實(shí)時獲取機(jī)器人的四個狀態(tài)。圖7展示了仿真系統(tǒng)中搭建的機(jī)器人整機(jī)系統(tǒng),表1展示了機(jī)器人模型參數(shù)。

圖7 機(jī)器人整機(jī)系統(tǒng)的搭建示意圖Fig.7 Schematic diagram of the whole robot system

表1 機(jī)器人模型參數(shù)Table 1 Parameters of robot model

對行走過程進(jìn)行仿真,反饋控制參數(shù)為kp=0.19、kd=-0.15、kz=-0.01。圖8為機(jī)器人行走過程截圖,可以看到機(jī)身有少許的向后傾倒,但并沒有妨礙機(jī)器人的穩(wěn)定行走。圖9為機(jī)器人行走過程中前部和后部髖關(guān)節(jié)電機(jī)的相軌跡圖。圖10為機(jī)器人行走過程中質(zhì)心速度隨時間的變化曲線,可以看出,雙足機(jī)器人在經(jīng)過一段時間后可以達(dá)到一個相對穩(wěn)態(tài),在仿真中實(shí)現(xiàn)了以一定速度向前行走的效果。

圖8 機(jī)器人行走仿真截圖Fig.8 Screenshot of robot walking simulation

圖9 髖關(guān)節(jié)電機(jī)的相軌跡圖Fig.9 Phase trajectory diagram of the hip joint motor

圖10 機(jī)器人質(zhì)心速度曲線（一段時間后趨于周期性穩(wěn)定）Fig.10 Curve of robot centroid velocity （cyclically stabilized after a period of time）

6 結(jié)論

針對點(diǎn)足式雙足機(jī)器人,本文提出了一種并聯(lián)五連桿結(jié)構(gòu),并基于該結(jié)構(gòu)進(jìn)行建模。通過正逆運(yùn)動學(xué)分析,將LIP 模型與PD 反饋控制進(jìn)行結(jié)合,對機(jī)器人質(zhì)心軌跡進(jìn)行跟蹤,并且根據(jù)實(shí)時反饋的自身狀態(tài)計算穩(wěn)定落腳點(diǎn)位置,實(shí)現(xiàn)閉環(huán)控制。結(jié)合實(shí)物模型構(gòu)建Matlab 仿真實(shí)驗(yàn),驗(yàn)證了控制算法的有效性,未來預(yù)期將本文所述控制方法用于實(shí)體機(jī)器人的二維矢狀面行走控制。