基于四足機(jī)器人的全向運(yùn)動(dòng)控制與仿真

張 博 何海龍 王 明 丁 雪

(西安電子工程研究所 西安 710100)

0 引言

四足機(jī)器人的研究開(kāi)始于20世紀(jì)60年代，在20世紀(jì)80年代初期四足機(jī)器人就已經(jīng)實(shí)現(xiàn)了一定的智能化[1]。21世紀(jì)以來(lái)，隨著微電子技術(shù)的發(fā)展和硬件運(yùn)算能力的提升，更加優(yōu)秀的控制算法和建模手段被不斷提出，計(jì)算機(jī)視覺(jué)技術(shù)和高精度傳感器的應(yīng)用，進(jìn)一步提高了機(jī)器人的姿態(tài)調(diào)整能力和環(huán)境適應(yīng)能力[2-4]。隨著機(jī)器學(xué)習(xí)和深度學(xué)習(xí)技術(shù)的發(fā)展，四足機(jī)器人已經(jīng)能夠根據(jù)自身實(shí)時(shí)運(yùn)動(dòng)狀態(tài)，以及復(fù)雜地形條件實(shí)現(xiàn)自我調(diào)整，平穩(wěn)行進(jìn)，并對(duì)外界突發(fā)的作用力進(jìn)行快速判斷和補(bǔ)償控制。使其成為一個(gè)穩(wěn)定可靠的機(jī)器運(yùn)動(dòng)載體。

本文介紹并分析了四足機(jī)器人的足端軌跡規(guī)劃和全向運(yùn)動(dòng)的基本控制算法和動(dòng)態(tài)仿真方法。

1 四足機(jī)器人的運(yùn)動(dòng)學(xué)研究

四足機(jī)器人的運(yùn)動(dòng)學(xué)主要研究機(jī)器人在空間中的位置、姿態(tài)和足端相對(duì)身體之間的運(yùn)動(dòng)關(guān)系。主要分為從關(guān)節(jié)角度控制量解算足端運(yùn)動(dòng)特性的正運(yùn)動(dòng)學(xué)和根據(jù)足端運(yùn)動(dòng)規(guī)劃求解關(guān)節(jié)角度驅(qū)動(dòng)的逆運(yùn)動(dòng)學(xué)分析。同時(shí)包括位置、姿態(tài)、速度、加速度等方面的分析[5-6]。

1.1 動(dòng)力學(xué)建模

通過(guò)對(duì)四足哺乳動(dòng)物身體結(jié)構(gòu)的仿生學(xué)等效得出了簡(jiǎn)單四足機(jī)器人結(jié)構(gòu)如圖1所示。每條腿由大腿、小腿和足三個(gè)部分組成，大腿與機(jī)體間設(shè)置相對(duì)機(jī)體橫向的和縱向的兩個(gè)自由度驅(qū)動(dòng)作為髖關(guān)節(jié)，并在小腿和大腿間設(shè)置相對(duì)機(jī)體縱向的自由度驅(qū)動(dòng)作為膝關(guān)節(jié)。每條腿有三個(gè)主動(dòng)自由度。對(duì)其使用Denavit-Hartenberg(D-H)方法[7]建模，得出四足機(jī)器人右前腿的運(yùn)動(dòng)學(xué)模型如圖2所示。其中的坐標(biāo)系Ow代表世界坐標(biāo)系，Ob代表機(jī)體坐標(biāo)系，四條腿坐標(biāo)系建立方法和右前腿一致。圖中運(yùn)動(dòng)方向與身體坐標(biāo)系X軸正方向同向，L(Left)代表左肢、R(Right)代表右肢、F(Front)代表前肢、H(Hind)代表后肢，右前腿可表示為RF。

圖1 仿生學(xué)等效模型

坐標(biāo)系(Xb,Yb,Zb)(如圖2所示)，原點(diǎn)Ob為機(jī)體質(zhì)心位置，設(shè)Xb軸方向?yàn)闄C(jī)體前向運(yùn)動(dòng)方向，Yb軸為反重力方向，Zb軸指向機(jī)體右側(cè)，其他坐標(biāo)系都按Z軸指向軸向，X軸指向前一個(gè)連桿的反向延長(zhǎng)線的規(guī)則建立。坐標(biāo)系0、1、2分別代表髖部側(cè)向運(yùn)動(dòng)、髖部前后運(yùn)動(dòng)和膝關(guān)節(jié)運(yùn)動(dòng)，原點(diǎn)都建立在關(guān)節(jié)中心。θ1、θ2和θ3分別代表各關(guān)節(jié)運(yùn)動(dòng)的角度。l1、l2、l3為側(cè)擺、大腿、小腿的長(zhǎng)度。圖2中坐標(biāo)系3代表右前腿足端坐標(biāo)系。

圖2 四足機(jī)器人連桿模型

1.2 運(yùn)動(dòng)學(xué)分析

(1)

用齊次變換矩陣可以描述各連桿坐標(biāo)系間的位置關(guān)系。如圖2中的右前腿D-H參數(shù)表如表1所示。

表1 單足D-H參數(shù)

(2)

(3)

(4)

(5)

式(2～5)中，si=sinθi,ci=cosθi,則可得到足端相對(duì)于機(jī)體坐標(biāo)的變換矩陣為

(6)

式(6)中，si.j=sin(θi+θj),ci.j=cos (θi+θj) 。已知關(guān)節(jié)驅(qū)動(dòng)求足端坐標(biāo)的過(guò)程被稱作動(dòng)力學(xué)正解過(guò)程，如式(7)所示。

(7)

式(7)中，(p′xp′yp′z)表示足端相對(duì)于機(jī)體坐標(biāo)系b的坐標(biāo)。已知足端坐標(biāo)求解各關(guān)節(jié)驅(qū)動(dòng)角的過(guò)程，稱為動(dòng)力學(xué)逆解，如式(8)所示。

(8)

(9)

式(8～9)中，(pxpypz)表示足端相對(duì)于坐標(biāo)系0的坐標(biāo)，由于坐標(biāo)系0相對(duì)于坐標(biāo)系b位置固定，運(yùn)動(dòng)學(xué)逆解通常使用坐標(biāo)系0做參考坐標(biāo)系。

2 四足機(jī)器人全向運(yùn)動(dòng)規(guī)劃

用D-H法分析機(jī)器人運(yùn)動(dòng)問(wèn)題，通常使用軌跡約束的方法，先確定足端的運(yùn)動(dòng)曲線，再通過(guò)運(yùn)動(dòng)學(xué)逆解，求解各關(guān)節(jié)轉(zhuǎn)動(dòng)驅(qū)動(dòng)的輸出，從而實(shí)現(xiàn)機(jī)器人的運(yùn)動(dòng)。文獻(xiàn)[8]提出了一種零沖擊的足端運(yùn)動(dòng)曲線?？梢允棺愣诉M(jìn)入支撐相時(shí)，速度和加速度都為零，如式(10)所示。

(10)

式(10)中，Tb代表足端處于擺動(dòng)相的周期；e=(exeyez)T是足端在世界坐標(biāo)系下的位置坐標(biāo)；ej=(ejxejyejz)T是j個(gè)運(yùn)動(dòng)周期后足端的位置；Sj=ejx-e(j-1)x表示足端在第j個(gè)運(yùn)動(dòng)周期相對(duì)于第j-1個(gè)運(yùn)動(dòng)周期的前向位移；Dj=ejz-e(j-1)z表示足端j周期的橫向位移；Hj表示足端抬起高度。

2.1 四足機(jī)器人的直線運(yùn)動(dòng)

四足機(jī)器人的直線行走作為周期運(yùn)動(dòng)，可以看成是四條腿按照特定的順序依次完成擺動(dòng)周期和支撐周期的過(guò)程。比如常見(jiàn)的步態(tài)trot(對(duì)角線的腿同時(shí)運(yùn)動(dòng))，可看作右前腿和左后腿同時(shí)開(kāi)始自己的運(yùn)動(dòng)周期，經(jīng)過(guò)一個(gè)擺動(dòng)到達(dá)支撐位置時(shí)，左前腿和右后退再開(kāi)始自己的運(yùn)動(dòng)周期。每條腿的運(yùn)動(dòng)周期T=Tz+Tb,是擺動(dòng)周期Tb和支撐周期Tz的和。支撐周期Tz占運(yùn)動(dòng)周期T的比例可以用負(fù)載因子β來(lái)表示。

2.1.1 四足機(jī)器人直線trot運(yùn)動(dòng)trot步態(tài)是哺乳動(dòng)物經(jīng)常使用的小跑步態(tài)，對(duì)角線上的兩腿可以看成一組，組內(nèi)兩腿運(yùn)動(dòng)同步，組間相差一個(gè)相位差。兩個(gè)組如果看成兩個(gè)虛腿，很像雙足動(dòng)物的行走動(dòng)作，但是由于每條虛腿支撐點(diǎn)和跨度很大，相比于雙足動(dòng)物，更容易實(shí)現(xiàn)平衡，穩(wěn)定性更好。

式(10)表示了足端在世界坐標(biāo)系下的軌跡，按此軌跡可以保證足端于地面接觸時(shí)的沖擊最小，同時(shí)獲得地面最大的摩擦力。

在直線運(yùn)動(dòng)時(shí)，可以假設(shè)身體在世界坐標(biāo)系下相當(dāng)于以Sj做前向勻速運(yùn)動(dòng)，并保持離地高度不變。就好像人在跑步機(jī)上跑步一樣，傳送帶勻速向后運(yùn)動(dòng)，人在上面保持身體不動(dòng)地奔跑，當(dāng)這個(gè)勻速運(yùn)動(dòng)的傳送帶撤掉，人便可以維持那種步態(tài)勻速前進(jìn)。從而得出足端相對(duì)于身體坐標(biāo)系的軌跡如圖3所示。可見(jiàn)相對(duì)于身體坐標(biāo)系，Y軸最低點(diǎn)落足的同時(shí)，X軸負(fù)向運(yùn)動(dòng)速度最快的點(diǎn)，并向相對(duì)于身體坐標(biāo)系向后，推動(dòng)身體向前運(yùn)動(dòng)。對(duì)角足相位相差半個(gè)周期就可以實(shí)現(xiàn)直線行走步態(tài)。

圖3 足端軌跡曲線圖

2.1.2 四足機(jī)器人直線walk運(yùn)動(dòng)

Walk運(yùn)動(dòng)指的是四足機(jī)器人更自由地分配四足的觸底時(shí)間，按順序向前邁步,每條腿之間相差四分之一個(gè)周期的相位差。相對(duì)于trot運(yùn)動(dòng)，walk運(yùn)動(dòng)穩(wěn)定性更高。

2.2 四足機(jī)器人的轉(zhuǎn)向運(yùn)動(dòng)

相比于直線運(yùn)動(dòng)，四足機(jī)器人的轉(zhuǎn)向運(yùn)動(dòng)可以看作是直線運(yùn)動(dòng)足端軌跡發(fā)生的轉(zhuǎn)動(dòng)變換。如圖4所示。

圖4 轉(zhuǎn)動(dòng)運(yùn)動(dòng)變換

身體轉(zhuǎn)動(dòng)角度為a，質(zhì)心轉(zhuǎn)動(dòng)半徑為R，世界坐標(biāo)系{W}建立在轉(zhuǎn)動(dòng)前的機(jī)體質(zhì)心處。轉(zhuǎn)動(dòng)前的機(jī)體坐標(biāo)系為{B},轉(zhuǎn)動(dòng)后的機(jī)體坐標(biāo)系為{B}′，則在轉(zhuǎn)動(dòng)過(guò)程中，坐標(biāo)系{B}′相對(duì)于世界坐標(biāo)系，產(chǎn)生了X軸方向上的平移和Y軸方向上的平移，以及沿Z軸發(fā)生的旋轉(zhuǎn)，體坐標(biāo)系的變換矩陣Gt，可用式(11)表示。

(11)

通過(guò)式(10)和式(11)可求出相對(duì)于轉(zhuǎn)動(dòng)運(yùn)動(dòng)機(jī)體坐標(biāo)系下的各足端軌跡。可通過(guò)(8)求得各關(guān)節(jié)的驅(qū)動(dòng)輸出。

2.3 四足機(jī)器人的斜向運(yùn)動(dòng)

四足機(jī)器人做斜向運(yùn)動(dòng)時(shí)同樣可以通過(guò)體坐標(biāo)系的變換，求解各足端的運(yùn)動(dòng)軌跡，從而各關(guān)節(jié)的驅(qū)動(dòng)輸出。如圖5所示，斜向角為Ф，機(jī)身橫向運(yùn)動(dòng)距離為H2,縱向移動(dòng)距離為H1，世界坐標(biāo)系{W}建立在轉(zhuǎn)動(dòng)前的機(jī)體質(zhì)心處。轉(zhuǎn)動(dòng)前的機(jī)體坐標(biāo)系為{B},轉(zhuǎn)動(dòng)后的機(jī)體坐標(biāo)系為{B}′，則在斜向過(guò)程中，坐標(biāo)系{B}′相對(duì)于世界坐標(biāo)系，產(chǎn)生了X軸方向上的平移和Y軸方向上的平移，體坐標(biāo)系的變換矩陣Gt，可用式(12)表示。

圖5 斜向運(yùn)動(dòng)變換

(12)

3 聯(lián)合仿真

ADAMS是美國(guó)MDI公司發(fā)布的三維虛擬軟件，提供了對(duì)Matlab的接口。本文通過(guò)SolidWorks軟件建立了一個(gè)用于仿真的四足機(jī)器人模型如圖6所示，并導(dǎo)入到ADAMS軟件中設(shè)置材料、連接關(guān)系、摩擦力等參數(shù)和系統(tǒng)單元。再導(dǎo)入Matlab的Simulink中如圖8所示，通過(guò)S函數(shù)的編寫(xiě)和Simulink內(nèi)部模塊的連接(如圖7所示)，實(shí)現(xiàn)了對(duì)四足機(jī)器人的動(dòng)態(tài)聯(lián)合仿真。

圖6 用于仿真的模型

圖7 Simulink連接圖

圖8 adams在simulink下的模型

4 仿真分析

本文在Simulink下分別對(duì)四足機(jī)器人的足端相對(duì)于身體質(zhì)心坐標(biāo)系下的位置進(jìn)行了數(shù)據(jù)錄取。直行步態(tài)、轉(zhuǎn)向步態(tài)、斜向步態(tài)的三維圖分別如圖9、圖10、圖11所示。

圖9 直行足端位置

圖10 轉(zhuǎn)向足端位置

圖11 斜向足端位置

以上數(shù)據(jù)反應(yīng)了四足機(jī)器人在兩個(gè)運(yùn)動(dòng)周期內(nèi)的四足足端相對(duì)于身體的位置變化情況。可以明顯看出運(yùn)動(dòng)趨勢(shì)和軌跡變化情況。

5 結(jié)束語(yǔ)

本文對(duì)四足機(jī)器人足端軌跡規(guī)劃運(yùn)動(dòng)方法進(jìn)行了仿真。從Adams和Simulink的交互式動(dòng)態(tài)仿真效果上看，已經(jīng)圓滿實(shí)現(xiàn)了全向運(yùn)動(dòng)的控制目的。但是運(yùn)動(dòng)中呈現(xiàn)明顯的晃動(dòng)和不平衡現(xiàn)象。要實(shí)現(xiàn)平穩(wěn)運(yùn)動(dòng)控制，還需要考慮機(jī)器人的平衡穩(wěn)定問(wèn)題。