基于輸入解耦特征模型的四足機(jī)器人運(yùn)動(dòng)控制*

張世俊

0 引 言

地面移動(dòng)機(jī)器人的運(yùn)動(dòng)方式主要包括了輪式、足式和履帶式等，其中輪式機(jī)器人在硬平地面上移動(dòng)速度快，具有較好的穩(wěn)定性，但是其在非結(jié)構(gòu)化環(huán)境中的運(yùn)動(dòng)能力不夠理想；與輪式機(jī)器人相比，足式機(jī)器人在平地上運(yùn)動(dòng)速度較低，但由于它可利用非連續(xù)地面支撐，且支撐點(diǎn)可達(dá)的空間范圍大，從而對(duì)于各種復(fù)雜地形情況的適應(yīng)能力也較強(qiáng)[1-2].

由于轉(zhuǎn)彎步態(tài)具有很強(qiáng)的擴(kuò)展性，當(dāng)轉(zhuǎn)彎半徑為變量時(shí)，轉(zhuǎn)彎步態(tài)可以跟蹤任意曲線軌跡[3]，在復(fù)雜地形環(huán)境下，足式機(jī)器人可采用轉(zhuǎn)彎步態(tài)繞過(guò)崎嶇的區(qū)域從而降低步態(tài)規(guī)劃的復(fù)雜程度，提高行進(jìn)效率.現(xiàn)有轉(zhuǎn)彎步態(tài)規(guī)劃方法主要通過(guò)結(jié)合偏航或側(cè)擺關(guān)節(jié)實(shí)現(xiàn)[4-5]，本文提出一種對(duì)角轉(zhuǎn)彎步態(tài)，在不需要根關(guān)節(jié)偏轉(zhuǎn)的情況下，僅依靠髖關(guān)節(jié)和膝關(guān)節(jié)的擺動(dòng)實(shí)現(xiàn)機(jī)器人轉(zhuǎn)彎.

所謂特征模型，就是根據(jù)對(duì)象動(dòng)力學(xué)特征、環(huán)境特征和控制性能要求相結(jié)合建立的模型，而不僅是以對(duì)象精確的動(dòng)力學(xué)分析來(lái)建模.特征模型比原動(dòng)力學(xué)方程簡(jiǎn)單，工程實(shí)現(xiàn)容易、方便.基于特征模型的自適應(yīng)控制方法采用離散設(shè)計(jì)方法，其算法簡(jiǎn)單、控制品質(zhì)好、適應(yīng)性和魯棒性強(qiáng)，并已經(jīng)取得了廣泛的應(yīng)用[6].

足式機(jī)器人腿部動(dòng)力學(xué)模型具有多輸入多輸出、時(shí)變、耦合和非線性等特性，腿部各關(guān)節(jié)之間存在耦合現(xiàn)象[7].本文針對(duì)足式機(jī)器人腿部動(dòng)力學(xué)方程，建立一種輸入解耦特征模型，具有狀態(tài)耦合和輸入解耦的形式，在保留了原系統(tǒng)模型輸入輸出整體完整性的前提下，考慮了結(jié)構(gòu)的耦合特點(diǎn)，進(jìn)而在此基礎(chǔ)上設(shè)計(jì)了相應(yīng)的自適應(yīng)控制方法.

1 機(jī)器人結(jié)構(gòu)與單腿動(dòng)力學(xué)模型

本文研究的四足機(jī)器人總體構(gòu)型如圖1所示，機(jī)器人包含4條腿和身體5個(gè)相對(duì)獨(dú)立的部分，每條腿具有3個(gè)關(guān)節(jié)，分別為根關(guān)節(jié)(偏航自由度)、髖關(guān)節(jié)(俯仰自由度)和膝關(guān)節(jié)(俯仰自由度).

圖2 單腿D-H模型Fig.2 D-H model of single leg

按照拉格朗日方法所述[8]，拉格朗日函數(shù)L為總動(dòng)能K減去總勢(shì)能U，其表達(dá)式為：

(1)

(2)

(3)

由動(dòng)力學(xué)方程(3)可知，該系統(tǒng)是一個(gè)非線性的高度耦合的控制系統(tǒng)，其耦合作用在方程中體現(xiàn)在慣量、哥氏力和重力之間的相互影響，在實(shí)際運(yùn)動(dòng)中機(jī)器人每一個(gè)關(guān)節(jié)的運(yùn)動(dòng)都受到其他關(guān)節(jié)的耦合影響.

2 特征建模及控制器設(shè)計(jì)

對(duì)于式(3)的動(dòng)力學(xué)模型，由于D(q)可逆[8]，動(dòng)力學(xué)方程可寫(xiě)為：

(4)

(5)

其中w=F′-H0+(B0-I)u.

對(duì)式(5)進(jìn)行離散化處理，采樣周期為Δt，采用如下形式的近似離散化方法：

可得到如下方程：

(6)

其中I代表三階單位陣，將式(6)寫(xiě)成每一通道的形式：

(7)

令

(8)

則式(7)可改寫(xiě)為

(9)

其中

注1．式(9)需滿足Δj(k)≠0，實(shí)際上當(dāng)Δj(k)=0時(shí)，可采用文獻(xiàn)[9]中的處理方法，仍舊可以獲得形如式(9)的特征模型.

至此，得到輸入解耦特征模型標(biāo)準(zhǔn)形式：

q(k+1)=F1(k)q(k)+F2(k)q(k-1)+
G0(k)u(k)

(10)

其中,q(k)=[q1(k)q2(k)q3(k)]T，

u(k)=[u1(k)u2(k)u3(k)]T，

F1(k)，F(xiàn)2(k)是正方陣，G0(k)是對(duì)角陣，并且滿足當(dāng)Δt→0時(shí)，f1,ii(k)→2，f1,ji(k)→0，f2,ii(k)→-1，f2,ji(k)→0，g0j(k)→0(i,j=1,2,3i≠j).特征模型把原復(fù)雜系統(tǒng)非線性、耦合特性轉(zhuǎn)化到幾個(gè)時(shí)變的特征參量中，大大簡(jiǎn)化了模型的復(fù)雜程度，利于控制器的設(shè)計(jì).

輸入解耦特征模型方程(10)中的特征參數(shù)為時(shí)變參數(shù)，需要通過(guò)在線辨識(shí)給出，由每一通道特征模型可知：

(11)

其中

采用如下投影梯度算法：

(12)

式中λ1，λ2為正常數(shù)，π[x]表示x到有界閉凸集Ds上的正交投影.

通過(guò)方程(12)辨識(shí)出特征參量，結(jié)合輸入解耦特征模型，設(shè)計(jì)輪足復(fù)合機(jī)器人的關(guān)節(jié)控制器，包含多變量黃金分割控制器和微分控制器.

將特征模型中F1(k)和F2(k)的非對(duì)角元素作為耦合項(xiàng)單獨(dú)列出，模型(10)可改寫(xiě)為：

q(k+1)=A1(k)q(k)+A2(k)q(k-1)+
G0(k)u(k)+C1(k)q(k)+C2(k)q(k-1)

(13)

其中,

此時(shí)，A1(k)，A2(k)，G0(k)都為對(duì)角陣，其逆矩陣就是對(duì)角元素的倒數(shù)，并將耦合項(xiàng)記作

C(k)=C1(k)q(k)+C2(k)q(k-1)

(14)

因此多變量黃金分割控制律為

(15)

(16)

其中微分系數(shù)Kd=diag{d1,d2,d3}為對(duì)角常數(shù)陣.

綜合得到總的控制律為:

u(k)=ul(k)+ud(k)

(17)

3 對(duì)角轉(zhuǎn)彎步態(tài)規(guī)劃

四足機(jī)器人對(duì)角小跑步態(tài)把四條腿分為兩組，右前腿和左后腿一組，左前腿和右后腿一組，兩組交替運(yùn)動(dòng)，同一時(shí)刻由對(duì)角的兩條腿支撐身體[10].轉(zhuǎn)彎步態(tài)是四足機(jī)器人的一種基本步態(tài)模式，本文提出一種轉(zhuǎn)彎步態(tài)規(guī)劃方法，在不需要轉(zhuǎn)向關(guān)節(jié)的情況下，僅依靠髖關(guān)節(jié)和膝關(guān)節(jié)的擺動(dòng)，通過(guò)調(diào)整左右腿的邁步步長(zhǎng)以實(shí)現(xiàn)四足機(jī)器人對(duì)角小跑下的轉(zhuǎn)彎步態(tài).如圖3所示，四足機(jī)器人進(jìn)行左轉(zhuǎn)彎，當(dāng)機(jī)器人機(jī)身長(zhǎng)度較小時(shí)，機(jī)器人左側(cè)的線速度為v1，右側(cè)的線速度為v2，機(jī)器人左右腿的距離為m，則機(jī)器人本體轉(zhuǎn)彎時(shí)的轉(zhuǎn)動(dòng)角速度ω為[11]

(18)

其中R為機(jī)器人的轉(zhuǎn)彎半徑，

(19)

由于足式運(yùn)動(dòng)依靠支撐腿的向后擺動(dòng)推動(dòng)機(jī)器人身體前進(jìn)，有

其中s1，s2分別為左腿和右腿的邁步步長(zhǎng)，ts為支撐相時(shí)間，s0為機(jī)器人以直線前進(jìn)時(shí)規(guī)劃的邁步步長(zhǎng)，Δs為機(jī)器人進(jìn)行轉(zhuǎn)彎時(shí)對(duì)左右腿邁步步長(zhǎng)的修正量.將其代入式(19)，得

(20)

當(dāng)Δs=0時(shí)，左右腿邁步腿步長(zhǎng)相等，轉(zhuǎn)彎半徑為無(wú)窮大，即機(jī)器人沿直線行走，隨著Δs逐漸增大，轉(zhuǎn)彎半徑逐漸減小，考慮到機(jī)器人足端工作空間的限制和前后腿不發(fā)生碰撞的限制，Δs存在一個(gè)上界，與機(jī)器人行走高度、直線行走時(shí)的邁步步長(zhǎng)和足端工作空間有關(guān)，此上界對(duì)應(yīng)于四足機(jī)器人的最小轉(zhuǎn)彎半徑.

注2．本文提出的對(duì)角轉(zhuǎn)彎步態(tài)不依靠轉(zhuǎn)向關(guān)節(jié)，因此存在最小轉(zhuǎn)彎半徑，若增加轉(zhuǎn)向關(guān)節(jié)，四足機(jī)器人可做到定點(diǎn)轉(zhuǎn)彎，即零轉(zhuǎn)彎半徑[12].

圖3 機(jī)器人對(duì)角轉(zhuǎn)彎示意圖Fig.3 Diagram of robot left turn via trotting gait

實(shí)際上由于機(jī)器人身體長(zhǎng)度不為零，理論轉(zhuǎn)彎半徑與實(shí)際轉(zhuǎn)彎半徑有一定差異，因此需要進(jìn)一步分析實(shí)際轉(zhuǎn)彎半徑與步長(zhǎng)修正量之間關(guān)系，為步態(tài)規(guī)劃提供參考.通過(guò)對(duì)仿真中四足機(jī)器人身體重心軌跡的記錄，可以畫(huà)出不同步長(zhǎng)修正量下機(jī)器人的運(yùn)動(dòng)軌跡，俯視圖如圖4所示，仿真時(shí)間均為15 s，其中仿真前2 s均為直線步態(tài)，即Δs=0，從2 s開(kāi)始采用不同的步長(zhǎng)修正量.從圖中可以看出，所有參數(shù)情況下仿真得到的機(jī)器人軌跡都近似于圓弧線，對(duì)軌跡曲線進(jìn)行最小二乘法的圓擬合，可以得到相應(yīng)的轉(zhuǎn)彎半徑，表1為不同步長(zhǎng)修正量下實(shí)際轉(zhuǎn)彎半徑的統(tǒng)計(jì)表.

圖4 不同步長(zhǎng)修正量下機(jī)器人重心軌跡俯視圖Fig.4 The top view of the centroid trajectory of robot with different step length correction

s0/mΔs/mR/m0.20∞0.20.01160.20.025.50.20.033.20.20.042.50.20.052

4 數(shù)學(xué)仿真

為驗(yàn)證本文提出的方法，在ADAMS中搭建輪腿復(fù)合機(jī)器人虛擬樣機(jī)，采用ADAMS和SIMULINK聯(lián)合仿真技術(shù)進(jìn)行虛擬樣機(jī)仿真.機(jī)器人身體質(zhì)量150 kg，每條腿質(zhì)量10.5 kg，其中大腿4 kg，小腿3 kg，輪子3.5 kg，大腿長(zhǎng)度為0.4 m，小腿長(zhǎng)度為0.3 m，輪子半徑為0.1 m.采用基于碰撞函數(shù)的接觸算法來(lái)計(jì)算足端接觸，接觸力參數(shù)設(shè)計(jì)如下：剛度為1.0×108N/m，阻尼為1.0×104(N·s)/m，穿透深度為1.0×10-4m，力指數(shù)為2.2，靜摩擦系數(shù)取為0.4，動(dòng)摩擦系數(shù)取為0.3.

仿真中機(jī)器人采用本文提出的對(duì)角轉(zhuǎn)彎步態(tài)規(guī)劃方法和基于輸入解耦特征模型的控制方法.設(shè)定步態(tài)參數(shù)如下：機(jī)器人行走高度為0.8 m，步態(tài)周期為0.72 s，直行邁步步長(zhǎng)為0.2 m.足端軌跡采用復(fù)合擺線形式[13]，可減小足端與地面的沖擊，邁腿高度為0.1 m.仿真時(shí)間為15 s，0～2 s時(shí)機(jī)器人沿直線前進(jìn)，2～15 s時(shí)機(jī)器人左轉(zhuǎn)彎，步長(zhǎng)修正量采用如下形式：

圖5為機(jī)器人負(fù)載為0和50 kg的情況下，左前腿髖關(guān)節(jié)和膝關(guān)節(jié)的跟蹤曲線，為方便觀看圖中只畫(huà)出了前兩秒的關(guān)節(jié)跟蹤曲線.圖6為機(jī)器人重心軌跡俯視圖.由圖中可以看出整個(gè)步態(tài)周期內(nèi)，關(guān)節(jié)角度跟蹤效果較好，并且對(duì)機(jī)器人的不同負(fù)載情況具有較好的適應(yīng)能力.

5 結(jié) 論

本文首先建立了四足機(jī)器人單腿動(dòng)力學(xué)模型，分析各關(guān)節(jié)之間的耦合特性，建立并推導(dǎo)了輸入解耦特征模型，基于此設(shè)計(jì)了相應(yīng)的多變量控制器，其次提出了一種對(duì)角轉(zhuǎn)彎步態(tài)，在不需要根關(guān)節(jié)偏轉(zhuǎn)的情況下實(shí)現(xiàn)機(jī)器人的轉(zhuǎn)彎，最后基于ADAMS的虛擬樣機(jī)仿真驗(yàn)證了本文方法的有效性.

圖5 不同負(fù)載下機(jī)器人左前腿關(guān)節(jié)角跟蹤曲線Fig.5 Joint angle curves of left front leg under different loads

圖6 機(jī)器人重心軌跡俯視圖Fig.6 The top view of the centroid trajectory of robot

[1] NAGANO K,FUJIMOTO Y.The stable wheeled locomotion in low speed region for a wheel-legged mobile robot[C]//The IEEE International Conference on Mechatronics.New York:IEEE,2015.

[2] ENDO G,HIROSE S.Study on Roller-Walker—Improvement of Locomotive Efficiency of Quadruped Robots by Passive Wheels[J].Advanced Robotics, 2012, 26(8-9): 969-988.

[3] 陳剛.六足步行機(jī)器人位姿控制及步態(tài)規(guī)劃研究 [D]. 杭州: 浙江大學(xué), 2014.4

CHEN G. Position-posture control and gait planning of six-legged walking robot[D]. Hangzhou: Zhejiang University, 2014.

[4] 郝仁劍,王軍政,史大威,等. 基于速度矢量的四足機(jī)器人間歇步態(tài)規(guī)劃方法 [J]. 機(jī)器人, 2016, 38(5): 540-549.

HAO R J, WANG J Z, SHI D W, et al. Intermittent gait planning method of quadruped robot based on velocity vector[J]. Robot, 2016, 38(5): 540-549.

[5] 孟健,李貽斌,李彬.四足機(jī)器人對(duì)角小跑步態(tài)全方位移動(dòng)控制方法及其實(shí)現(xiàn) [J]. 機(jī)器人, 2015, 37(1): 74-84.

MENG J, LI Y B, LI B. Control method and its implementation of quadruped robot in omni-directional trotting gait[J]. Robot, 2015, 37(1): 74-84.

[6] 吳宏鑫.基于特征模型的智能自適應(yīng)控制[M].北京：中國(guó)科學(xué)技術(shù)出版社, 2009.

[7] 高炳微.液壓四足機(jī)器人單腿關(guān)節(jié)解耦控制及力/位切換控制研究[D].哈爾濱：哈爾濱理工大學(xué), 2015.

GAO B W. Research on single leg joint decoupling and force/position switching control of hudraulic quadruped robot[D]. Haerbin: Harbin University of Science and Technology, 2015.

[8] SPONG M W,HUTCHINSON S,VIDYASAGAR M. Robot Modeling and Control [M]. Hoboken:John Wiley & Sons, 2004.

[9] 徐李佳,胡勇.非合作機(jī)動(dòng)目標(biāo)交會(huì)的相對(duì)位置控制[J].空間控制技術(shù)與應(yīng)用, 2015, 41(6): 13-18.

XU L J, HU Y. Control for relative position of spacecraft rendezvous with a non-cooperative maneuver target[J]. Aerospace control and application, 2015, 41(6): 13-18.

[10] MORO F L,SPROWITZ A,TULEU A,et al. Horse-like walking, trotting, and galloping derived from kinematic Motion Primitives (kMPs) and their application to walk/trot transitions in a compliant quadruped robot[J].Biological cybernetics,2013, 107(3): 309-20.

[11] 黃淼,聶宏,陳金寶.基于ADAMS的載人月球車(chē)性能分析[J]. 科學(xué)技術(shù)與工程,2010,10(20): 5000-5004.

HUANG M, NIE H, CHEN J B. The performance analysis of manned-lunar rover based on ADAMS software[J]. Science technology and engineering, 2010,10(20): 5000-5004.

[12] MA S,TOMIYAMA T,WADA H.Omnidirectional static walking of a quadruped robot[J].IEEE Transactions on Robotics, 2005, 21(2): 152-161.

[13] 王立鵬,王軍政,汪首坤,等. 基于足端軌跡規(guī)劃算法的液壓四足機(jī)器人步態(tài)控制策略[J].機(jī)械工程學(xué)報(bào), 2013, 49(01): 39-44.

WANG L P, WANG J Z, WANG S K, et al. Strategy of foot trajectory generation for hydraulic quadruped robots gait planning[J]. Journal of mechanical engineering,2013, 49(01): 39-44.