一種電驅(qū)動(dòng)六足機(jī)器人運(yùn)動(dòng)控制系統(tǒng)及方法
【專利摘要】一種電驅(qū)動(dòng)六足機(jī)器人運(yùn)動(dòng)控制系統(tǒng)及方法，本發(fā)明涉及六足機(jī)器人運(yùn)動(dòng)控制領(lǐng)域，本發(fā)明要解決器人普遍存在自主靈活度不高，整體適應(yīng)性差，運(yùn)動(dòng)控制響應(yīng)速度低，對(duì)工作環(huán)境的依賴性強(qiáng)等缺點(diǎn)以及自由度繁多從而增加了控制系統(tǒng)的復(fù)雜性的問題，控制系統(tǒng)由足式模塊和輪式模塊組成，該系統(tǒng)具體是按照以下步驟進(jìn)行的：1、建立六足機(jī)器人進(jìn)行建模模塊；2、建立坐標(biāo)系運(yùn)算模塊；3、運(yùn)動(dòng)控制器控制伺服電機(jī)進(jìn)行精確的位置運(yùn)動(dòng)；位姿運(yùn)動(dòng)模塊運(yùn)用機(jī)器人坐標(biāo)變換矩陣確定機(jī)器人平臺(tái)質(zhì)心變化；4、實(shí)現(xiàn)輪式系統(tǒng)的前進(jìn)、后退、左轉(zhuǎn)和右轉(zhuǎn)等步驟實(shí)現(xiàn)的。本發(fā)明應(yīng)用于六足機(jī)器人運(yùn)動(dòng)控制領(lǐng)域。
【專利說明】—種電驅(qū)動(dòng)六足機(jī)器人運(yùn)動(dòng)控制系統(tǒng)及方法

【技術(shù)領(lǐng)域】
[0001]本發(fā)明涉及一種電驅(qū)動(dòng)六足機(jī)器人運(yùn)動(dòng)控制系統(tǒng)及方法。

【背景技術(shù)】
[0002]電驅(qū)動(dòng)六足機(jī)器人運(yùn)動(dòng)控制系統(tǒng)是足式系統(tǒng)的核心單元，承擔(dān)著將來自操縱者的指令轉(zhuǎn)化為具體的驅(qū)動(dòng)單元運(yùn)動(dòng)的重要任務(wù)。以保證平臺(tái)運(yùn)動(dòng)穩(wěn)定性、靈活性以及操作便易性為追求目標(biāo)，運(yùn)動(dòng)控制系統(tǒng)的設(shè)計(jì)重點(diǎn)關(guān)注了系統(tǒng)行走功能與操作性能的對(duì)應(yīng)與互補(bǔ)。
[0003]目前，我國足式機(jī)器人起步較晚，機(jī)器人普遍存在自主靈活度不高，整體適應(yīng)性差，運(yùn)動(dòng)控制響應(yīng)速度低，對(duì)工作環(huán)境的依賴性強(qiáng)等缺點(diǎn)。六足機(jī)器人在結(jié)構(gòu)上屬于串并聯(lián)混合結(jié)構(gòu)，在控制方面屬于多輸入多輸出的冗余耦合系統(tǒng)，六足機(jī)器人由于自由度繁多從而增加了控制系統(tǒng)的復(fù)雜性。


【發(fā)明內(nèi)容】

[0004]本發(fā)明的目的是為了解決機(jī)器人普遍存在自主靈活度不高，整體適應(yīng)性差，運(yùn)動(dòng)控制響應(yīng)速度低，對(duì)工作環(huán)境的依賴性強(qiáng)等缺點(diǎn)以及自由度繁多從而增加了控制系統(tǒng)的復(fù)雜性的問題而提出了一種電驅(qū)動(dòng)六足機(jī)器人運(yùn)動(dòng)控制系統(tǒng)及方法。
[0005]上述的發(fā)明目的是通過以下技術(shù)方案實(shí)現(xiàn)的:
[0006]一種電驅(qū)動(dòng)六足機(jī)器人運(yùn)動(dòng)控制系統(tǒng)包括:足式模塊和輪式模塊；
[0007]所述的足式模塊包括:六足機(jī)器人建模模塊、坐標(biāo)系運(yùn)算模塊、步態(tài)運(yùn)動(dòng)模塊、旋轉(zhuǎn)運(yùn)動(dòng)模塊、位姿運(yùn)動(dòng)模塊和單腿運(yùn)動(dòng)模塊；
[0008]一種電驅(qū)動(dòng)六足機(jī)器人運(yùn)動(dòng)控制方法具體是按照以下步驟完成的:
[0009]步驟一、建立六足機(jī)器人進(jìn)行建模模塊，將機(jī)器人機(jī)體簡化成一個(gè)平板和將各腿及驅(qū)動(dòng)電機(jī)簡化為桿件的默認(rèn)狀態(tài)下選擇機(jī)器人參數(shù)；
[0010]步驟二、根據(jù)設(shè)置的機(jī)器人參數(shù)，利用機(jī)器人腿部的正逆運(yùn)動(dòng)學(xué)建立坐標(biāo)系運(yùn)算模塊；
[0011]步驟三、步態(tài)運(yùn)動(dòng)模塊和旋轉(zhuǎn)運(yùn)動(dòng)模塊運(yùn)用低沖擊軌跡規(guī)劃法確定步態(tài)運(yùn)動(dòng)的足末端軌跡和旋轉(zhuǎn)運(yùn)動(dòng)的足末端軌跡；單腿運(yùn)動(dòng)模塊根據(jù)腿部正逆運(yùn)動(dòng)學(xué)確定足末端軌跡，根據(jù)足末端軌跡利用坐標(biāo)系運(yùn)算模塊計(jì)算得出足末端位置的笛卡爾坐標(biāo)F[Xf，Yf, Zf]，將足末端位置的笛卡爾坐標(biāo)提供給PMAC運(yùn)動(dòng)控制器，運(yùn)動(dòng)控制器控制伺服電機(jī)進(jìn)行精確的位置運(yùn)動(dòng)；位姿運(yùn)動(dòng)模塊運(yùn)用機(jī)器人坐標(biāo)變換矩陣確定機(jī)器人平臺(tái)質(zhì)心變化，通過各腿的同時(shí)運(yùn)動(dòng)使機(jī)器人平臺(tái)發(fā)生平移或傾斜；
[0012]其中，步態(tài)運(yùn)動(dòng)模塊和旋轉(zhuǎn)運(yùn)動(dòng)模塊運(yùn)用低沖擊軌跡規(guī)劃法確定步態(tài)運(yùn)動(dòng)的足末端軌跡和旋轉(zhuǎn)運(yùn)動(dòng)的足末端軌跡具體過程為:
[0013](I)設(shè)足端X軸坐標(biāo)px，
[0014]px = S(I)
[0015]式中，S為支撐寬度，即足端與機(jī)體的橫向距離；
[0016](2)設(shè)足端y軸坐標(biāo)py，
[0017]時(shí)單腿處于擺動(dòng)相，令足端在y(1方向的位置y、速度和加速度ji，需要滿足以下約束條件:

v(0) = v(|-) = O
v(0) = (丄} = O
[0018]^ (2)
ν(ι,)—-—一―
?2
v(——)=—
2— 2
[0019]式中，T為步態(tài)周期，λ為半步長；令:y。具體為足端在y方向的初始位置；
[0020]y(i) - C1' Sin(^1Z)(3)
[0021]式中，C1為常數(shù)，Co1為角頻率；式(3)兩次積分有:
[0022]j(0 = cos(c^i) + c%

mI
[0023].ν(0 =+ Cji + Cn(4)

COf
[0024]式中，C2和C3為常數(shù)；結(jié)合式⑵的約束條件求出C1 = 2 λ ο^/Τ，C2 = 2 λ /T，C3=- λ/2, Co1 = 2η π/T, η = 2，4，…,取η = 2,故單腿處于擺動(dòng)相時(shí)足端坐標(biāo)py的軌跡方程為:
Γ ?a.4n , 21 IT、
「r\ cH___Cl Tlf_t\ -4__奮 ____4 仁 / ?\ _\
LUUZD」―"I~i5inIi t^/
2.71 Iι JfJ.
[0026]ie(|T,T)時(shí)單腿處于支撐相，足端在y(l方向的運(yùn)動(dòng)為擺動(dòng)相時(shí)的逆運(yùn)動(dòng)，故將式
(5)中的t替換為T-t，則腿部處于支撐相時(shí)有:
r π1.^ 4π、 21 31J
[0027]=--— —i) ~~ -----1 + — t ε {—，i) 1())
2 TCT T 22
[0028](3)足端坐標(biāo)pz
[0029]t e (O,-j Γ)時(shí)，足端從地面抬至最1?點(diǎn)，令ζ、?和￡分別為足端在Zq方向的位直、速度和加速度，需滿足:
'ζ(())^0
T
i(0):1(?) = 0
[0030]40) = -1/(7):(丄)=1 —Η
sAf Ttiax
[0031]式中，hmax為足端最大抬起1?度；令:
[0032]?(?) = C1* - sin(,*i)(8)
[0033]式中，c' 1為常數(shù)，《' 頻率；式(8)兩次積分，同理求得c' ! = ^fflaxW ^ JT,c' 2 = 4hmax/T, C1 3 =-H, ω ' ι = 2η π/T, η = 4，8，…;取 η = 4,則足端坐標(biāo) ρζ 的軌跡方程為:
[0034]z(t) = ^issLsin(^i) + ? - H te(0,-)(9)
2fi T T4
[0035]ΙΕ_Τ?|Τ)時(shí)，足端從最高點(diǎn)落回地面，其在Ztl方向的運(yùn)動(dòng)為?..("」T')時(shí)的逆運(yùn)動(dòng)，將式(9)中的t替換為T/2-t，則該階段？2的軌跡方程為:z0具體為足端在ζ方向的初始位置；
L.QAL.下了
[0036]=Usmx ^ff re(--) (1)
2πTT4 2
[0037]ie({T,T)時(shí)單腿處于支撐相，pz = -H ;得到的足端軌跡曲線和單腿足端速度曲

I*
線.
[0038]步驟四、輪式運(yùn)動(dòng)通過設(shè)定輪式系統(tǒng)的運(yùn)動(dòng)時(shí)間、速度及轉(zhuǎn)彎角速度等進(jìn)行輪式模塊的設(shè)定，實(shí)現(xiàn)輪式系統(tǒng)的前進(jìn)、后退、左轉(zhuǎn)和右轉(zhuǎn)功能；從而建立六足機(jī)器人運(yùn)動(dòng)控制系統(tǒng)；即完成了一種電驅(qū)動(dòng)六足機(jī)器人運(yùn)動(dòng)控制方法。
[0039]發(fā)明效果
[0040]通過對(duì)六足機(jī)器人運(yùn)動(dòng)控制系統(tǒng)的研究，設(shè)計(jì)了一個(gè)靈活性好，適應(yīng)性強(qiáng)，足地接觸沖擊低、且能實(shí)現(xiàn)多自由度協(xié)調(diào)運(yùn)動(dòng)的六足機(jī)器人運(yùn)動(dòng)控制系統(tǒng)。該系統(tǒng)首先解析來自操控系統(tǒng)的一系列指令，結(jié)合環(huán)境感知反饋與綜合分析，生成機(jī)體位姿、主運(yùn)動(dòng)參數(shù)和步態(tài)參數(shù)。運(yùn)動(dòng)規(guī)劃將根據(jù)操縱系統(tǒng)的指令信息，規(guī)劃出合理的機(jī)身位姿變化規(guī)律，通過步態(tài)生成算法細(xì)化得到機(jī)體質(zhì)心運(yùn)動(dòng)軌跡、其足端運(yùn)動(dòng)軌跡如圖5所示，擺動(dòng)相軌跡及支撐相軌跡。由運(yùn)動(dòng)規(guī)劃、反饋修正所形成的最終控制量全部統(tǒng)一為足端位移，進(jìn)而依照腿部逆運(yùn)動(dòng)學(xué)解算為所有腿部關(guān)節(jié)角位移，其單腿三關(guān)節(jié)關(guān)節(jié)角度如圖6所示，其單腿足端速度如圖14所示。這種規(guī)劃使得足端與關(guān)節(jié)運(yùn)動(dòng)平滑無沖擊，足端與關(guān)節(jié)在AEP和PEP位置的加速度和速度為零，足端與關(guān)節(jié)的速度和加速度連續(xù)無突變，并最終由底層關(guān)節(jié)位置伺服控制實(shí)現(xiàn)運(yùn)動(dòng)輸出。為了提高機(jī)器人在復(fù)雜環(huán)境下的運(yùn)行可靠性，控制系統(tǒng)也為人工直接干預(yù)預(yù)留接口，便于當(dāng)機(jī)器人在極端復(fù)雜環(huán)境中行走時(shí)直接操控單腿運(yùn)動(dòng)。
[0041]本運(yùn)動(dòng)控制平臺(tái)是以UMAC控制器為基礎(chǔ)，結(jié)合我們對(duì)電驅(qū)動(dòng)六足機(jī)器人的一些理論研究，進(jìn)一步開發(fā)的成果。該運(yùn)動(dòng)控制平臺(tái)包含了六足機(jī)器人建模模塊、坐標(biāo)系運(yùn)算模塊、步態(tài)運(yùn)動(dòng)模塊、旋轉(zhuǎn)運(yùn)動(dòng)模塊、位姿運(yùn)動(dòng)模塊、單腿運(yùn)動(dòng)模塊及輪式運(yùn)動(dòng)模塊等7個(gè)模塊組成。
[0042]結(jié)合運(yùn)動(dòng)控制器的特點(diǎn)，我們將運(yùn)動(dòng)控制程序分成三級(jí)，分別為主控級(jí)、職能控制級(jí)和運(yùn)動(dòng)執(zhí)行級(jí)。由主控級(jí)來完成系統(tǒng)參數(shù)的初始化，啟動(dòng)實(shí)時(shí)位置顯示，接收操控的命令并按照命令啟動(dòng)和停止職能控制級(jí)相應(yīng)運(yùn)動(dòng)程序；職能控制級(jí)由完成特定功能的PLC程序組成，由主控級(jí)啟動(dòng)，完成運(yùn)動(dòng)數(shù)據(jù)的轉(zhuǎn)換和組織，運(yùn)動(dòng)的節(jié)律控制，運(yùn)動(dòng)執(zhí)行級(jí)程序的啟動(dòng)等；運(yùn)動(dòng)執(zhí)行級(jí)為坐標(biāo)系所能執(zhí)行的Program運(yùn)動(dòng)程序，包含了各軸的運(yùn)動(dòng)形式，運(yùn)動(dòng)距離和運(yùn)動(dòng)時(shí)間等具體運(yùn)動(dòng)控制，如圖1示。整個(gè)運(yùn)動(dòng)控制過程不依賴工作環(huán)境，在運(yùn)動(dòng)控制器中，有兩種可用于程序運(yùn)算的變量，P變量和Q變量。P變量是全局變量，可用于運(yùn)動(dòng)程序中和PLC程序中；Q變量是坐標(biāo)系變量，用于坐標(biāo)系運(yùn)算程序中。

【專利附圖】

【附圖說明】
[0043]圖1是【具體實(shí)施方式】一提出的運(yùn)動(dòng)控制系統(tǒng)設(shè)計(jì)結(jié)構(gòu)示意圖；
[0044]圖2是【具體實(shí)施方式】二提出的機(jī)器人機(jī)構(gòu)示意圖；
[0045]圖3是具體方式五提出的機(jī)器人二步態(tài)行走示意圖；其中，oi (i = 1..6)分別代表為機(jī)器人的六條腿；機(jī)器人一共六條腿，六條腿分成兩組，分別為O組和I組，O組擺動(dòng)相，I組對(duì)應(yīng)支撐相；
[0046]圖4是具體方式五提出的機(jī)器人運(yùn)動(dòng)相序圖；其中，擺動(dòng)相為機(jī)器人行走時(shí)腿端抬離地面并向前擺動(dòng)的過程；支撐相為機(jī)器人行走時(shí)腿端支撐地面并推動(dòng)機(jī)體向前移動(dòng)的過程；
[0047]圖5是具體方式四提出的足端運(yùn)動(dòng)軌跡示意圖；
[0048]圖6是具體方式四提出的單腿三關(guān)節(jié)關(guān)節(jié)角度示意圖；
[0049]圖7(a)是具體方式四提出的六足機(jī)器人單腿側(cè)視圖坐標(biāo)系示意圖；圖7(b)是具體方式四提出的六足機(jī)器人單腿俯視圖坐標(biāo)系示意圖；圖7((:)是具體方式四提出的六足機(jī)器人單簡化圖坐標(biāo)系示意圖；六足機(jī)器人腿部由三部分組成:根關(guān)節(jié)、髖關(guān)節(jié)及膝關(guān)節(jié)；腿部笛卡爾坐標(biāo)系如圖7 (a)和圖7 (b)所不，X軸代表機(jī)體的前進(jìn)方向，Y軸代表機(jī)體的橫向方向，Z軸代表機(jī)體的高度方向，H。代表軀干初始高度，腿部的橫向距離為S，α、β、Y分別為根、髖和膝關(guān)節(jié)的轉(zhuǎn)角；機(jī)器人單腿機(jī)構(gòu)的簡化如圖7(c)所示，F(xiàn)點(diǎn)為小腿末端，U、L2、L3分別為根部、大腿和小腿的尺寸；R為機(jī)器人六邊形平臺(tái)的半徑；
[0050]圖8是【具體實(shí)施方式】四提出的六足機(jī)器人平臺(tái)全局坐標(biāo)系與單腿坐標(biāo)簡化示意圖；其中，六足機(jī)器人平臺(tái)分為全局坐標(biāo)系和局部坐標(biāo)系:平臺(tái)全局坐標(biāo)系是以平臺(tái)O圓心，X軸、Y軸和Z軸滿足右手螺旋法則，Z為沿著重力反方向的坐標(biāo)軸，X為沿著機(jī)體前進(jìn)方向的坐標(biāo)軸，Y為沿著機(jī)體橫向的坐標(biāo)軸，平臺(tái)全局坐標(biāo)系用于描述各個(gè)腿與平臺(tái)間的位置關(guān)系；平臺(tái)局部坐標(biāo)系分別在六條腿上，基節(jié)位置是坐標(biāo)系的原點(diǎn)，六條腿分別為oi α =
1..6) ,xi (i = 1..6)軸和yi (i = 1..6)軸分別是經(jīng)過坐標(biāo)平移和旋轉(zhuǎn)得來的單腿方向坐標(biāo)軸，相對(duì)于六個(gè)坐標(biāo)系分別為CSi (i = 1..6)；
[0051]圖9是【具體實(shí)施方式】六提出的六足機(jī)器人二步態(tài)旋轉(zhuǎn)示意圖；其中，此圖的前、后極限位置:在機(jī)器人足端所能夠達(dá)到的區(qū)域范圍內(nèi)，相對(duì)于機(jī)體向前所達(dá)到的極限位置稱為前極限位置(AEP)，相對(duì)于機(jī)體向后所達(dá)到的極限位置稱為后極限位置(PEP);機(jī)器人繞X軸旋轉(zhuǎn)Θ角度；
[0052]圖10是【具體實(shí)施方式】九提出的六足機(jī)器人位姿運(yùn)動(dòng)示意圖；其中，F(xiàn)為足端坐標(biāo)，假設(shè)機(jī)器人繞X軸旋轉(zhuǎn)Θ角度，六足末端位置在平臺(tái)全局坐標(biāo)系中的坐標(biāo)為[Xi, Yi, Zi],通過旋轉(zhuǎn)變換矩陣，將平臺(tái)全局坐標(biāo)系旋轉(zhuǎn)Θ度，將足末端位置在新的平臺(tái)坐標(biāo)系中表示出來[X' u2' i]，在原平臺(tái)全局坐標(biāo)系中，將各足末端位置D^YpZi]運(yùn)動(dòng)到新位置[X' J，即機(jī)器人完成指定位姿運(yùn)動(dòng)；
[0053]圖11是【具體實(shí)施方式】三提出的六足機(jī)器人足式模式主程序流程圖；
[0054]圖12是【具體實(shí)施方式】七提出的六足機(jī)器人步態(tài)程序流程圖；
[0055]圖13是【具體實(shí)施方式】七提出的單腿控制程序流程圖；
[0056]圖14是【具體實(shí)施方式】四提出的單腿足端速度曲線示意圖；其中，y方向?yàn)闄C(jī)體前進(jìn)方向，X方向?yàn)闄C(jī)體側(cè)向方向，Z方向?yàn)闄C(jī)體垂直方向；擺動(dòng)相時(shí)為連續(xù)平滑的弧形，支撐相時(shí)為直線。

【具體實(shí)施方式】
[0057]【具體實(shí)施方式】一:本實(shí)施方式的一種電驅(qū)動(dòng)六足機(jī)器人運(yùn)動(dòng)控制系統(tǒng)包括:足式模塊和輪式模塊；
[0058]所述的足式模塊包括:六足機(jī)器人建模模塊、坐標(biāo)系運(yùn)算模塊、步態(tài)運(yùn)動(dòng)模塊、旋轉(zhuǎn)運(yùn)動(dòng)模塊、位姿運(yùn)動(dòng)模塊和單腿運(yùn)動(dòng)模塊。
[0059]本實(shí)施方式效果:
[0060]通過對(duì)六足機(jī)器人運(yùn)動(dòng)控制系統(tǒng)的研究，設(shè)計(jì)了一個(gè)靈活性好，適應(yīng)性強(qiáng)，足地接觸沖擊低、且能實(shí)現(xiàn)多自由度協(xié)調(diào)運(yùn)動(dòng)的六足機(jī)器人運(yùn)動(dòng)控制系統(tǒng)。該系統(tǒng)首先解析來自操控系統(tǒng)的一系列指令，結(jié)合環(huán)境感知反饋與綜合分析，生成機(jī)體位姿、主運(yùn)動(dòng)參數(shù)和步態(tài)參數(shù)。運(yùn)動(dòng)規(guī)劃將根據(jù)操縱系統(tǒng)的指令信息，規(guī)劃出合理的機(jī)身位姿變化規(guī)律，通過步態(tài)生成算法細(xì)化得到機(jī)體質(zhì)心運(yùn)動(dòng)軌跡、其足端運(yùn)動(dòng)軌跡如圖5所示，擺動(dòng)相軌跡及支撐相軌跡。由運(yùn)動(dòng)規(guī)劃、反饋修正所形成的最終控制量全部統(tǒng)一為足端位移，進(jìn)而依照腿部逆運(yùn)動(dòng)學(xué)解算為所有腿部關(guān)節(jié)角位移，其單腿三關(guān)節(jié)關(guān)節(jié)角度如圖6所示，其單腿足端速度如圖14所示。這種規(guī)劃使得足端與關(guān)節(jié)運(yùn)動(dòng)平滑無沖擊，足端與關(guān)節(jié)在AEP和PEP位置的加速度和速度為零，足端與關(guān)節(jié)的速度和加速度連續(xù)無突變，并最終由底層關(guān)節(jié)位置伺服控制實(shí)現(xiàn)運(yùn)動(dòng)輸出。為了提高機(jī)器人在復(fù)雜環(huán)境下的運(yùn)行可靠性，控制系統(tǒng)也為人工直接干預(yù)預(yù)留接口，便于當(dāng)機(jī)器人在極端復(fù)雜環(huán)境中行走時(shí)直接操控單腿運(yùn)動(dòng)。
[0061]本運(yùn)動(dòng)控制平臺(tái)是以UMAC控制器為基礎(chǔ)，結(jié)合我們對(duì)電驅(qū)動(dòng)六足機(jī)器人的一些理論研究，進(jìn)一步開發(fā)的成果。該運(yùn)動(dòng)控制平臺(tái)包含了六足機(jī)器人建模模塊、坐標(biāo)系運(yùn)算模塊、步態(tài)運(yùn)動(dòng)模塊、旋轉(zhuǎn)運(yùn)動(dòng)模塊、位姿運(yùn)動(dòng)模塊、單腿運(yùn)動(dòng)模塊及輪式運(yùn)動(dòng)模塊等7個(gè)模塊組成。
[0062]結(jié)合運(yùn)動(dòng)控制器的特點(diǎn)，我們將運(yùn)動(dòng)控制程序分成三級(jí)，分別為主控級(jí)、職能控制級(jí)和運(yùn)動(dòng)執(zhí)行級(jí)。由主控級(jí)來完成系統(tǒng)參數(shù)的初始化，啟動(dòng)實(shí)時(shí)位置顯示，接收操控的命令并按照命令啟動(dòng)和停止職能控制級(jí)相應(yīng)運(yùn)動(dòng)程序；職能控制級(jí)由完成特定功能的PLC程序組成，由主控級(jí)啟動(dòng)，完成運(yùn)動(dòng)數(shù)據(jù)的轉(zhuǎn)換和組織，運(yùn)動(dòng)的節(jié)律控制，運(yùn)動(dòng)執(zhí)行級(jí)程序的啟動(dòng)等；運(yùn)動(dòng)執(zhí)行級(jí)為坐標(biāo)系所能執(zhí)行的Program運(yùn)動(dòng)程序，包含了各軸的運(yùn)動(dòng)形式，運(yùn)動(dòng)距離和運(yùn)動(dòng)時(shí)間等具體運(yùn)動(dòng)控制，如圖1示。整個(gè)運(yùn)動(dòng)控制過程不依賴工作環(huán)境，在運(yùn)動(dòng)控制器中，有兩種可用于程序運(yùn)算的變量，P變量和Q變量。P變量是全局變量，可用于運(yùn)動(dòng)程序中和PLC程序中；Q變量是坐標(biāo)系變量，用于坐標(biāo)系運(yùn)算程序中。
[0063]【具體實(shí)施方式】二:本實(shí)施方式與【具體實(shí)施方式】一不同的是:所述的六足機(jī)器人建模模塊用于直觀的看到機(jī)器人構(gòu)型，計(jì)算機(jī)器人各條腿的關(guān)節(jié)角度以及機(jī)器人腿部運(yùn)動(dòng)學(xué)解算；機(jī)器人機(jī)構(gòu)示意圖如圖2所示；其中，六足機(jī)器人腿部由跟關(guān)節(jié)、髖關(guān)節(jié)及膝關(guān)節(jié)三部分組成；
[0064]所述的足式模塊用于包括二步態(tài)行走、三步態(tài)行走、六步態(tài)行走、越溝態(tài)行走、爬坡態(tài)行走、越障態(tài)行走、原地旋轉(zhuǎn)、姿態(tài)調(diào)整、單腿控制、六足站立以及六足收起功能；
[0065]所述的坐標(biāo)系運(yùn)算模塊用于表示運(yùn)動(dòng)關(guān)系的一組運(yùn)動(dòng)系，被控制的各關(guān)節(jié)定義到坐標(biāo)系的軸上，在運(yùn)動(dòng)程序中執(zhí)行運(yùn)動(dòng)，其中，坐標(biāo)系運(yùn)算是運(yùn)控系統(tǒng)的重要組成部分；
[0066]所述的步態(tài)運(yùn)動(dòng)模塊用于解決支撐相和擺動(dòng)相的時(shí)序問題，結(jié)合低沖擊足端軌跡規(guī)劃方法，用于降低機(jī)器人擺動(dòng)相的足地接觸沖擊，以及支撐運(yùn)動(dòng)和擺動(dòng)運(yùn)動(dòng)的實(shí)現(xiàn)問題；
[0067]所述的旋轉(zhuǎn)運(yùn)動(dòng)模塊用于通過平臺(tái)坐標(biāo)系旋轉(zhuǎn)的方式完成了機(jī)器人繞坐標(biāo)軸的旋轉(zhuǎn)運(yùn)動(dòng)，這里我們通過姿態(tài)和步態(tài)融合的方式形成旋轉(zhuǎn)步態(tài)規(guī)劃；
[0068]所述的位姿運(yùn)動(dòng)模塊用于機(jī)器人完成指定位姿運(yùn)動(dòng)；
[0069]所述的單腿運(yùn)動(dòng)模塊用于單獨(dú)對(duì)機(jī)器人足端和關(guān)節(jié)進(jìn)行控制，使其實(shí)現(xiàn)機(jī)器人的全局坐標(biāo)系和局部坐標(biāo)系下的運(yùn)動(dòng)模式；
[0070]所述的輪式模塊用于實(shí)現(xiàn)輪式系統(tǒng)的前進(jìn)、后退、左轉(zhuǎn)、右轉(zhuǎn)等功能即上位機(jī)輸入電機(jī)的參數(shù)來控制驅(qū)動(dòng)輪的轉(zhuǎn)速，從而達(dá)到機(jī)器人的輪式前行。其它步驟及參數(shù)與【具體實(shí)施方式】一相同。
[0071]【具體實(shí)施方式】三:本實(shí)施方式與【具體實(shí)施方式】一或二不同的是:所述的足式模塊主程序的流程為如圖11所示:
[0072](I)系統(tǒng)對(duì)PLC顯示程序和初始化程序進(jìn)行參數(shù)初始化；
[0073](2)選擇啟動(dòng)原地轉(zhuǎn)向程序、六足站立收起程序、爬坡、越障、越溝程序及其自動(dòng)和手動(dòng)程序，運(yùn)動(dòng)控制程序根據(jù)操控系統(tǒng)的用戶需求進(jìn)行下位機(jī)程序的執(zhí)行；其中，運(yùn)動(dòng)控制程序包括原地轉(zhuǎn)向程序、六足站立收起程序、爬坡、越障、越溝程序及其自動(dòng)和手動(dòng)程序。其它步驟及參數(shù)與【具體實(shí)施方式】一或二相同。
[0074]【具體實(shí)施方式】四:本實(shí)施方式與【具體實(shí)施方式】一至三之一不同的是:一種電驅(qū)動(dòng)六足機(jī)器人運(yùn)動(dòng)控制方法具體是按照以下步驟完成的:
[0075]步驟一、建立六足機(jī)器人進(jìn)行建模模塊，在機(jī)器人建模之前，我們對(duì)機(jī)器人的各部分運(yùn)動(dòng)進(jìn)行了分析，為了減少不必要的運(yùn)算以及碰撞檢測(cè)，對(duì)機(jī)器人作了簡化，將機(jī)器人機(jī)體簡化成一個(gè)平板和將各腿及驅(qū)動(dòng)電機(jī)簡化為桿件的默認(rèn)狀態(tài)下選擇機(jī)器人參數(shù)如圖7(a)?圖7(c)和表2所示；
[0076]表2機(jī)器人機(jī)構(gòu)參數(shù)
[0077]參數(shù)輸_MX_
RelHghi P50)機(jī)體《坆
TransA(P40)+f臺(tái)* I'心距跟關(guān)節(jié)中心距離
Lcnglh H P41)At 節(jié) K 度
Length2(P42)大腿長度
Lcngth3(P43)小腿 K度
MtrISF(PSl)腿部跟關(guān)節(jié)比例因子
Mlr2SF(P52)腿部髖關(guān)節(jié)比例_子
Mlr3SF(P53)疆部膝關(guān)節(jié)比例因子
[0078]
MtrPos_l/2/3,..17/謂(Pl-PW)六條腿1%個(gè)電機(jī)位質(zhì)
[0079]步驟二、根據(jù)機(jī)器人腿部結(jié)構(gòu)分析即設(shè)置的機(jī)器人參數(shù)，如圖7(a)?圖7(c)所示，通過機(jī)器人腿部的正逆運(yùn)動(dòng)學(xué)建立坐標(biāo)系運(yùn)算模塊；
[0080]步驟三、通過低沖擊軌跡規(guī)劃法并融合腿部正逆運(yùn)動(dòng)學(xué)計(jì)算出落足點(diǎn)間的軌跡，步態(tài)運(yùn)動(dòng)模塊和旋轉(zhuǎn)運(yùn)動(dòng)模塊運(yùn)用低沖擊軌跡規(guī)劃法確定步態(tài)運(yùn)動(dòng)的足末端軌跡和旋轉(zhuǎn)運(yùn)動(dòng)的足末端軌跡；單腿運(yùn)動(dòng)模塊根據(jù)腿部正逆運(yùn)動(dòng)學(xué)確定足末端軌跡，根據(jù)足末端軌跡利用坐標(biāo)系運(yùn)算模塊計(jì)算得出足末端位置的笛卡爾坐標(biāo)F[Xf，Yf, Zf]如圖8，將足末端位置的笛卡爾坐標(biāo)提供給PMAC運(yùn)動(dòng)控制器，運(yùn)動(dòng)控制器控制伺服電機(jī)進(jìn)行精確的位置運(yùn)動(dòng)；位姿運(yùn)動(dòng)模塊運(yùn)用機(jī)器人坐標(biāo)變換矩陣確定機(jī)器人平臺(tái)質(zhì)心變化，通過各腿的同時(shí)運(yùn)動(dòng)使機(jī)器人平臺(tái)發(fā)生平移或傾斜；
[0081]其中，低沖擊軌跡規(guī)劃法建立步態(tài)運(yùn)動(dòng)模塊在宏觀上規(guī)劃了六條腿的運(yùn)動(dòng)順序，具體運(yùn)動(dòng)方式靠單腿連貫動(dòng)作來實(shí)現(xiàn)；單腿擺動(dòng)相時(shí)單腿前擺，單腿足端由后極限位置PEP運(yùn)動(dòng)到前極限位置AEP ;支撐相時(shí)單腿后擺，足端由AEP位置回到PEP位置；足端由AEP位置回到PEP位置的兩落足點(diǎn)間的軌跡曲線無窮多，無論采用何種軌跡，須滿足:①足端與關(guān)節(jié)運(yùn)動(dòng)平滑無沖擊足端與關(guān)節(jié)在AEP和PEP位置的加速度和速度為零；③足端與關(guān)節(jié)的速度和加速度連續(xù)無突變；為減小足地接觸沖擊，計(jì)算兩落足點(diǎn)間的軌跡方法為低沖擊足端軌跡規(guī)劃方法:
[0082]所述的步態(tài)運(yùn)動(dòng)模塊和旋轉(zhuǎn)運(yùn)動(dòng)模塊運(yùn)用低沖擊軌跡規(guī)劃法確定步態(tài)運(yùn)動(dòng)的足末端軌跡和旋轉(zhuǎn)運(yùn)動(dòng)的足末端軌跡具體過程為:
[0083](I)設(shè)足端X軸坐標(biāo)Px,
[0084]px = S(I)
[0085]式中，S為支撐寬度，即足端與機(jī)體的橫向距離；
[0086](2)設(shè)足端y軸坐標(biāo)Py，
[0087]時(shí)單腿處于擺動(dòng)相，令足端在yQ方向的位置y、速度#和加速度需要滿足以下約束條件:
V(O)= νφ = 0
_ν(0) = j(丄)=O
[0088] ,2 (2)
A'

^>1 I, I mm..................2
J\ Λ
v(—) = —
；2 2
[0089]式中，T為步態(tài)周期，λ為半步長；令:y。具體為足端在y方向的初始位置
[0090]y(t) = Ci ^sm(ma)(3)
[0091]式中，C1為常數(shù)，ωχ為角頻率；式⑶兩次積分有:
[0092]排)=-—cos^O + C2
*,
1-QQQql v(0 ?--+ C-Λ + C1(4)
L 」。^
[0094]式中，C2和C3為常數(shù)；結(jié)合式(2)的約束條件求出(^ = 2人Q1A, C2 = 2λ/T,c3=- λ /2，Co1 = 2n Ji /Τ，η = 2，4，…;為避免足端頻繁加減速，取η = 2，故單腿處于擺動(dòng)相時(shí)足端坐標(biāo)Py的軌跡方程為:
Γ ? ^ Λ.Jt1、 2Α I /A T' /￡、
[0095]y(i) ~ sm(—?) + —t t e (0?—) (5)
tL.7?, X* XitL.
[0096]Κ.Τ5Τ)時(shí)單腿處于支撐相，足端在％方向的運(yùn)動(dòng)為擺動(dòng)相時(shí)的逆運(yùn)動(dòng)，故將式
(5)中的t替換為T-t，則腿部處于支撐相時(shí)有:
λλ’ 3 *2 ?1T1
「?-1,■ - -§ H ~* X^ X 1W1X't "V
L0097J= ———gin^47i —————I + —— t E (—, I) IsOi)
itJLJl
[0098](3)足端坐標(biāo)pz
[0099]?e(0, jT)時(shí)，足端從地面抬至最尚點(diǎn)，令z、i和f分別為足端在Z0方向的位直、速度和加速度，需滿足:i'(0) = O
z(0) = i{工)=O
[0100]4(7)
L 」 柳=-1/1 ；:今)=U
[0101]式中，hmax為足端最大抬起聞度；令:
[0102]z(t) = c{.sin(ct(f)(8)
[0103]式中，c' 常數(shù)，ω'頻率；式(8)兩次積分，同理可求得c' ! = ^fflaxW ^ JT,c' 2 = 4hmax/T, C1 3 =-H, ω 'ι = 2η π/T, η = 4，8，…;取 η = 4,則足端坐標(biāo) ρζ 的軌跡方程為:
k Hit 4kT
[0104]Z(t) =+ — H ie (0，一) (9)
2π T T4
[0105]? e 4τ,|τ)時(shí)，足端從最高點(diǎn)落回地面，其在Ztl方向的運(yùn)動(dòng)為t e (O^T)時(shí)的逆運(yùn)
> —*4
動(dòng)，將式(9)中的t替換為T/2-t，則該階段？2的軌跡方程為..zQ具體為為足端在ζ方向的初始位置；
AAhT T
[0106]z(f) = --^-sm(4H-^i)-—+ Ihtw^ - H f e (—...—) (10)
[0107]其中，κ(|τ，Τ)時(shí)單腿處于支撐相，ρζ = -H ；
[0108]得到的足端軌跡曲線如圖5所示和單腿足端速度曲線如圖14 ；
[0109]機(jī)器人足端運(yùn)動(dòng)軌跡和單腿三關(guān)節(jié)角度曲線和可選參數(shù)表如圖6和表4所示；
[0110]表4步態(tài)運(yùn)動(dòng)參數(shù)表
[0111]
參數(shù)名稱_MX_
SPLINE樣條_線規(guī)劃
ABS絕對(duì)沿運(yùn)動(dòng)
TM機(jī)器人步態(tài)周期
Gait—Typc(P205)步態(tài)選擇
XDirecl(Q87)規(guī)劃_ X向運(yùn)動(dòng)向S:
YDirect(Q88)規(guī)劃ff<J 7 向運(yùn)動(dòng)向.?
ZDircct(Q89)規(guī)劃的Z向運(yùn)動(dòng)_量
Hordist(P87)足端橫_距離
SwHgh(Pgg)擺動(dòng)相擺動(dòng)的高度
[0112]步驟四、輪式運(yùn)動(dòng)通過設(shè)定輪式系統(tǒng)的運(yùn)動(dòng)時(shí)間、速度及轉(zhuǎn)彎角速度等進(jìn)行輪式模塊的設(shè)定，實(shí)現(xiàn)輪式系統(tǒng)的前進(jìn)、后退、左轉(zhuǎn)、右轉(zhuǎn)等功能；輪式運(yùn)動(dòng)參數(shù)如表5所示:
[0113]表5輪式運(yùn)動(dòng)參數(shù)
[0114]參數(shù)名稱_MX_
LINEAR?￡線插補(bǔ)運(yùn)動(dòng)方式
INC增.Μ:運(yùn)動(dòng)模式
TA運(yùn)動(dòng)加速時(shí)_
TSS _線加速時(shí)_
FORmRDiP 195}輪式前進(jìn)
BACK(P195)輪式 ISa
LTURN(P195)輪式左轉(zhuǎn)
RTURN(P 195)輪式右轉(zhuǎn)
ONLTURN(P195)輪式 Miit 左轉(zhuǎn)
[0115]
ONRTURN(P195)輪式原地^轉(zhuǎn)
[0116]；從而建立六足機(jī)器人運(yùn)動(dòng)控制系統(tǒng)；即完成了一種電驅(qū)動(dòng)六足機(jī)器人運(yùn)動(dòng)控制方法。其它步驟及參數(shù)與【具體實(shí)施方式】一至三之一相同。
[0117]【具體實(shí)施方式】五:本實(shí)施方式與【具體實(shí)施方式】一至四之一不同的是:步驟三中步態(tài)運(yùn)動(dòng)模塊運(yùn)用低沖擊軌跡規(guī)劃法確定步態(tài)運(yùn)動(dòng)的足末端軌跡具體為:
[0118](I)機(jī)器人在足式移動(dòng)階段采用六足移動(dòng)方式，六足六條腿對(duì)應(yīng)18個(gè)自由度；其中，一條腿上有三個(gè)關(guān)節(jié)，三個(gè)關(guān)節(jié)對(duì)應(yīng)三個(gè)自由度，六足六條腿對(duì)應(yīng)18個(gè)自由度；
[0119](2)系統(tǒng)進(jìn)行初始化參數(shù)，選擇啟動(dòng)原地轉(zhuǎn)向程序，六足站立收起程序，爬坡，越障，越溝程序和運(yùn)動(dòng)控制程序，并根據(jù)操控系統(tǒng)的用戶需求進(jìn)行下位機(jī)程序的執(zhí)行；足式模式主程序流程圖如圖11所示；機(jī)體行進(jìn)時(shí)18個(gè)自由度協(xié)調(diào)運(yùn)動(dòng)，達(dá)到指定的運(yùn)動(dòng)軌跡；其中，下位機(jī)程序的執(zhí)行功能程序包括:二步態(tài)子程序、三步態(tài)子程序、六步態(tài)子程序、爬坡態(tài)子程序、越溝態(tài)子程序、越障態(tài)子程序、原地旋轉(zhuǎn)子程序、姿態(tài)調(diào)整子程序、單腿控制子程序、六足站立子程序和六足收起子程序，六足機(jī)器人的步態(tài)運(yùn)動(dòng)是通過腿部持續(xù)的擺動(dòng)或支撐交替運(yùn)動(dòng)實(shí)現(xiàn)的，為了滿足機(jī)器人直線穩(wěn)定運(yùn)動(dòng)，要求各支撐相在支撐運(yùn)動(dòng)中，運(yùn)動(dòng)方向和運(yùn)動(dòng)距離保持一致；步態(tài)規(guī)劃主要解決支撐相和擺動(dòng)相的時(shí)序問題，以及支撐運(yùn)動(dòng)和擺動(dòng)運(yùn)動(dòng)的實(shí)現(xiàn)問題；例如機(jī)器人二步態(tài)行走示意圖如圖3與運(yùn)動(dòng)相序和圖4所示。其它步驟及參數(shù)與【具體實(shí)施方式】一至四之一相同。
[0120]【具體實(shí)施方式】六:本實(shí)施方式與【具體實(shí)施方式】一至五之一不同的是:步驟三中旋轉(zhuǎn)運(yùn)動(dòng)模塊運(yùn)用低沖擊軌跡規(guī)劃法確定旋轉(zhuǎn)運(yùn)動(dòng)的足末端軌跡具體為:
[0121](I)旋轉(zhuǎn)運(yùn)動(dòng)是六足機(jī)器人以機(jī)器平臺(tái)為中心，通過步態(tài)運(yùn)動(dòng)使平臺(tái)在水平方向旋轉(zhuǎn)；旋轉(zhuǎn)步態(tài)和行進(jìn)步態(tài)節(jié)律性相同，采用行進(jìn)步態(tài)的分組和節(jié)律控制方法；
[0122](2)機(jī)器人在旋轉(zhuǎn)運(yùn)動(dòng)中，要求所有的支撐足在整個(gè)支撐過程中相對(duì)位置保持不變，否則機(jī)器人平臺(tái)將受到腿部之間產(chǎn)生的巨大應(yīng)力，有可能損壞機(jī)器人機(jī)械結(jié)構(gòu)；在姿態(tài)運(yùn)動(dòng)中，我們通過平臺(tái)坐標(biāo)系旋轉(zhuǎn)的方式完成了機(jī)器人繞坐標(biāo)軸的旋轉(zhuǎn)運(yùn)動(dòng)，這里我們通過姿態(tài)和步態(tài)融合的方式形成旋轉(zhuǎn)步態(tài)規(guī)劃；六足機(jī)器人二步態(tài)旋轉(zhuǎn)運(yùn)動(dòng)如圖9所示；
[0123](3)擺動(dòng)相的運(yùn)動(dòng)和步態(tài)的相同，即從當(dāng)前點(diǎn)擺動(dòng)到AEP位置；其中，AEP位置的求解方法是，將足末端基準(zhǔn)位點(diǎn)通過坐標(biāo)系反向旋轉(zhuǎn)變換后的新坐標(biāo)點(diǎn)作為AEP點(diǎn)；
[0124]旋轉(zhuǎn)運(yùn)動(dòng)是六足機(jī)器人以平臺(tái)為中心，通過步態(tài)運(yùn)動(dòng)使平臺(tái)在水平方向旋轉(zhuǎn)；旋轉(zhuǎn)步態(tài)和行進(jìn)步態(tài)節(jié)律性相同，因此采用行進(jìn)步態(tài)的分組和節(jié)律控制方法；機(jī)器人在旋轉(zhuǎn)運(yùn)動(dòng)中，要求所有的支撐足在整個(gè)支撐過程中相對(duì)位置保持不變，否則機(jī)器人平臺(tái)將受到腿部之間產(chǎn)生的巨大應(yīng)力，有可能損壞機(jī)器人機(jī)械結(jié)構(gòu)；在姿態(tài)運(yùn)動(dòng)中，我們通過平臺(tái)坐標(biāo)系旋轉(zhuǎn)的方式完成了機(jī)器人繞坐標(biāo)軸的旋轉(zhuǎn)運(yùn)動(dòng)，這里我們通過姿態(tài)和步態(tài)融合的方式形成旋轉(zhuǎn)步態(tài)規(guī)劃；六足機(jī)器人二步態(tài)旋轉(zhuǎn)運(yùn)動(dòng)如圖9所示；
[0125]擺動(dòng)相的運(yùn)動(dòng)和步態(tài)的相同，即從當(dāng)前點(diǎn)擺動(dòng)到AEP位置；AEP位置的求解方法是，將足末端基準(zhǔn)位點(diǎn)通過坐標(biāo)系反向旋轉(zhuǎn)變換后的新坐標(biāo)點(diǎn)作為AEP點(diǎn)；支撐相運(yùn)動(dòng)采用類似位姿規(guī)劃的方法，將足末端點(diǎn)從當(dāng)前位置沿到平臺(tái)坐標(biāo)系中心半徑相等的圓弧運(yùn)動(dòng)，直到平臺(tái)旋轉(zhuǎn)了設(shè)定的角度；
[0126]設(shè)第i = 1，2，3，4，5，6腿當(dāng)前足端點(diǎn)N坐標(biāo)為(Xin，Yin, Zin)，足末端基準(zhǔn)位點(diǎn)O為(xi0, Y1, ZJ，則腿i的AEP位置坐標(biāo)(X耐，Y謹(jǐn)P，ZiAEP)求解如公式(3)所示:
[0127]YiAEP \ = rot(z-eyv\ 4 i(3)

LzJ
[0128]采用這種坐標(biāo)變換的方法，結(jié)合步態(tài)運(yùn)動(dòng)規(guī)劃模塊即解算出機(jī)器人擺動(dòng)相和支撐相的軌跡規(guī)劃。其它步驟及參數(shù)與【具體實(shí)施方式】一至五之一相同。
[0129]【具體實(shí)施方式】七:本實(shí)施方式與【具體實(shí)施方式】一至六之一不同的是:步驟三中機(jī)器人單腿運(yùn)動(dòng)模塊為腿坐標(biāo)系運(yùn)動(dòng)，對(duì)機(jī)器人足端和關(guān)節(jié)進(jìn)行控制:使其實(shí)現(xiàn)機(jī)器人的全局坐標(biāo)系和局部坐標(biāo)系下的運(yùn)動(dòng)模式；單腿運(yùn)控參數(shù)如表5所示:
[0130]表5單腿運(yùn)動(dòng)參數(shù)
[0131]參數(shù)名稱_MX_
LINEAR1?線插補(bǔ)運(yùn)動(dòng)方式
ABS絕對(duì)if(.運(yùn)動(dòng)投式
TA運(yùn)動(dòng)加速時(shí)_
TSS _線加速時(shí)_
StrSpd中.關(guān)節(jié)運(yùn)動(dòng)速嗖
JogFwii+)箏關(guān)節(jié)丨卜:轉(zhuǎn) JogRv(J-) 關(guān)節(jié)反轉(zhuǎn)
XDircct(QB7)全部 X 方 _ 運(yùn)動(dòng) IW M
YDirect(Qgg)全局/+局部Γ方_運(yùn)動(dòng)向量
ZDirecl(Q89)全W/W部Z方向運(yùn)動(dòng)向量
[0132](I)當(dāng)操縱系統(tǒng)執(zhí)行步態(tài)運(yùn)動(dòng)模塊程序時(shí)，步態(tài)運(yùn)動(dòng)模塊程序?qū)⒏鶕?jù)操控系統(tǒng)軟件設(shè)置的參數(shù)值生成控制指令，并根據(jù)控制器的控制指令來完成機(jī)器人步態(tài)行走過程；
[0133]其中所述的機(jī)器人步態(tài)程序流程為:a系統(tǒng)進(jìn)行初始化步態(tài)參數(shù)，b執(zhí)行步態(tài)參數(shù)設(shè)定，c如果設(shè)定失敗，返回到步態(tài)參數(shù)階段，d如果設(shè)定成功則生成步態(tài)參數(shù)，e進(jìn)行起步規(guī)劃判斷，進(jìn)而執(zhí)行起步規(guī)劃程序，其步態(tài)程序流程圖如圖12所示，設(shè)定成功具體為點(diǎn)擊步態(tài)運(yùn)動(dòng)六足機(jī)器人可以運(yùn)動(dòng)即為設(shè)定成功；
[0134](2)當(dāng)操縱系統(tǒng)執(zhí)行單腿運(yùn)動(dòng)模塊程序時(shí)，步態(tài)程序通過PMAC運(yùn)動(dòng)控制器生成控制指令來完成機(jī)器人單腿控制；
[0135]其中單腿控制流程為:a系統(tǒng)進(jìn)行初始化單腿控制參數(shù)；b執(zhí)行單腿控制參數(shù)設(shè)定，如果設(shè)定成功則生成位姿調(diào)整參數(shù)；c選擇在全局坐標(biāo)系下運(yùn)動(dòng)或局部坐標(biāo)系下運(yùn)動(dòng)，當(dāng)選擇之后執(zhí)行坐標(biāo)系程序，其單腿控制流程圖如圖13所示；設(shè)定成功為點(diǎn)擊單腿運(yùn)動(dòng)中的關(guān)節(jié)模式或足端模式六足機(jī)器人可以運(yùn)動(dòng)即為設(shè)定成功；
[0136]結(jié)合運(yùn)動(dòng)控制器的特點(diǎn)，我們將運(yùn)動(dòng)控制程序分成三級(jí)，分別為主控級(jí)、職能控制級(jí)和運(yùn)動(dòng)執(zhí)行級(jí)；由主控級(jí)來完成系統(tǒng)參數(shù)的初始化，啟動(dòng)實(shí)時(shí)位置顯示，接收操控的命令并按照命令啟動(dòng)和停止職能控制級(jí)相應(yīng)運(yùn)動(dòng)程序；職能控制級(jí)由完成特定功能的PLC程序組成，由主控級(jí)啟動(dòng)，完成運(yùn)動(dòng)數(shù)據(jù)的轉(zhuǎn)換和組織，運(yùn)動(dòng)的節(jié)律控制，運(yùn)動(dòng)執(zhí)行級(jí)程序的啟動(dòng)等；運(yùn)動(dòng)執(zhí)行級(jí)為坐標(biāo)系所能執(zhí)行的Program運(yùn)動(dòng)程序，包含了各軸的運(yùn)動(dòng)形式，運(yùn)動(dòng)距離和運(yùn)動(dòng)時(shí)間等具體運(yùn)動(dòng)控制，如圖1示；在運(yùn)動(dòng)控制器中，有兩種可用于程序運(yùn)算的變量，P變量和Q變量；P變量是全局變量，可用于運(yùn)動(dòng)程序中和PLC程序中；Q變量是坐標(biāo)系變量，用于坐標(biāo)系運(yùn)算程序中；運(yùn)動(dòng)控制系統(tǒng)的程序架構(gòu)參數(shù)如下表所示:
[0137] 參數(shù)名稱意義
PLCl中控
PLClO實(shí)時(shí) ?,’?β?
PLC2步態(tài)邏輯控制
PLC5單腿31輯控；M
PLC8旋轉(zhuǎn)麗輯摶M
PLC9姿態(tài)邏輯控M
PLC6輪式邏輯控制
Program2步M運(yùn)動(dòng)控?_
Programs單腿運(yùn)動(dòng)控;M
[0138]
Programg旋轉(zhuǎn)運(yùn)動(dòng)控制
Program9姿態(tài)運(yùn)動(dòng)控制
Program6輪式運(yùn)動(dòng)控制
[0139]。其它步驟及參數(shù)與【具體實(shí)施方式】一至六之一相同。
[0140]【具體實(shí)施方式】八:本實(shí)施方式與【具體實(shí)施方式】一至七之一不同的是:步驟三中利用坐標(biāo)系運(yùn)算模塊計(jì)算得出足末端位置的笛卡爾坐標(biāo)F[Xf，Yf，Zf]具體過程為:
[0141](I)PMAC(Programmable Mult1-Axis Controller可編程多軸運(yùn)動(dòng)控制器)運(yùn)動(dòng)控制器最多支持有16個(gè)坐標(biāo)系；
[0142](2)六足機(jī)器人每條腿有三個(gè)關(guān)節(jié)，每個(gè)關(guān)節(jié)的運(yùn)動(dòng)都關(guān)系到足末端位置，將這三個(gè)關(guān)節(jié)定義到同一坐標(biāo)系的三條軸上，則坐標(biāo)系中足末端位置的笛卡爾坐標(biāo)F[Xf，Yf, Zf]，這樣只需著重考慮坐標(biāo)規(guī)劃(足末端軌跡規(guī)劃)，不需要過多關(guān)注關(guān)節(jié)角度；其中，坐標(biāo)系是有運(yùn)動(dòng)關(guān)系的一組運(yùn)動(dòng)系，被控制的電機(jī)定義到坐標(biāo)系的軸上，在運(yùn)動(dòng)程序中執(zhí)行運(yùn)動(dòng)；坐標(biāo)系運(yùn)算是運(yùn)控系統(tǒng)的重要組成部分；坐標(biāo)系運(yùn)算參數(shù)如表3所示；
[0143]表3坐標(biāo)系運(yùn)算參數(shù)
[0144] 參數(shù)名稱意義
CSPosX J/2/3/4/5/6(Q7)機(jī)器人各隱足求端尤坐餘
CSPosYJ/2/3/4/5/6(Q8)機(jī)器人各腿足求端F坐標(biāo)
CSPosZ^I/2/3/4/5/6(Q9)機(jī)器人各腿足求端Z坐標(biāo)
CSRotJ/2/3/4/5/6(P34-P39)各腿坐標(biāo)系旋轉(zhuǎn)角度
GlbXBia(PSl)機(jī)器人平臺(tái)偏移AlR離
GlbYBia(P82)機(jī)器人+f臺(tái)偏移K ffi—離
GtbZBia(P83)機(jī)器入平臺(tái)偏移Z距離
Matr(Q41-Q46)局部到企局坐標(biāo)系的矩陣參數(shù)
I_(Q51-Q56)-全1.?到I.?靠坐標(biāo)系的矩陣參數(shù)
ForJA/B/C(Q21-Q23)正運(yùn)動(dòng)學(xué)屮二關(guān)節(jié)度
InvJAO/C(Q31-Q33)逆運(yùn)動(dòng)學(xué)中二關(guān)節(jié)角度 C S.....GI obal J/2/3/4/5/6(P 1007-P1057) 全局坐標(biāo)系下各腿位置信息
CSXPOS^l/2/3/4/5/6(QBI)單腿前進(jìn) X 方_4置信息
CSYPOS^I/2/3/4/5/6(Q82)單腿側(cè)_ K 方_位置倍&
CSZPOS?l/2/3/4/5/6(Q83)Φ腿垂 ft Z 方向置信息
[0145](3)假設(shè)已知單腿關(guān)節(jié)轉(zhuǎn)角α、β和Y的角度，根據(jù)單腿關(guān)節(jié)轉(zhuǎn)角求解出機(jī)器人腿部的運(yùn)動(dòng)學(xué)正解一種空間碎片減緩措施評(píng)估及費(fèi)效評(píng)估系統(tǒng)為公式所示:

=(Z1 + [2 cos +i, cos(f - β)) sm a
[0146]? Yf: (L1 + L2 cos β+Li ms{y—β)) cos a ,
[ZF = H + Ι^?ηβ — ￡, sin(/ — β)
[0147]由正運(yùn)動(dòng)學(xué)方程及三角形關(guān)系，求出機(jī)器人腿部的運(yùn)動(dòng)學(xué)逆解為:
a = tan (—.―)


■ >；■ +

Jin^z ζ + K2Α —已
B = XM1--sm (------,_..................::.....................................)
Hy.2L: 2￡,^(//^Z^7^T[_]‘ an-Zl..f + K:m 、，從而求
γ = π~~^ιη (-----十..........................F==:?===.)

21^?.:Ζι? +K:
二 4 f 4研:三:1:-、 ( W _ Yf T I
一 SlH (...........................f 二二)/L —-- 一 Lh

2i2IL2^UJ -Z,y +K2' K cos a J
解出足末端坐標(biāo)為F[Xf，Yf, Zf]，其中，Lp L2, L3分別為根部、大腿和小腿的尺寸。其它步驟及參數(shù)與【具體實(shí)施方式】一至七之一相同。
[0149]【具體實(shí)施方式】九:本實(shí)施方式與【具體實(shí)施方式】一至八之一不同的是:步驟三中位姿運(yùn)動(dòng)模塊運(yùn)用機(jī)器人坐標(biāo)變換矩陣確定機(jī)器人平臺(tái)質(zhì)心變化具體為:
[0150](I)假設(shè)機(jī)器人繞X軸旋轉(zhuǎn)Θ度，六足末端位置在平臺(tái)全局坐標(biāo)系中的坐標(biāo)為(Xi, Yi, Zi),通過旋轉(zhuǎn)變換矩陣，將平臺(tái)全局坐標(biāo)系旋轉(zhuǎn)Θ度，將足末端位置在新的平臺(tái)坐標(biāo)系中表示出來(X' UY' i，Z' i)，在原平臺(tái)全局坐標(biāo)系中，將各足末端位置(UpZi)運(yùn)動(dòng)到新位置(X^ V V i，Z' i)，即機(jī)器人完成指定位姿運(yùn)動(dòng)；機(jī)器人位姿運(yùn)動(dòng)如圖10所示；
[0151](2)假定機(jī)器人繞X軸旋轉(zhuǎn)Θ角度，在X軸坐標(biāo)為X，則新平臺(tái)坐標(biāo)系相對(duì)于原平臺(tái)坐標(biāo)系旋轉(zhuǎn)Θ角度，變換矩陣T和Γ1如公式(4)及(5)所示:
'I O O
[0152]T = mt(x,0) = O cos O ~ sin Θ(4)

O sin O cos Θ
"I O O
[0153]= rot(x,0) '= O cos Θ sin θ(5)

O -sin θ cos θ
[0154]通過變換矩陣，足末端新位置(X' i，Y' UZ' D的表示如公式(6)所示:
);] 網(wǎng) Γ A I
[0155]Y； =Γι Y, = Υ,οο^θ + Ζ,.?ηθ料《其它步驟及參數(shù)與具
^Z； J [zj [― M sin#+ Z; cos 0」
體實(shí)施方式一至八之一相同。
【權(quán)利要求】
1.一種電驅(qū)動(dòng)六足機(jī)器人運(yùn)動(dòng)控制系統(tǒng)，其特征在于:一種電驅(qū)動(dòng)六足機(jī)器人運(yùn)動(dòng)控制系統(tǒng)包括:足式模塊和輪式模塊； 所述的足式模塊包括:六足機(jī)器人建模模塊、坐標(biāo)系運(yùn)算模塊、步態(tài)運(yùn)動(dòng)模塊、旋轉(zhuǎn)運(yùn)動(dòng)模塊、位姿運(yùn)動(dòng)模塊和單腿運(yùn)動(dòng)模塊。
2.根據(jù)權(quán)利要求1所述一種電驅(qū)動(dòng)六足機(jī)器人運(yùn)動(dòng)控制系統(tǒng)，其特征在于: 所述的六足機(jī)器人建模模塊用于直觀的看到機(jī)器人構(gòu)型，計(jì)算機(jī)器人各條腿的關(guān)節(jié)角度以及機(jī)器人腿部運(yùn)動(dòng)學(xué)解算；其中，六足機(jī)器人腿部由跟關(guān)節(jié)、髖關(guān)節(jié)及膝關(guān)節(jié)三部分組成； 所述的足式模式程序用于包括二步態(tài)行走、三步態(tài)行走、六步態(tài)行走、越溝態(tài)行走、爬坡態(tài)行走、越障態(tài)行走、原地旋轉(zhuǎn)、姿態(tài)調(diào)整、單腿控制、六足站立以及六足收起功能； 所述的坐標(biāo)系運(yùn)算模塊用于表示運(yùn)動(dòng)關(guān)系的一組運(yùn)動(dòng)系，被控制的各關(guān)節(jié)定義到坐標(biāo)系的軸上，在運(yùn)動(dòng)程序中執(zhí)彳丁運(yùn)動(dòng)； 所述的步態(tài)運(yùn)動(dòng)模塊用于解決支撐相和擺動(dòng)相的時(shí)序問題，結(jié)合低沖擊足端軌跡規(guī)劃方法，用于降低機(jī)器人擺動(dòng)相的足地接觸沖擊，以及支撐運(yùn)動(dòng)和擺動(dòng)運(yùn)動(dòng)的實(shí)現(xiàn)問題； 所述的旋轉(zhuǎn)運(yùn)動(dòng)模塊用于通過姿態(tài)和步態(tài)融合的方式形成旋轉(zhuǎn)步態(tài)規(guī)劃； 所述的位姿運(yùn)動(dòng)模塊用于機(jī)器人完成指定位姿運(yùn)動(dòng)； 所述的單腿運(yùn)動(dòng)模塊用于單獨(dú)對(duì)機(jī)器人足端和關(guān)節(jié)進(jìn)行控制，使其實(shí)現(xiàn)機(jī)器人的全局坐標(biāo)系和局部坐標(biāo)系下的運(yùn)動(dòng)模式； 所述的輪式模塊用于實(shí)現(xiàn)輪式系統(tǒng)的前進(jìn)、后退、左轉(zhuǎn)、右轉(zhuǎn)功能即上位機(jī)輸入電機(jī)的參數(shù)來控制驅(qū)動(dòng)輪的轉(zhuǎn)速，從而達(dá)到機(jī)器人的輪式前行。
3.根據(jù)權(quán)利要求1所述一種電驅(qū)動(dòng)六足機(jī)器人運(yùn)動(dòng)控制系統(tǒng)，其特征在于:所述的足式模塊主程序的流程為: (1)系統(tǒng)對(duì)PLC顯示程序和初始化程序進(jìn)行參數(shù)初始化； (2)選擇啟動(dòng)原地轉(zhuǎn)向程序、六足站立收起程序、爬坡、越障、越溝程序及其自動(dòng)和手動(dòng)程序，運(yùn)動(dòng)控制程序根據(jù)操控系統(tǒng)的用戶需求進(jìn)行下位機(jī)程序的執(zhí)行；其中，運(yùn)動(dòng)控制程序包括原地轉(zhuǎn)向程序、六足站立收起程序、爬坡、越障、越溝程序及其自動(dòng)和手動(dòng)程序。
4.一種電驅(qū)動(dòng)六足機(jī)器人運(yùn)動(dòng)控制方法，其特征在于:一種電驅(qū)動(dòng)六足機(jī)器人運(yùn)動(dòng)控制方法具體是按照以下步驟完成的: 步驟一、建立六足機(jī)器人進(jìn)行建模模塊，將機(jī)器人機(jī)體簡化成一個(gè)平板和將各腿及驅(qū)動(dòng)電機(jī)簡化為桿件的默認(rèn)狀態(tài)下選擇機(jī)器人參數(shù)； 步驟二、根據(jù)設(shè)置的機(jī)器人參數(shù)，利用機(jī)器人腿部的正逆運(yùn)動(dòng)學(xué)建立坐標(biāo)系運(yùn)算模塊； 步驟三、步態(tài)運(yùn)動(dòng)模塊和旋轉(zhuǎn)運(yùn)動(dòng)模塊運(yùn)用低沖擊軌跡規(guī)劃法確定步態(tài)運(yùn)動(dòng)的足末端軌跡和旋轉(zhuǎn)運(yùn)動(dòng)的足末端軌跡；單腿運(yùn)動(dòng)模塊根據(jù)腿部正逆運(yùn)動(dòng)學(xué)確定足末端軌跡，根據(jù)足末端軌跡利用坐標(biāo)系運(yùn)算模塊計(jì)算得出足末端位置的笛卡爾坐標(biāo)F[Xf，Yf, Zf]，將足末端位置的笛卡爾坐標(biāo)提供給PMAC運(yùn)動(dòng)控制器，運(yùn)動(dòng)控制器控制伺服電機(jī)進(jìn)行精確的位置運(yùn)動(dòng)；位姿運(yùn)動(dòng)模塊運(yùn)用機(jī)器人坐標(biāo)變換矩陣確定機(jī)器人平臺(tái)質(zhì)心變化，通過各腿的同時(shí)運(yùn)動(dòng)使機(jī)器人平臺(tái)發(fā)生平移或傾斜； 其中，步態(tài)運(yùn)動(dòng)模塊和旋轉(zhuǎn)運(yùn)動(dòng)模塊運(yùn)用低沖擊軌跡規(guī)劃法確定步態(tài)運(yùn)動(dòng)的足末端軌跡和旋轉(zhuǎn)運(yùn)動(dòng)的足末端軌跡具體過程為: (1)設(shè)足端X軸坐標(biāo)px， Px = S(I) 式中，S為支撐寬度，即足端與機(jī)體的橫向距離； (2)設(shè)足端y軸坐標(biāo)py， ?Ε(0，|τ)時(shí)單腿處于擺動(dòng)相，令足端在Υ(1方向的位置y、速度#和加速度P，需要滿足以下約束條件:
ν(0)= ?>(-?) = 0
2 ,T
ir(0) = v(—) = 0 ； 'I(2)
p I 孓多 I,,,,,,,,
2 1.-1YT 式中，T為步態(tài)周期，λ為半步長；令:％具體為足端在y方向的初始位置； 廁=C1 - Sin(^1I)(3) 式中，C1為常數(shù)，ω1為角頻率；式(3)兩次積分有:
C
y(t) = CQBimlS) +.?\.y(i)=——Vsin(,i) + c2i + e3 (4) 式中，C2和C3為常數(shù)；結(jié)合式⑵的約束條件求出C1 = 2λ ω/Γ，C2 = 2λ/τ，C3=- λ/2, Co1 = 2η π/T, η = 2，4，…,取η = 2,故單腿處于擺動(dòng)相時(shí)足端坐標(biāo)py的軌跡方程為: /a-_jL?—?\ —t-L.fn Σλ /c\
vf ?》_ ~sm( ~? J "t" "********* t ~ ? ε I o, I c 31 Bi S.^IVΛ\ 3^S r
IXJL ^(|x,T)時(shí)單腿處于支撐相，足端在y。方向的運(yùn)動(dòng)為擺動(dòng)相時(shí)的逆運(yùn)動(dòng)，故將式(5)中的t替換為T-t，則腿部處于支撐相時(shí)有:
A …4%、 2Λ 3A,T ^ (￡X
y(i) ^^ — ij--/ + — ? € (―, i jf (ο)
2冗"XT22 (3)足端坐標(biāo)ρζ ^(Ο,?τ)時(shí)，足端從地面抬至最高點(diǎn)，令ζ、?和2分別為足端在Z0方向的位置、速度和加速度，需滿足:
f{0)? i(工)=O 4(7) Z(O) = -1I' ,
T 式中，hmax為足端最大抬起1?度；令:
Z(I) = Cfl *sin?f)(8) 式中，c' 常數(shù)，ω' 頻率；式(8)兩次積分，同理求得, = 4hmaxo ^ /Γ，C1 2 = 4hmax/T, c' 3 = -H，ω ' χ = 2n Ji /T, η = 4, 8,…；取 η = 4，則足端坐標(biāo) ρζ 的軌跡方程為:
ζ(?) =+ i^!2? I ^ H 16(0,1)(9) i / 2π T T1 4 / le(^T^T)時(shí)，足端從最高點(diǎn)落回地面，其在Ztl方向的運(yùn)動(dòng)為? e (O 士 T)時(shí)的逆運(yùn)動(dòng),將式(9)中的t替換為T/2-t，則該階段？2的軌跡方程為:Z(I具體為足端在ζ方向的初始位置； _ = —tsin{4it—辛 ?> —— if ie(H) (10) ΙΕ‘Τ.Τ)時(shí)單腿處于支撐相，ρζ = -H ;得到的足端軌跡曲線和單腿足端速度曲線；
L 步驟四、輪式運(yùn)動(dòng)通過設(shè)定輪式系統(tǒng)的運(yùn)動(dòng)時(shí)間、速度及轉(zhuǎn)彎角速度等進(jìn)行輪式模塊的設(shè)定，實(shí)現(xiàn)輪式系統(tǒng)的前進(jìn)、后退、左轉(zhuǎn)和右轉(zhuǎn)功能；從而建立六足機(jī)器人運(yùn)動(dòng)控制系統(tǒng)；即完成了 一種電驅(qū)動(dòng)六足機(jī)器人運(yùn)動(dòng)控制方法。
5.根據(jù)權(quán)利要求4所述一種電驅(qū)動(dòng)六足機(jī)器人運(yùn)動(dòng)控制方法，其特征在于:步驟三中步態(tài)運(yùn)動(dòng)模塊運(yùn)用低沖擊軌跡規(guī)劃法確定步態(tài)運(yùn)動(dòng)的足末端軌跡具體為: (1)機(jī)器人在足式移動(dòng)階段采用六足移動(dòng)方式，六足六條腿對(duì)應(yīng)18個(gè)自由度；其中，一條腿上有三個(gè)關(guān)節(jié)，三個(gè)關(guān)節(jié)對(duì)應(yīng)三個(gè)自由度，六足六條腿對(duì)應(yīng)18個(gè)自由度； (2)系統(tǒng)進(jìn)行初始化參數(shù)，選擇啟動(dòng)原地轉(zhuǎn)向程序，六足站立收起程序，爬坡、越障和越溝程序運(yùn)動(dòng)控制程序，并跟據(jù)操控系統(tǒng)的用戶需求進(jìn)行下位機(jī)程序的執(zhí)行；從而機(jī)體行進(jìn)時(shí)18個(gè)自由度協(xié)調(diào)運(yùn)動(dòng)，達(dá)到指定的運(yùn)動(dòng)軌跡；其中，下位機(jī)程序的執(zhí)行功能程序包括:二步態(tài)子程序、三步態(tài)子程序、六步態(tài)子程序、爬坡態(tài)子程序、越溝態(tài)子程序、越障態(tài)子程序、原地旋轉(zhuǎn)子程序、姿態(tài)調(diào)整子程序、單腿控制子程序、六足站立子程序和六足收起子程序。
6.根據(jù)權(quán)利要求5所述一種電驅(qū)動(dòng)六足機(jī)器人運(yùn)動(dòng)控制方法，其特征在于:步驟三中旋轉(zhuǎn)運(yùn)動(dòng)模塊運(yùn)用低沖擊軌跡規(guī)劃法確定旋轉(zhuǎn)運(yùn)動(dòng)的足末端軌跡具體為: (1)旋轉(zhuǎn)運(yùn)動(dòng)是六足機(jī)器人以機(jī)器平臺(tái)為中心，通過步態(tài)運(yùn)動(dòng)使平臺(tái)在水平方向旋轉(zhuǎn)；旋轉(zhuǎn)步態(tài)和行進(jìn)步態(tài)節(jié)律性相同，采用行進(jìn)步態(tài)的分組和節(jié)律控制方法； (2)機(jī)器人在旋轉(zhuǎn)運(yùn)動(dòng)中，要求所有的支撐足在整個(gè)支撐過程中相對(duì)位置保持不變，在姿態(tài)運(yùn)動(dòng)中，通過平臺(tái)坐標(biāo)系旋轉(zhuǎn)的方式完成了機(jī)器人繞坐標(biāo)軸的旋轉(zhuǎn)運(yùn)動(dòng)，通過姿態(tài)和步態(tài)融合的方式形成旋轉(zhuǎn)步態(tài)規(guī)劃； (3)擺動(dòng)相的運(yùn)動(dòng)和步態(tài)的相同，即從當(dāng)前足端點(diǎn)N坐標(biāo)為(Xin，Yin，Zin)擺動(dòng)到AEP位置；其中，AEP位置的求解方法是，將足末端基準(zhǔn)位點(diǎn)通過坐標(biāo)系反向旋轉(zhuǎn)變換后的新坐標(biāo)點(diǎn)作為AEP點(diǎn)； 設(shè)第i = 1，2，3，4，5，6腿當(dāng)前足端點(diǎn)N坐標(biāo)為(Xin，Yin，Zin)，足末端基準(zhǔn)位點(diǎn)O為(Xi0, Y1, ZJ，則腿i的AEP位置坐標(biāo)(X耐，Y謹(jǐn)p，ZiAEP)求解如公式(3)所示: ^iAEP^1 ΥΙΛΕρ =roi(z^ff) 1 Ym(3)77 采用這種坐標(biāo)變換的方法，結(jié)合步態(tài)運(yùn)動(dòng)規(guī)劃模塊即解算出機(jī)器人擺動(dòng)相和支撐相的軌跡規(guī)劃。
7.根據(jù)權(quán)利要求6所述一種電驅(qū)動(dòng)六足機(jī)器人運(yùn)動(dòng)控制方法，其特征在于:步驟三中機(jī)器人單腿運(yùn)動(dòng)模塊為腿坐標(biāo)系運(yùn)動(dòng)，對(duì)機(jī)器人足端和關(guān)節(jié)進(jìn)行控制: (1)當(dāng)操縱系統(tǒng)執(zhí)行步態(tài)運(yùn)動(dòng)模塊程序時(shí)，步態(tài)運(yùn)動(dòng)模塊程序?qū)⒏鶕?jù)操控系統(tǒng)軟件設(shè)置的參數(shù)值生成控制指令，并根據(jù)控制器的控制指令來完成機(jī)器人步態(tài)行走過程； 其中所述的機(jī)器人步態(tài)程序流程為:a系統(tǒng)進(jìn)行初始化步態(tài)參數(shù)，b執(zhí)行步態(tài)參數(shù)設(shè)定，c如果設(shè)定失敗，返回到步態(tài)參數(shù)階段，d如果設(shè)定成功則生成步態(tài)參數(shù)，e進(jìn)行起步規(guī)劃判斷，進(jìn)而執(zhí)行起步規(guī)劃程序； (2)當(dāng)操縱系統(tǒng)執(zhí)行單腿運(yùn)動(dòng)模塊程序時(shí)，步態(tài)程序通過PMAC運(yùn)動(dòng)控制器生成控制指令來完成機(jī)器人單腿控制； 其中單腿控制流程為:a系統(tǒng)進(jìn)行初始化單腿控制參數(shù)山執(zhí)行單腿控制參數(shù)設(shè)定，如果設(shè)定成功則生成位姿調(diào)整參數(shù)；c選擇在全局坐標(biāo)系下運(yùn)動(dòng)或局部坐標(biāo)系下運(yùn)動(dòng)，當(dāng)選擇之后執(zhí)行坐標(biāo)系程序；設(shè)定成功為點(diǎn)擊單腿運(yùn)動(dòng)中的關(guān)節(jié)模式或足端模式六足機(jī)器人運(yùn)動(dòng)即為設(shè)定成功。
8.根據(jù)權(quán)利要求7—種電驅(qū)動(dòng)六足機(jī)器人運(yùn)動(dòng)控制方法，其特征在于:步驟三中利用坐標(biāo)系運(yùn)算模塊計(jì)算得出足末端位置的笛卡爾坐標(biāo)F[Xf，Yf，Zf]具體過程為: (1)PMAC運(yùn)動(dòng)控制器最多支持有16個(gè)坐標(biāo)系； (2)六足機(jī)器人每條腿有三個(gè)關(guān)節(jié)，每個(gè)關(guān)節(jié)的運(yùn)動(dòng)都關(guān)系到足末端位置，將這三個(gè)關(guān)節(jié)定義到同一坐標(biāo)系的三條軸上，則坐標(biāo)系中足末端位置的笛卡爾坐標(biāo)F[Xf，Yf, Zf]，其中，坐標(biāo)系是有運(yùn)動(dòng)關(guān)系的一組運(yùn)動(dòng)系，被控制的電機(jī)定乂到坐標(biāo)系的軸上，在運(yùn)動(dòng)程序中執(zhí)行運(yùn)動(dòng)； (3)假設(shè)已知單腿關(guān)節(jié)轉(zhuǎn)角α、β和Y的角度，根據(jù)單腿關(guān)節(jié)轉(zhuǎn)角求解出機(jī)器人腿部的運(yùn)動(dòng)學(xué)正解一種空間碎片減緩措施評(píng)估及費(fèi)效評(píng)估系統(tǒng)為公式所示:
K ^ (L1+ ￡2cos/?+Li cos(/ — β)) sin a
劣 Yf +L1 cos β+L3 cos(yD) cosdf
Zf = H + L2Sinf1-L3 sm(/ - β) 由正運(yùn)動(dòng)學(xué)方程及三角形關(guān)系，求出機(jī)器人腿部的運(yùn)動(dòng)學(xué)逆解為: ^A".a = lan
' y /:4:)2 + Ki e'—ix = --sin !(^-^--- -Zf2L32L2^(//^"7+^ '.,Jii1...=Zf)2.....+K2I^i22 、，從而求解 y =露一 sm (~—------------------------------------------------------------------------------------------------------------------------------------^...................-?.....................-................................-)
2421^(Η -ZF、2 +K2.^^.........^......................................)卜 i—J
24IL2^IUl ~ZFf+ K2 K cosα J出足末端坐標(biāo)為F[Xf，Yf, Zf]，其中，U、L2、L3分別為根部、大腿和小腿的尺寸。
9.根據(jù)權(quán)利要求8所述一種電驅(qū)動(dòng)六足機(jī)器人運(yùn)動(dòng)控制方法，其特征在于:步驟三中位姿運(yùn)動(dòng)模塊運(yùn)用機(jī)器人坐標(biāo)變換矩陣確定機(jī)器人平臺(tái)質(zhì)心變化具體為: (1)假設(shè)機(jī)器人繞X軸旋轉(zhuǎn)Θ度，六足末端位置在平臺(tái)全局坐標(biāo)系中的坐標(biāo)為(Xi, Yi, Zi),通過旋轉(zhuǎn)變換矩陣，將平臺(tái)全局坐標(biāo)系旋轉(zhuǎn)Θ度，將足末端位置在新的平臺(tái)坐標(biāo)系中表示出來(X' i，Y' i，Z' i)，在原平臺(tái)全局坐標(biāo)系中，將各足末端位置(Xi, Yi, Zi)運(yùn)動(dòng)到新位置(X' O V ,,V ,)； (2)假定機(jī)器人繞X軸旋轉(zhuǎn)Θ角度，在X軸坐標(biāo)為X，則新平臺(tái)坐標(biāo)系相對(duì)于原平臺(tái)坐標(biāo)系旋轉(zhuǎn)Θ角度，變換矩陣T和T—1如公式(4)及(5)所示:
"I O O T == O COS O - sin ^(4)
. O sin Θ m%0
'I O O T"4 = roi(x,ey1= 0 cos <9 sin θ(5)
. 0 - sin 0 cos 0 通過變換矩陣，足末端新位置(X' i，Y, i，Z' i)的表示如公式(6)所示: Xq Γχ,Ι Γ X1- Y； =?！? Yi = Fi cos ^+ Z,.Si η 沒(6μ
Ζ] Zi J — Fi sin θ + Ζ? cos θ
【文檔編號(hào)】G05B19/418GK104192221SQ201410503295
【公開日】2014年12月10日 申請(qǐng)日期:2014年9月26日 優(yōu)先權(quán)日:2014年9月26日
【發(fā)明者】丁亮, 于海濤, 劉宇飛, 劉逸群, 鄧宗全, 高海波, 李楠, 劉振 申請(qǐng)人:哈爾濱工業(yè)大學(xué)