本發(fā)明涉及機(jī)器人運(yùn)動(dòng)規(guī)劃領(lǐng)域，具體涉及一種規(guī)劃四足機(jī)器人靜步態(tài)機(jī)身運(yùn)動(dòng)軌跡的方法。

背景技術(shù)：

目前地面機(jī)器人的行走方式主要分為輪式、履帶式和足式.輪履式機(jī)器人的研究已經(jīng)相對(duì)成熟，但其在復(fù)雜崎嶇地形上的通過(guò)能力較差，足式機(jī)器人采用仿生原理，模擬自然界的足式動(dòng)物，在狹小崎嶇地形具有良好的通過(guò)性，有效彌補(bǔ)了輪履式機(jī)器人的不足。在足式機(jī)器人中，四足機(jī)器人比雙足機(jī)器人簡(jiǎn)單、比多足機(jī)器人行走效率高，因此具有較高的研究?jī)r(jià)值。

四足機(jī)器人步態(tài)分為靜步態(tài)和動(dòng)步態(tài)兩種。其行走時(shí)，任意時(shí)刻最多有兩條腿處于支撐相，則稱(chēng)為動(dòng)步態(tài)；至少有三條腿處于支撐相，則稱(chēng)為靜步態(tài)。靜步態(tài)具有良好的穩(wěn)定性和通過(guò)性，尤其在復(fù)雜崎嶇路面上表現(xiàn)良好。目前靜步態(tài)機(jī)身運(yùn)動(dòng)設(shè)計(jì)的理論研究主要集中在穩(wěn)定性分析和軌跡規(guī)劃兩個(gè)方面。穩(wěn)定性分析方法包含縱向穩(wěn)定裕度、能量穩(wěn)定裕度、力角穩(wěn)定裕度等；軌跡規(guī)劃方法包含優(yōu)化學(xué)習(xí)方法、零力矩點(diǎn)軌跡、中樞模式發(fā)生器等。但是目前的設(shè)計(jì)還不夠全面，且計(jì)算方法較為復(fù)雜，尚且缺乏一種簡(jiǎn)潔有效的靜步態(tài)機(jī)身運(yùn)動(dòng)規(guī)劃方法。

為此，尋找一種四足機(jī)器人靜步態(tài)機(jī)身運(yùn)動(dòng)軌跡規(guī)劃方法，使得軌跡規(guī)劃設(shè)計(jì)全面，計(jì)算方法簡(jiǎn)單，更加有效的規(guī)劃靜步態(tài)機(jī)身運(yùn)動(dòng)，成為本領(lǐng)域技術(shù)人員迫切需要解決的問(wèn)題。

技術(shù)實(shí)現(xiàn)要素：

為了解決現(xiàn)有技術(shù)中所存在的上述不足，本發(fā)明提供一種規(guī)劃四足機(jī)器人靜步態(tài)機(jī)身運(yùn)動(dòng)軌跡的方法。所述方法包括：

i、規(guī)劃靜步態(tài)行走時(shí)機(jī)身和足端的擺動(dòng)順序及步態(tài)時(shí)序；

ii、計(jì)算四足機(jī)器人在三條腿支撐地面時(shí)的穩(wěn)定區(qū)域；

iii、計(jì)算連續(xù)邁步時(shí)同側(cè)腿支撐地面時(shí)的穩(wěn)定區(qū)域，并求取機(jī)身運(yùn)動(dòng)的目標(biāo)位置；

iv、擬合機(jī)身運(yùn)動(dòng)軌跡曲線(xiàn)。

優(yōu)選的，所述步驟i的靜步態(tài)行走時(shí)機(jī)身和足端的擺動(dòng)順序包括：一側(cè)的后腿、前腿，另一側(cè)腿的后腿、前腿；

所述靜步態(tài)行走時(shí)機(jī)身和足端的步態(tài)時(shí)序包括：一側(cè)的后腿擺動(dòng)結(jié)束后立即開(kāi)始同側(cè)的前腿擺動(dòng)，同側(cè)前腿擺動(dòng)結(jié)束后進(jìn)入四足支撐狀態(tài)，四足支撐結(jié)束后進(jìn)入另一側(cè)的后腿擺動(dòng)，另一側(cè)后腿擺動(dòng)結(jié)束后立即進(jìn)入另一側(cè)的前腿擺動(dòng)，另一側(cè)的前腿擺動(dòng)結(jié)束后進(jìn)入再次四足支撐狀態(tài)；足端擺動(dòng)時(shí)，機(jī)身靜止不動(dòng)，四足支撐時(shí)，機(jī)身開(kāi)始擺動(dòng)。

優(yōu)選的，所述步驟i-2中所述擺動(dòng)的時(shí)間均相等，所述支撐的時(shí)間均相等。

優(yōu)選的，所述步驟ii的四足機(jī)器人在三條腿支撐地面時(shí)的穩(wěn)定區(qū)域的計(jì)算包括：求取機(jī)身重心在支撐平面上的投影點(diǎn)，當(dāng)所述投影點(diǎn)位于支撐三角形內(nèi)部，根據(jù)機(jī)身與足端的受力和力矩平衡分析，計(jì)算投影點(diǎn)到支撐三角形邊緣的最小穩(wěn)定裕度，求取支撐三角形三條邊對(duì)應(yīng)的三個(gè)最小穩(wěn)定裕度，得到穩(wěn)定區(qū)域。

優(yōu)選的，所述投影點(diǎn)到支撐三角形邊緣的最小穩(wěn)定裕度按下式計(jì)算：

式中，g'：機(jī)身重心g在支撐平面上的投影點(diǎn)；mg：施加在機(jī)身的合力矩；fg：施加在機(jī)身的合力；與下標(biāo)向量方向相同的單位向量；沿著z軸的單位向量；rgg'：連接下標(biāo)兩個(gè)點(diǎn)的向量。

優(yōu)選的，所述步驟iii同側(cè)腿連續(xù)邁步時(shí)對(duì)應(yīng)的穩(wěn)定區(qū)域的計(jì)算包括：求取同側(cè)兩條腿分別擺動(dòng)時(shí)，各自對(duì)應(yīng)的穩(wěn)定區(qū)域，兩個(gè)所述穩(wěn)定區(qū)域相交得到的共同穩(wěn)定區(qū)域?yàn)橥瑐?cè)腿連續(xù)邁步時(shí)對(duì)應(yīng)的穩(wěn)定區(qū)域。

優(yōu)選的，所述步驟iii機(jī)身運(yùn)動(dòng)的目標(biāo)位置的求取包括：忽略腿部質(zhì)量，當(dāng)機(jī)身質(zhì)量均勻分布時(shí)，機(jī)身運(yùn)動(dòng)軌跡等同于機(jī)身質(zhì)心的運(yùn)動(dòng)軌跡，則共同穩(wěn)定區(qū)域的重心為機(jī)身質(zhì)心運(yùn)動(dòng)的目標(biāo)位置。

優(yōu)選的，采用正弦函數(shù)擬合所述步驟iv的機(jī)身運(yùn)動(dòng)軌跡曲線(xiàn)。

優(yōu)選的，所述機(jī)身運(yùn)動(dòng)軌跡曲線(xiàn)的擬合包括：四足支撐狀態(tài)的下一狀態(tài)為一側(cè)腿擺動(dòng)，所述支撐狀態(tài)對(duì)應(yīng)的機(jī)身運(yùn)動(dòng)為向另一側(cè)擺動(dòng)，以四足支撐狀態(tài)剛開(kāi)始時(shí)刻的機(jī)身質(zhì)心位置為運(yùn)動(dòng)軌跡的原點(diǎn)，根據(jù)一側(cè)腿連續(xù)擺動(dòng)對(duì)應(yīng)的質(zhì)心運(yùn)動(dòng)目標(biāo)位置作為運(yùn)動(dòng)的軌跡的終點(diǎn)，采用正弦函數(shù)對(duì)平面內(nèi)的運(yùn)動(dòng)軌跡進(jìn)行規(guī)劃。

優(yōu)選的，所述運(yùn)動(dòng)軌跡按下式計(jì)算：

式中，和為目標(biāo)位置的橫坐標(biāo)和縱坐標(biāo)；tst：四足支撐相的持續(xù)時(shí)間，t：時(shí)間。

與現(xiàn)有技術(shù)相比，本發(fā)明的有益效果為：

(1)在步態(tài)運(yùn)動(dòng)的順序和時(shí)序規(guī)劃上，考慮了運(yùn)動(dòng)穩(wěn)定裕度特性，初步保證了行走穩(wěn)定性。

(2)根據(jù)力和力矩平衡原理分析了最小穩(wěn)定裕度，在一條腿擺動(dòng)情況下對(duì)應(yīng)的支撐三角形內(nèi)部，求取了穩(wěn)定區(qū)域，進(jìn)一步確定了行走穩(wěn)定性。

(3)采用正弦函數(shù)擬合機(jī)身運(yùn)動(dòng)軌跡，使得運(yùn)動(dòng)軌跡和運(yùn)動(dòng)速度均連續(xù)變化。

附圖說(shuō)明

圖1為本發(fā)明的靜步態(tài)的四足擺動(dòng)順序示意圖；

圖2為本發(fā)明的靜步態(tài)的機(jī)身與足端運(yùn)動(dòng)狀態(tài)示意圖；

圖3為本發(fā)明的三條腿支撐時(shí)的最小穩(wěn)定裕度分析示意圖；

圖4為本發(fā)明的三條腿支撐時(shí)對(duì)應(yīng)的穩(wěn)定區(qū)域；

圖5為本發(fā)明的同側(cè)腿連續(xù)擺動(dòng)時(shí)對(duì)應(yīng)的共同穩(wěn)定區(qū)域；

圖6為本發(fā)明的四足機(jī)器人在xy平面內(nèi)的運(yùn)動(dòng)軌跡示意圖。

具體實(shí)施方式

為了更好地理解本發(fā)明，下面結(jié)合說(shuō)明書(shū)附圖和實(shí)例對(duì)本發(fā)明的內(nèi)容做進(jìn)一步的說(shuō)明。

本發(fā)明提供了一種規(guī)劃四足機(jī)器人靜步態(tài)機(jī)身運(yùn)動(dòng)軌跡的方法，結(jié)合具體實(shí)施例做進(jìn)一步說(shuō)明。

步驟i：規(guī)劃靜步態(tài)行走時(shí)機(jī)身和足端的擺動(dòng)順序及步態(tài)時(shí)序

如圖1和圖2所示，步驟1從穩(wěn)定性的角度出發(fā)，對(duì)靜步態(tài)行走時(shí)四條腿的擺動(dòng)順序和步態(tài)時(shí)序進(jìn)行規(guī)劃，具體包括：

步驟11：圖1中，在一個(gè)步態(tài)周期內(nèi)，設(shè)計(jì)邁步順序?yàn)椋鹤蠛笸?、左前腿、右后腿、右前腿?/p>

步驟12：圖2中，在一個(gè)步態(tài)周期內(nèi)，設(shè)計(jì)運(yùn)動(dòng)時(shí)序?yàn)椋鹤蠛笸葦[動(dòng)結(jié)束后立即開(kāi)始左前腿擺動(dòng)，左前腿擺動(dòng)結(jié)束后進(jìn)入四足支撐狀態(tài)，四足支撐結(jié)束后進(jìn)入右后腿擺動(dòng)，右后腿擺動(dòng)結(jié)束后立即進(jìn)入右前腿擺動(dòng)，右前腿擺動(dòng)結(jié)束后進(jìn)入再次四足支撐狀態(tài)。所有擺動(dòng)相時(shí)間相等，所有支撐相時(shí)間相等。

步驟13：圖2中，足端擺動(dòng)時(shí)，機(jī)身靜止不動(dòng)；四足支撐時(shí)，機(jī)身開(kāi)始擺動(dòng)。

步驟2：計(jì)算四足機(jī)器人在三條腿支撐地面時(shí)的穩(wěn)定區(qū)域，如圖3和圖4所示，具體包括：

步驟21：圖3中，當(dāng)三條腿支撐地面時(shí)，其與地面的三個(gè)支撐點(diǎn)pa、pb、pc形成的三角形即為支撐三角形。

步驟22：求取機(jī)身重心g在支撐平面上的投影點(diǎn)g’，當(dāng)該投影點(diǎn)位于支撐三角形δpapbpc內(nèi)部，根據(jù)機(jī)身與足端的受力和力矩平衡分析，計(jì)算投影點(diǎn)到支撐三角形邊緣的最小穩(wěn)定裕度。其計(jì)算方法如下：

其中，g'：機(jī)身重心g在支撐平面上的投影點(diǎn)；mg：施加在機(jī)身的合力矩；fg：施加在機(jī)身的合力；與下標(biāo)向量方向相同的單位向量；沿著z軸的單位向量；rgg'：連接下標(biāo)兩個(gè)點(diǎn)的向量。

步驟23：圖4中，以左后腿擺動(dòng)為例，根據(jù)步驟22求取支撐三角形δbdc三條邊對(duì)應(yīng)的三個(gè)最小穩(wěn)定裕度dbc,dbd,ddc，從而在其內(nèi)部的陰影部分才是左后腿擺動(dòng)時(shí)對(duì)應(yīng)的機(jī)身穩(wěn)定區(qū)域。

步驟3計(jì)算連續(xù)邁步時(shí)同側(cè)腿支撐地面時(shí)的穩(wěn)定區(qū)域，并求取機(jī)身運(yùn)動(dòng)的目標(biāo)位置，如圖5所示，具體包括：

步驟31：根據(jù)步驟2，求取同側(cè)的兩條腿分別擺動(dòng)時(shí)，各自對(duì)應(yīng)的穩(wěn)定區(qū)域。

步驟32：同側(cè)兩條腿對(duì)應(yīng)的穩(wěn)定區(qū)域相交得到的共同穩(wěn)定區(qū)域，存在三種情況：三角形、四邊形和五邊形，如圖5三個(gè)分圖中的陰影部分所示。

步驟33：忽略腿部質(zhì)量，假設(shè)機(jī)身質(zhì)量均勻分布，則剛體運(yùn)動(dòng)可以轉(zhuǎn)化為質(zhì)心運(yùn)動(dòng)，機(jī)身運(yùn)動(dòng)軌跡等同于機(jī)身質(zhì)心的運(yùn)動(dòng)軌跡。

步驟33：將步驟32得到的共同穩(wěn)定區(qū)域的重心，作為機(jī)身質(zhì)心運(yùn)動(dòng)的目標(biāo)位置，如圖5中的點(diǎn)pd所示。

步驟4采用正弦函數(shù)擬合機(jī)身運(yùn)動(dòng)軌跡曲線(xiàn)的實(shí)驗(yàn)結(jié)果，如圖6所示，具體包括：

步驟41：當(dāng)四足支撐相的下一狀態(tài)為左側(cè)腿擺動(dòng)時(shí)，該支撐相對(duì)應(yīng)的機(jī)身運(yùn)動(dòng)為向右擺動(dòng)，以四足支撐相剛開(kāi)始時(shí)刻的機(jī)身質(zhì)心位置為運(yùn)動(dòng)軌跡的原點(diǎn)，根據(jù)步驟3求取左側(cè)腿連續(xù)擺動(dòng)對(duì)應(yīng)的質(zhì)心運(yùn)動(dòng)目標(biāo)位置，作為運(yùn)動(dòng)軌跡的終點(diǎn)，采用正弦函數(shù)對(duì)xy平面內(nèi)的運(yùn)動(dòng)軌跡進(jìn)行規(guī)劃。其計(jì)算方法如下：

其中，tst為四足支撐相的持續(xù)時(shí)間，t為機(jī)身擺動(dòng)時(shí)刻，和為目標(biāo)位置的橫坐標(biāo)和縱坐標(biāo)，x(t)和y(t)為機(jī)身質(zhì)心在t時(shí)刻的坐標(biāo)位置。

步驟42：當(dāng)四足支撐相的下一狀態(tài)為右側(cè)腿擺動(dòng)時(shí)，該支撐相對(duì)應(yīng)的機(jī)身運(yùn)動(dòng)為向左擺動(dòng)，以四足支撐相剛開(kāi)始時(shí)刻的機(jī)身質(zhì)心位置為運(yùn)動(dòng)軌跡的原點(diǎn)，根據(jù)步驟3求取右側(cè)腿連續(xù)擺動(dòng)對(duì)應(yīng)的質(zhì)心運(yùn)動(dòng)目標(biāo)位置，作為運(yùn)動(dòng)軌跡的終點(diǎn)，采用正弦函數(shù)對(duì)xy平面內(nèi)的運(yùn)動(dòng)軌跡進(jìn)行規(guī)劃。

本領(lǐng)域內(nèi)的技術(shù)人員應(yīng)明白，本申請(qǐng)的實(shí)施例可提供為方法、系統(tǒng)、或計(jì)算機(jī)程序產(chǎn)品。因此，本申請(qǐng)可采用完全硬件實(shí)施例、完全軟件實(shí)施例、或結(jié)合軟件和硬件方面的實(shí)施例的形式。而且，本申請(qǐng)可采用在一個(gè)或多個(gè)其中包含有計(jì)算機(jī)可用程序代碼的計(jì)算機(jī)可用存儲(chǔ)介質(zhì)(包括但不限于磁盤(pán)存儲(chǔ)器、cd-rom、光學(xué)存儲(chǔ)器等)上實(shí)施的計(jì)算機(jī)程序產(chǎn)品的形式。

本申請(qǐng)是參照根據(jù)本申請(qǐng)實(shí)施例的方法、設(shè)備(系統(tǒng))、和計(jì)算機(jī)程序產(chǎn)品的流程圖和/或方框圖來(lái)描述的。應(yīng)理解可由計(jì)算機(jī)程序指令實(shí)現(xiàn)流程圖和/或方框圖中的每一流程和/或方框、以及流程圖和/或方框圖中的流程和/或方框的結(jié)合。可提供這些計(jì)算機(jī)程序指令到通用計(jì)算機(jī)、專(zhuān)用計(jì)算機(jī)、嵌入式處理機(jī)或其他可編程數(shù)據(jù)處理設(shè)備的處理器以產(chǎn)生一個(gè)機(jī)器，使得通過(guò)計(jì)算機(jī)或其他可編程數(shù)據(jù)處理設(shè)備的處理器執(zhí)行的指令產(chǎn)生用于實(shí)現(xiàn)在流程圖一個(gè)流程或多個(gè)流程和/或方框圖一個(gè)方框或多個(gè)方框中指定的功能的裝置。

這些計(jì)算機(jī)程序指令也可存儲(chǔ)在能引導(dǎo)計(jì)算機(jī)或其他可編程數(shù)據(jù)處理設(shè)備以特定方式工作的計(jì)算機(jī)可讀存儲(chǔ)器中，使得存儲(chǔ)在該計(jì)算機(jī)可讀存儲(chǔ)器中的指令產(chǎn)生包括指令裝置的制造品，該指令裝置實(shí)現(xiàn)在流程圖一個(gè)流程或多個(gè)流程和/或方框圖一個(gè)方框或多個(gè)方框中指定的功能。

這些計(jì)算機(jī)程序指令也可裝載到計(jì)算機(jī)或其他可編程數(shù)據(jù)處理設(shè)備上，使得在計(jì)算機(jī)或其他可編程設(shè)備上執(zhí)行一系列操作步驟以產(chǎn)生計(jì)算機(jī)實(shí)現(xiàn)的處理，從而在計(jì)算機(jī)或其他可編程設(shè)備上執(zhí)行的指令提供用于實(shí)現(xiàn)在流程圖一個(gè)流程或多個(gè)流程和/或方框圖一個(gè)方框或多個(gè)方框中指定的功能的步驟。

以上僅為本發(fā)明的實(shí)施例而已，并不用于限制本發(fā)明，凡在本發(fā)明的精神和原則之內(nèi)，所做的任何修改、等同替換、改進(jìn)等，均包含在申請(qǐng)待批的本發(fā)明的權(quán)利要求范圍之內(nèi)。