本發(fā)明涉及一種實(shí)現(xiàn)仿人機(jī)器人相似性運(yùn)動(dòng)的步態(tài)規(guī)劃裝置,用于仿人機(jī)器人的步態(tài)穩(wěn)定及運(yùn)動(dòng)規(guī)劃問(wèn)題。

背景技術(shù)：

長(zhǎng)期的自然進(jìn)化使人體具備了對(duì)自身肌肉與骨骼近乎完美的綜合控制能力,表現(xiàn)出動(dòng)作變換的自然平滑過(guò)渡和高度優(yōu)化的運(yùn)動(dòng)能量消耗等特點(diǎn),對(duì)軀干與肢體的運(yùn)動(dòng)協(xié)調(diào)充分適應(yīng)復(fù)雜多變的外部環(huán)境。由于仿人機(jī)器人與人體具有較為接近的關(guān)節(jié)比例結(jié)構(gòu),實(shí)現(xiàn)機(jī)器人的相似性運(yùn)動(dòng),從而使仿人機(jī)器人較好地模仿人體自然平滑的運(yùn)動(dòng)軌跡,并優(yōu)化運(yùn)動(dòng)中的能量消耗已成為當(dāng)前仿人機(jī)器人領(lǐng)域的研究熱點(diǎn)之一.本發(fā)明提出基于關(guān)鍵幀相似性的步態(tài)規(guī)劃算法，利用離線(xiàn)生成的步態(tài)數(shù)據(jù)及對(duì)步態(tài)相似性的度量，在機(jī)器人運(yùn)動(dòng)過(guò)程中實(shí)時(shí)計(jì)算零力矩點(diǎn)對(duì)步態(tài)數(shù)據(jù)進(jìn)行校正，在降低計(jì)算量的同時(shí)保證機(jī)器人的穩(wěn)定，較好地解決步態(tài)的穩(wěn)定及快速規(guī)劃，同時(shí)避免某些情況下的運(yùn)動(dòng)周期增加。

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

本發(fā)明的目的是將特定場(chǎng)合下捕獲的人體離散或連續(xù)運(yùn)動(dòng)軌跡，應(yīng)用在仿人機(jī)器人動(dòng)作設(shè)計(jì)上，使仿人機(jī)器人行走的穩(wěn)定性及正確性得到大大提高。

本發(fā)明的技術(shù)解決方案為：建立關(guān)鍵姿勢(shì)的相似性變換方法,并設(shè)定動(dòng)力學(xué)與實(shí)際物理約束條件,同時(shí)考慮到仿人機(jī)器人與人體在關(guān)節(jié)比例、轉(zhuǎn)角幅度與速度等方面的差異,引入?yún)?shù)化控制與強(qiáng)化技術(shù),優(yōu)化了仿人機(jī)器人前向倒地的觸地過(guò)程,較好地控制了機(jī)器人觸地時(shí)的地面沖擊、觸地位置與倒地穩(wěn)定性。本發(fā)明的具體技術(shù)方案如圖3所示，主要包括應(yīng)用判定步態(tài)相似性、規(guī)劃髖關(guān)節(jié)、實(shí)時(shí)調(diào)整zmp點(diǎn)相結(jié)合的方法規(guī)劃?rùn)C(jī)器人的步態(tài)，使zmp穩(wěn)定裕量趨近最大，步態(tài)可以實(shí)時(shí)調(diào)整，并且調(diào)整后的步態(tài)能夠較好地逼近參考zmp點(diǎn)基于運(yùn)動(dòng)相似性的仿人機(jī)器人前向倒地動(dòng)作。

本發(fā)明的有益效果:在降低運(yùn)算量的同時(shí)較好解決了仿人機(jī)器人的步態(tài)穩(wěn)定及運(yùn)動(dòng)規(guī)劃問(wèn)題，并且可以減少步態(tài)選擇不理想時(shí)的時(shí)間消耗。

附圖說(shuō)明

圖1為本發(fā)明實(shí)施例的仿人機(jī)器人五連桿模型圖。

圖2為本發(fā)明實(shí)施例的高層控制函數(shù)的控制過(guò)程示意圖。

圖3為本發(fā)明實(shí)施例的實(shí)時(shí)調(diào)整控制流圖。

具體實(shí)施方式

以下結(jié)合附圖和具體實(shí)施例對(duì)本發(fā)明進(jìn)行詳細(xì)陳述。

為達(dá)到上述目的，本發(fā)明的技術(shù)方案是：建立關(guān)鍵姿勢(shì)的相似性變換方法,并設(shè)定動(dòng)力學(xué)與實(shí)際物理約束條件,同時(shí)考慮到仿人機(jī)器人與人體在關(guān)節(jié)比例、轉(zhuǎn)角幅度與速度等方面的差異,引入?yún)?shù)化控制與強(qiáng)化技術(shù),優(yōu)化仿人機(jī)器人前向倒地的觸地過(guò)程,較好地控制機(jī)器人觸地時(shí)的地面沖擊、觸地位置與倒地穩(wěn)定性，具體描述如下：

步驟ａ，本實(shí)施例的圖1為本發(fā)明仿人機(jī)器人五連桿模型圖；在系統(tǒng)建立之初，對(duì)機(jī)器人前向運(yùn)動(dòng)單獨(dú)進(jìn)行建模分析，其過(guò)程如下：

步驟ａ-１，設(shè)機(jī)器人有n個(gè)關(guān)節(jié)，各關(guān)節(jié)角度為:

a={a1，a2，…，an}

步驟ａ-２，設(shè)機(jī)器人有m個(gè)連桿，其位姿的齊次變換矩陣為:

a={a1，a2，…，am}

其中，ai為各個(gè)關(guān)節(jié)的齊次變換矩陣;a1,a2分別表示左腳及右腳的齊次變換矩陣。

步驟ａ-３，給定機(jī)器人的結(jié)構(gòu)，則設(shè)定ri(a)為4x4的齊次變換矩陣且其只與ai相關(guān)。

步驟ａ-４，設(shè)fij為第i個(gè)關(guān)節(jié)與第j個(gè)關(guān)節(jié)之間的齊次變換矩陣，且其只與若干ai取值相關(guān)，表示為:

aj=fijai

其中，1≦i,j≦n。

步驟ｂ，利用關(guān)鍵幀對(duì)機(jī)器人某時(shí)刻t各關(guān)節(jié)角度的樣本，分析運(yùn)動(dòng)或者變化過(guò)程中關(guān)鍵部位所處的狀態(tài)，其過(guò)程如下：

步驟ｂ-１，設(shè)單一關(guān)鍵幀的定義為:

g=g(a.p)

其中，a表示關(guān)鍵幀各個(gè)關(guān)節(jié)的角度;p表示關(guān)鍵幀對(duì)應(yīng)支撐腳的下標(biāo)。由于a1、a2分別表示左腳及右腳的齊次變換矩陣，因此p≡{1,2}。

步驟ｂ-２，定義步態(tài)為一系列關(guān)鍵幀的序列，表示為:

g={gi}

其中，i為關(guān)鍵幀在步態(tài)中的索引。

步驟ｃ，通過(guò)判斷步態(tài)與理想目標(biāo)點(diǎn)間距離的改變趨勢(shì)判斷步態(tài)相似性，利用線(xiàn)性插值算法逼近理想步態(tài)，其過(guò)程如下：

步驟ｃ-１，定義前進(jìn)函數(shù)walk，函數(shù)輸入為起始坐標(biāo),朝向、步態(tài)g，函數(shù)輸出為執(zhí)行一次后所處的坐標(biāo)及朝向，表示為:

步驟ｃ-２，設(shè)目標(biāo)點(diǎn)坐標(biāo)目標(biāo)朝向，定義前進(jìn)函數(shù)walk的相似度為:

其中，為距離相似性權(quán)重;為角度相似性權(quán)重。

步驟ｃ-３，設(shè)機(jī)器人共實(shí)現(xiàn)了個(gè)步態(tài)，即:

在這些步態(tài)中，最優(yōu)步態(tài)的選擇如下:

其中，為各個(gè)步態(tài);即為最優(yōu)步態(tài)對(duì)應(yīng)的下標(biāo)。

步驟ｃ-４，求出的最優(yōu)步態(tài)即為各個(gè)步態(tài)中運(yùn)行若干次后最接近目標(biāo)點(diǎn)的步態(tài)，根據(jù)最優(yōu)步態(tài)利用線(xiàn)性插值算法計(jì)算最優(yōu)關(guān)鍵幀。

步驟ｃ-５，設(shè)起始坐標(biāo)為，起始朝向?yàn)?imgfile="dest_path_image006a.gif"wi="10"he="10"img-content="drawing"img-format="gif"orientation="portrait"inline="no"/>，目標(biāo)坐標(biāo)為，目標(biāo)朝向?yàn)?imgfile="dest_path_image016a.gif"wi="14"he="13"img-content="drawing"img-format="gif"orientation="portrait"inline="no"/>，從起始點(diǎn)到目標(biāo)點(diǎn)需要執(zhí)行步態(tài)的關(guān)鍵幀總個(gè)數(shù)為,則朝向角度差為，每次步態(tài)執(zhí)行時(shí)，髖關(guān)節(jié)的變化值為。其對(duì)應(yīng)于線(xiàn)性插值坐標(biāo)中，起始點(diǎn)為，目標(biāo)點(diǎn)為，則各個(gè)關(guān)鍵幀中髖關(guān)節(jié)的角度為，其中，分別為運(yùn)行過(guò)程中關(guān)鍵幀的序號(hào)及髖關(guān)節(jié)角度，據(jù)此求出的關(guān)鍵幀即為最優(yōu)關(guān)鍵幀，從而實(shí)現(xiàn)基于關(guān)鍵幀相似性的步態(tài)規(guī)劃算法。

圖2為本發(fā)明高層控制函數(shù)的控制過(guò)程示意圖。進(jìn)入控制過(guò)程后，單機(jī)器人高層控制中使機(jī)器人能夠以最快的速度接近目標(biāo)，即已知目標(biāo)點(diǎn)及朝向，使得機(jī)器人總體運(yùn)動(dòng)時(shí)間最短。將其抽象為以下函數(shù):

其中，為起始坐標(biāo)及朝向;為目的坐標(biāo)及朝向;為高層控制時(shí)間;為高層控制函數(shù)。

高層控制時(shí)間直接取決于步態(tài)相似性度量，為實(shí)現(xiàn)機(jī)器人的轉(zhuǎn)向，對(duì)機(jī)器人的關(guān)鍵關(guān)節(jié)進(jìn)行調(diào)整。

圖3為本發(fā)明實(shí)施例的仿人機(jī)器人實(shí)時(shí)調(diào)整控制流圖。進(jìn)入實(shí)時(shí)調(diào)整后，需要對(duì)零力矩點(diǎn)(zeromomentpoint,zmp)判據(jù)對(duì)機(jī)器人的穩(wěn)定性進(jìn)行判定，即當(dāng)zmp點(diǎn)在水平面的投影在由支撐腳掌所組成的凸形區(qū)域在水平面上的投影內(nèi)時(shí)，機(jī)器人便處于動(dòng)態(tài)平衡狀態(tài)。

仿人機(jī)器人zmp的計(jì)算公式如下:

其中，為機(jī)器人身體各部分在笛卡爾坐標(biāo)系下的質(zhì)心坐標(biāo);為各部分的質(zhì)量;為zmp點(diǎn)。

為降低計(jì)算量及調(diào)整運(yùn)動(dòng)方向，因此，需要對(duì)髖關(guān)節(jié)進(jìn)行逆向運(yùn)動(dòng)學(xué)計(jì)算，計(jì)算過(guò)程描述如下:

步驟ａ，利用讀取的各關(guān)節(jié)角度，得到各關(guān)節(jié)的齊次變換矩陣，然后以不同的順序相乘得到機(jī)器人的運(yùn)動(dòng)方程。

步驟ｂ，利用頭部的運(yùn)動(dòng)軌跡得到頭部的運(yùn)動(dòng)方程，由于除髖關(guān)節(jié)外的齊次變換矩陣在步驟ａ中已經(jīng)算得，因此利用逆向運(yùn)動(dòng)學(xué)便可以算得髖關(guān)節(jié)理想齊次變換矩陣。

步驟ｃ，利用步驟ｂ算出的髖關(guān)節(jié)旋轉(zhuǎn)矩陣調(diào)整其運(yùn)動(dòng)，并在下一個(gè)周期算出其zmp點(diǎn)，如果其穩(wěn)定裕量有增加的趨勢(shì)，則在下一個(gè)周期繼續(xù)選取由逆向運(yùn)動(dòng)學(xué)算得的髖關(guān)節(jié)狀態(tài);否則，不對(duì)髖關(guān)節(jié)進(jìn)行調(diào)整。

上面所述僅僅是對(duì)本發(fā)明優(yōu)選實(shí)施方式進(jìn)行描述，并非對(duì)本發(fā)明的構(gòu)思和范圍進(jìn)行限定，在不脫離本發(fā)明設(shè)計(jì)思想的前提下，本領(lǐng)域中普通工程技術(shù)人員如對(duì)本發(fā)明的技術(shù)方案中的步態(tài)規(guī)劃裝置和規(guī)劃方法進(jìn)行改變，均應(yīng)屬于本發(fā)明的保護(hù)范圍。