基于四足機器人的全向運動控制與仿真

張 博 何海龍 王 明 丁 雪

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

0 引言

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

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

1 四足機器人的運動學(xué)研究

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

1.1 動力學(xué)建模

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

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

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

圖2 四足機器人連桿模型

1.2 運動學(xué)分析

(1)

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

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

(2)

(3)

(4)

(5)

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

(6)

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

(7)

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

(8)

(9)

式(8～9)中，(pxpypz)表示足端相對于坐標系0的坐標，由于坐標系0相對于坐標系b位置固定，運動學(xué)逆解通常使用坐標系0做參考坐標系。

2 四足機器人全向運動規(guī)劃

用D-H法分析機器人運動問題，通常使用軌跡約束的方法，先確定足端的運動曲線，再通過運動學(xué)逆解，求解各關(guān)節(jié)轉(zhuǎn)動驅(qū)動的輸出，從而實現(xiàn)機器人的運動。文獻[8]提出了一種零沖擊的足端運動曲線。可以使足端進入支撐相時，速度和加速度都為零，如式(10)所示。

(10)

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

2.1 四足機器人的直線運動

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

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

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

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

圖3 足端軌跡曲線圖

2.1.2 四足機器人直線walk運動

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

2.2 四足機器人的轉(zhuǎn)向運動

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

圖4 轉(zhuǎn)動運動變換

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

(11)

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

2.3 四足機器人的斜向運動

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

圖5 斜向運動變換

(12)

3 聯(lián)合仿真

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

圖6 用于仿真的模型

圖7 Simulink連接圖

圖8 adams在simulink下的模型

4 仿真分析

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

圖9 直行足端位置

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

圖11 斜向足端位置

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

5 結(jié)束語

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