基于2(6UPUR+3P)型六足步行機器人動力學建模及仿真

王 瑞，唐新星，張德勇，韓方元

(1.長春工業(yè)大學，吉林 長春130012；2.長春理工大學，吉林 長春130022)

0 前言

自Stewart提出并聯(lián)運動平臺以來，不同結(jié)構(gòu)、不同自由度的并聯(lián)機構(gòu)相繼產(chǎn)生，并聯(lián)機構(gòu)具有精度高、承載能力強、剛度高等優(yōu)點，在軍事、醫(yī)療、工業(yè)、航空航天等領(lǐng)域廣泛應(yīng)用[1，2]。以并聯(lián)機構(gòu)為基礎(chǔ)構(gòu)建的并聯(lián)腿式步行機器人，例如WL-16RIV的腿部機構(gòu)為6自由度并聯(lián)機構(gòu)，六足“章魚”機器人，輪足復(fù)合電動并聯(lián)機器人“北理哪吒”，與輪式、履帶式移動機器人相比，具有較強跨越障礙物的能力，對崎嶇地形、危險環(huán)境具有更好的適應(yīng)性[3-5]。

多足機器人動力學建模是對機構(gòu)進行性能分析、優(yōu)化設(shè)計的必要前提，構(gòu)建動力學方程的方法有拉格朗日法、虛功原理法、牛頓-歐拉法和凱恩方程法等[6-9]。王洪波等人對空間三自由度串并混聯(lián)腿機構(gòu)(2-UPS+U)進行了動力學建模和仿真，基于矢量法和虛功原理建立了該混聯(lián)腿機構(gòu)的動力學模型,推導出該混聯(lián)腿機構(gòu)的動力學方程[10]；王英波等人對六自由度并聯(lián)機器人描述為單個剛體，采用凱恩方法建立其單剛體動力學模型，并進行了仿真分析[11]。

本文構(gòu)建2(6UPUR+3P)型六足步行機器人結(jié)構(gòu)，擁有多維復(fù)雜系統(tǒng)，步行過程中，驅(qū)動機構(gòu)產(chǎn)生的力與力矩會對機器人運動性能有很大的影響。因此，采用推導速度快、實時性好的牛頓-歐拉法求解其動力學方程，將六足步行機器人的2個腿部結(jié)構(gòu)分別定義為支撐腿和擺動腿，支撐腿與擺動腿交替變換，每個腿部6-UPUR結(jié)構(gòu)又描述為單個剛體，單剛體動力學方程能夠充分描述整個機械系統(tǒng)的特性，特別是對重載運動系統(tǒng)，最終到所需的動力學方程。利用Matlab在矩陣和向量運算方面的優(yōu)勢，根據(jù)數(shù)學模型公式編寫成Matlab程序，將六自由度并聯(lián)機器人動力學方程轉(zhuǎn)化成計算機內(nèi)的模型；為了驗證Maltab程序所描述模型的正確性，采用剛體建模軟件 SimMechanics在計算機中重新構(gòu)建六足步行機器人的動力學模型，并對兩種計算機模型的仿真結(jié)果進行了對比分析。

1 2(6UPUR+3P)型六足步行機器人結(jié)構(gòu)設(shè)計

1.1 六足步行機器人結(jié)構(gòu)描述

基于2(6UPUR+3P)型的六足步行機器人結(jié)構(gòu)包含2條腿，每條腿都是采用6UPUR+3P結(jié)構(gòu)，每條腿上有3個足，其中6-UPUR并聯(lián)腿部分包括上平臺、下平臺及連接上平臺、下平臺的6個相同的基于UPUR結(jié)構(gòu)的支鏈組成，且各支鏈上端均連在各自的上平臺上，而2個上平臺固連在一起形成髖關(guān)節(jié)[12]。對于下平臺而言，2個下平臺相互獨立且無相互干擾，均包括1個足弓連接架、3個踝關(guān)節(jié)固定座、3個腳趾，6個UPUR結(jié)構(gòu)的支鏈下端分別兩兩鉸接在3個踝關(guān)節(jié)固定座上。為保證基于6-UPUR結(jié)構(gòu)并聯(lián)腿具有較強的越障能力，在3個踝關(guān)節(jié)固定座上各安裝了一個輔助腿，輔助腿的伸長與縮短改變了6-UPUR并聯(lián)腿的z向工作空間大小，基于三維制圖軟件Solidworks中的虛擬樣機結(jié)構(gòu)如圖1所示。由于兩條腿結(jié)構(gòu)相同，邁步程中，支撐腿與擺動腿交替變換，本文僅以一個腿為例進行分析。

圖1 六足步行機器人的虛擬樣機結(jié)構(gòu)

1.2 機構(gòu)建模

為了方便仿真驗證建立數(shù)學的模型，將基于Solidworks軟件繪制的2(6UPUR+3P)型并聯(lián)機器人實體模型，另存為XML文件，并通過指定窗口導入到Matlab軟件中，在 Simulink/SimMechanics中自動生成整體機構(gòu)模塊圖，如圖2所示，此時各個構(gòu)件的參數(shù)已經(jīng)自動設(shè)置好，只需要對機構(gòu)模型添加驅(qū)動模塊以及輸入輸出模塊即可。將設(shè)置好的初始參考值和位姿輸入到物理模型中，并給定輸入軌跡運行后便得到可視化的結(jié)構(gòu)圖，如圖3所示。

圖2 SimMechanics 中的動力學模型

圖3 并聯(lián)機器人單腿可視化結(jié)構(gòu)圖

2 2(6UPUR+3P)并聯(lián)機器人腿部動態(tài)特性分析

2.1 運動學分析

腿部運動學位置分析是其腿部各支鏈速度、加速度分析的基礎(chǔ)。機器人腿部中的輔助腿3P結(jié)構(gòu)主要用于提升機器人腿部在z方向上的工作空間，為此，建立如圖4所示的機器人6-UPUR并聯(lián)腿坐標系，在6-UPUR并聯(lián)機構(gòu)的髖關(guān)節(jié)、下平臺的幾何中心分別建立坐標系，其中o為上平臺(定平臺)幾何中心的位置，m為下平臺(動平臺)幾何中心的位置，分別以上、下平臺幾何中心o和m為原點，建立上平臺的定坐標系{o-xyz}和下平臺的動坐標系{m-uvw}，上平臺與下平臺的質(zhì)心分別與其各自平臺上所建立的坐標系的原點重合，上平臺與支鏈連接的鉸點處建立支鏈的坐標系

圖4 6-UPUR并聯(lián)腿坐標系建立

{ni}={ni-difigi} (i=1,2,3,…6)

圖5為6-UPUR腿部支鏈幾何構(gòu)型簡圖，為了便于后面的分析及公式推導，將上平臺與各UPUR支鏈相連接的鉸點表示為Ai(i=1,2,…,6)，下平臺與各UPUR支鏈連接的鉸點表示為Bi(i=1,2,…,6)。其中上平臺鉸點在定坐標系{o}下的位置矢量為ci(=1,2,…,6)，下平臺鉸點Bi在動坐標系{m}下的位置矢量為Hi(i=1,2,…,6)。假定上平臺的外接圓半徑為R，下平臺的外接圓半徑為r。

圖5 6-UPUR腿部支鏈幾何構(gòu)型簡圖

定平臺坐標系{o}相對于動坐標系{m}的旋轉(zhuǎn)矩陣0Rm表示為

(1)

式中,α、β和γ分別表示為動平臺繞x、y、z軸的歐拉角；cγ=cosγ，sγ=sinγ，cβ=cosβ，sβ=sinβ，cα=cosα，sα=sinα。

圖6為6-UPUR腿部支鏈閉環(huán)圖。如圖6所示，在支鏈坐標系下支鏈的方向向量為ui；Li為支鏈i的長度；電動缸推桿質(zhì)心到Ai的距離為lai，電動推桿質(zhì)心到Bi的距離為lbi；則電動缸和電動推桿質(zhì)心的位置向量分別表示為

圖6 6-UPUR腿部支鏈閉環(huán)詳細圖

Lai=laiui

(2)

Lbi=(Li-Lbi)ui

(3)

動坐標系{m}的原點在定坐標系{o}原點的位置矢量，用P表示，其中P=[xyz]T；動平臺上的鉸點Bi(=1,2,…,6)到定平臺坐標中心的位置矢量，用Qi表示；位置矢量Hi在定坐標系{o}下的表示為ki，其中ki=oRmHi；因此根據(jù)矢量法能夠列出閉環(huán)方程為

ci+Liui=Qi=P+ki

(4)

整理得

(5)

(6)

動平臺質(zhì)心在定坐標系下的位置矢量為d=oRmdm。其中，dm表示動平臺質(zhì)心在動坐標系下的位置矢量。則動平臺質(zhì)心的加速度為

(7)

每條支鏈的下鉸點Bi質(zhì)心速度VBi表示為

(8)

根據(jù)式(4)，VBi也可以表示為

(9)

式(7)和(8)分別對時間求導，可以得到球鉸Bi質(zhì)心加速度的兩種表達式為

(10)

(11)

將式(8)和式(9)聯(lián)立得

(12)

將式(10)和式(11)聯(lián)立得

(13)

對式(12)等式兩邊同時叉乘ui并整理得支鏈i的角速度為

(14)

對式(11)等式兩邊叉乘ui得支鏈的角加速度為

(15)

分別對式(12)和式(13)等式兩邊同時點乘ui得支鏈伸縮的速度為

(16)

(17)

因此，電動缸的速度及加速度為

vsi=laiωn×ui

(18)

(19)

電動桿的速度和加速度為

八歲那年的暑假里，我們?nèi)胰ハ愀勐糜巍Ｎ覀冏p層巴士，穿過跨海大橋，又坐了幾站地鐵，終于到了繁華的市中心。那里真是人山人海，高樓林立，不愧是國際大都市。我品嘗了許多美食，乘坐了古老的電車，看到了許多的外國人和豪宅豪車，真是令人目不暇接。

asi=lai(ωn×ui-ωn·ωn·ui)

(20)

(21)

2.2 動力學分析

根據(jù)質(zhì)點的角動量定理可得出本機構(gòu)中作用于定平臺鉸點Ai的合外力矩Mn與支鏈作用于該點的合角動量Lh的關(guān)系為

(22)

已知電動缸的質(zhì)量為mai，電動桿的質(zhì)量為mbi。

式(22)中的合角動量Lh表示為

Lh=Ma+Mn+mailai(ui×vsi)+mbi(Li-lbi)(ui×vxi)

(23)

式中，電動缸質(zhì)心和電動推桿質(zhì)心的角動量為Ma=Isiωn，Mb=Ixiωn，Isi和Ixi分別為電動缸和電動推桿關(guān)于各自質(zhì)心的慣性矩陣。

式(23)對時間求導并整理得

(24)

令

(25)

定平臺對支鏈的約束力矩為Mw；支鏈對動平臺的作用力為連桿對電動缸的作用力矩為Mzh；電動缸對連桿作用力矩為-Mzh；fbi=[fbi,dfbi,ffbi,g]T為動平臺對支鏈的作用力；g=[0 0gzz]T為重力加速度的向量。那么電動缸所受到的合力矩Ms和連桿所受到的合力矩Mx的表達式分別為

Ms=mailai(ui×g)+Mwui+Mzh

(26)

Mx=mbi(Li-lbi)(ui×g)+Liui×(fbi)-Mzh

(27)

式(25)與式(26)合并相加，得到單個支鏈整體合外力矩Mn，整理得

Mn=[mailai+mbi(Li-lbi)(ui×g)+Liui×fbi+Mwui]

(28)

將式(24)、式(25)和式(28)代入到式(22)中可得單個支鏈的歐拉方程為

(29)

式中，mB為動平臺的質(zhì)量。以動平臺為研究對象，建立動平臺的力平衡方程，根據(jù)牛頓第二定律得

(30)

根據(jù)歐拉方程，動平臺質(zhì)心處的合力矩方程為

(31)

式中，rci為動平臺上的第i個支鏈的位置矢量；Nm為動平臺關(guān)于質(zhì)心的合力矩，即動平臺的慣性力矩。

(32)

式中，Im為動坐標系下，動平臺關(guān)于其自身質(zhì)心的慣性矩陣。

將(32)代入(31)中得

(33)

通過聯(lián)立式(29)、式(30)和式(33)可求出約束力bbi。

(34)

3 仿真

2(6UPUR+3P)并聯(lián)機器人邁步腿的結(jié)構(gòu)是對稱的，且每條支鏈完全相同。機器人的幾何參數(shù)及物理參數(shù)見表1。

表1 機器人的幾何參數(shù)及物理參數(shù)

根據(jù)表1的參數(shù)，可以根據(jù)圖1所示的結(jié)構(gòu)獲取動平臺質(zhì)心、電動缸質(zhì)心和電動推桿質(zhì)心的慣性矩陣(單位:kg·mm2)依次為

為了增加機器人步行的穩(wěn)定性和減少起始和終止時足端與地面的沖擊，規(guī)劃了一種基于五次多項式的足端軌跡，則機器人邁步時起始和著地時終止的速度和加速度均為0。令機器人單次邁步距離為dx=100 mm，高度為dz=30 mm，即

(35)

其中，Et=6t3-15t4+10t5，t∈[0，1]。

為了驗證2(6UPUR+3P)并聯(lián)機器人動力學模型的正確性并分析其動力學特性，在Simulink和SimMechanics中設(shè)計了物理模型，得到的仿真曲線進行驗證，如圖7所示。

圖7 SimMechanics模型仿真圖

由圖8～圖10可以看出，SimMechanics環(huán)境下的動力學模型與數(shù)學模型的理論值一致，對比后的腿部各支鏈的誤差在-0.3 N到0.2 N之間，此誤差可歸結(jié)于電動缸與電動推桿質(zhì)心的位置與仿真環(huán)境下測量而導致仿真值產(chǎn)生偏差。

圖8 數(shù)學模型仿真得到的驅(qū)動力

圖9 SimMechanics仿真得到的驅(qū)動力

圖10 兩種模型對比的驅(qū)動力誤差圖

4 結(jié)論

本文構(gòu)建了基于2(6UPUR+3P)型的六足步行機器人的進行了結(jié)構(gòu)設(shè)計，以此六足步行機器人為研究對象，首先運用矢量法進行運動學分析，基于此，通過牛頓-歐拉法進行動力學建模，對機器人的動態(tài)特性進行分析；其次在SimMechanics中搭建了六足步行機器人的動力學模型；最后進行了仿真分析，仿真結(jié)果表明，在2種環(huán)境下所獲得的腿部的動力學響應(yīng)曲線是一致的，說明了在matlab和SimMechanics下的動力學模型都是正確的。