專利名稱：一種具有環(huán)境適應(yīng)性的足式機(jī)器人穩(wěn)定性控制方法及系統(tǒng)的制作方法
技術(shù)領(lǐng)域：
本發(fā)明屬于機(jī)器人技術(shù)領(lǐng)域，具體涉及一種具有環(huán)境適應(yīng)性的足式機(jī)器人穩(wěn)定性控制方法及系統(tǒng)。
背景技術(shù)：
地球陸地表面有超過(guò)50%以上是山丘或沼澤等復(fù)雜地形，相對(duì)于輪式機(jī)器人和履帶機(jī)器人，足式機(jī)器人在這些復(fù)雜地形上具有更好的適應(yīng)性和運(yùn)動(dòng)性能。人們希望足式機(jī)器人能夠像自然界中各種足式動(dòng)物一樣可以在陸地復(fù)雜地形上穩(wěn)定、快速的運(yùn)動(dòng)，從而輔助人類完成各種任務(wù)。因此，越來(lái)越多的研究人員將仿生學(xué)方法引入到機(jī)器人研究之中，通過(guò)研究、學(xué)習(xí)、模仿的仿生學(xué)方法來(lái)復(fù)制和再造生物的結(jié)構(gòu)、功能及控制機(jī)制。自然界中的生物往往是一個(gè)復(fù)雜的高度非線性系統(tǒng)，而且多腿、多關(guān)節(jié)和肌肉冗余嚴(yán)重，因此對(duì)生物體不加簡(jiǎn)化的模仿和復(fù)現(xiàn)相當(dāng)困難且不現(xiàn)實(shí)。為了解決這個(gè)問(wèn)題，研究人員科學(xué)合理的將各種具有不同骨骼結(jié)構(gòu)、腿數(shù)和姿態(tài)的動(dòng)物用相對(duì)統(tǒng)一的模型表示，同時(shí)獲取相應(yīng)的等效參數(shù)，力求通過(guò)合理有效的簡(jiǎn)化模型來(lái)等效足式動(dòng)物的運(yùn)動(dòng)。為考察系統(tǒng)整體的行為特性所建立的等效模型應(yīng)該是最為簡(jiǎn)化的，一種有效的嘗試是彈簧倒立擺模型(SLIP, Spring Loaded Inverted Pendulum),即將生物體簡(jiǎn)化為單質(zhì)量一彈簧系統(tǒng)，用不同質(zhì)量、剛度、阻尼以及相關(guān)形態(tài)參數(shù)來(lái)表征具體的生物體運(yùn)動(dòng)。SLIP模型是20世紀(jì)80年代以來(lái)發(fā)展起來(lái)的研究足式機(jī)器人的有效模型，研究人員通過(guò)對(duì)哺乳動(dòng)物的腿部和肌肉的細(xì)致研究表明彈性機(jī)制確實(shí)存在于動(dòng)物的運(yùn)動(dòng)中，而且對(duì)動(dòng)物的性能有很大的影響。該模型可以有效的解釋腿部對(duì)地面沖擊的緩沖機(jī)制，通過(guò)調(diào)整系統(tǒng)著地角度、腿部等效剛度等參數(shù)可以達(dá)到控制系統(tǒng)水平運(yùn)動(dòng)速度和彈跳高度的目的。且通過(guò)相關(guān)學(xué)者提出的等效虛擬腿的概念，足式機(jī)器人(如雙足機(jī)器人、四足機(jī)器人、六足機(jī)器人等)均可等效為最簡(jiǎn)化的單質(zhì)量——彈簧系統(tǒng)即單SLIP等效模型進(jìn)行研究和分析。現(xiàn)有的足式機(jī)器人大都停留在實(shí)驗(yàn)室樣機(jī)階段，少有能夠真正實(shí)現(xiàn)復(fù)雜環(huán)境下的運(yùn)動(dòng)控制，其難點(diǎn)就在于足式機(jī)器人復(fù)雜環(huán)境下的動(dòng)態(tài)穩(wěn)定控制方法尚不成熟。足式機(jī)器人是一種多自由度、強(qiáng)非線性、多冗余的系統(tǒng)，其完整動(dòng)力學(xué)特性分析十分復(fù)雜，直接對(duì)其進(jìn)行復(fù)雜環(huán)境下的動(dòng)態(tài)穩(wěn)定性控制往往不能滿足控制實(shí)時(shí)性的要求。因此越來(lái)越多的研究人員開始從SLIP等效模型入手，研究和分析SLIP等效模型在復(fù)雜環(huán)境下的動(dòng)態(tài)穩(wěn)定性控制方法，從而以該等效模型為基礎(chǔ)，再次延伸和擴(kuò)展至整個(gè)足式機(jī)器人，由簡(jiǎn)入繁，最終達(dá)到對(duì)整個(gè)足式機(jī)器人系統(tǒng)穩(wěn)定控制的目的?，F(xiàn)有技術(shù)一公開了一種針對(duì)無(wú)阻尼SLIP模型的周期穩(wěn)定運(yùn)動(dòng)控制方法，其通過(guò)推導(dǎo)無(wú)阻尼SLIP模型在飛行相和著地相的動(dòng)力學(xué)方程，近似得到整個(gè)著地過(guò)程中SLIP模型著地角度和離地角度與時(shí)間的關(guān)系，以及整個(gè)著地過(guò)程中彈簧長(zhǎng)度和時(shí)間的關(guān)系等。進(jìn)而通過(guò)對(duì)不同可變參數(shù)的取值迭代，仿真得到不同條件下SLIP模型一次著地前后的狀態(tài)，從而找到滿足SLIP不動(dòng)點(diǎn)周期運(yùn)動(dòng)的合適初始參數(shù)，實(shí)現(xiàn)其穩(wěn)定運(yùn)動(dòng)控制?，F(xiàn)有技術(shù)二公開了一種針對(duì)有阻尼SLIP模型的周期穩(wěn)定運(yùn)動(dòng)控制方法，其在SLIP動(dòng)力學(xué)模型推導(dǎo)中增加了阻尼項(xiàng)，考慮了整個(gè)運(yùn)動(dòng)過(guò)程中的能量損失，近似得到SLIP模型著地過(guò)程中的能量損失方程，通過(guò)改變彈簧剛度等方法對(duì)損失的能量進(jìn)行補(bǔ)償，繼續(xù)通過(guò)對(duì)不同可變參數(shù)的取值迭代，仿真得到能量補(bǔ)償后的SLIP模型著地前后的狀態(tài)，從而找到滿足SLIP不動(dòng)點(diǎn)周期運(yùn)動(dòng)的合適初始參數(shù)，實(shí)現(xiàn)其穩(wěn)定運(yùn)動(dòng)控制。在對(duì)現(xiàn)有技術(shù)進(jìn)行研究后，本發(fā)明人發(fā)現(xiàn)現(xiàn)有技術(shù)一中考慮的是一種十分理想的等效模型，其忽略了足式機(jī)器人運(yùn)動(dòng)過(guò)程中存在的阻尼因素，在反映機(jī)器人實(shí)際運(yùn)動(dòng)過(guò)程中存在模型誤差；而現(xiàn)有技術(shù)二雖然考慮了等效模型的阻尼因素，且對(duì)其運(yùn)動(dòng)過(guò)程中損失的能量進(jìn)行了補(bǔ)償，但是該技術(shù)實(shí)現(xiàn)不動(dòng)點(diǎn)穩(wěn)定控制需要通過(guò)大量的賦值迭代才能找到合適的初值條件，不滿足機(jī)器人系統(tǒng)實(shí)時(shí)性控制的要求，不易運(yùn)用在實(shí)際的機(jī)器人穩(wěn)定控 制中。且現(xiàn)有技術(shù)均只考慮了 SLIP模型在平面地形上的控制，對(duì)相對(duì)較為復(fù)雜的環(huán)境地形不具有適應(yīng)性。

發(fā)明內(nèi)容
本發(fā)明的目的在于提供一種能夠克服上述缺陷的足式機(jī)器人穩(wěn)定性控制方法及系統(tǒng)，以SLIP等效模型為研究對(duì)象，不僅考慮足式機(jī)器人運(yùn)動(dòng)過(guò)程中的阻尼損失，而且使得控制后的SLIP模型不再僅僅適應(yīng)于平面地形，對(duì)臺(tái)階、坑洼等較為復(fù)雜的地形也具有較好的穩(wěn)定適應(yīng)性，另外對(duì)模型的初始條件要求較低，不需要多次迭代，解決了控制實(shí)時(shí)性不能滿足的問(wèn)題。本發(fā)明提供的一種具有環(huán)境適應(yīng)性的足式機(jī)器人穩(wěn)定性控制方法，該方法包括下述步驟第I步系統(tǒng)參數(shù)初始化，包括系統(tǒng)基本信息、轉(zhuǎn)角PID控制的比例增益系數(shù)kP、積分時(shí)間常數(shù)匕和微分時(shí)間常數(shù)kD ;令11=1 ;令第一次觸地相補(bǔ)償?shù)哪芰緼E1+為零；初始化控制目標(biāo)，并計(jì)算得到穩(wěn)定周期運(yùn)動(dòng)期望系統(tǒng)總能量Ed ;所述系統(tǒng)基本信息包括質(zhì)量m、等效 彈簧剛度k、等效阻尼c和初始等效腿長(zhǎng)A ;所述控制目標(biāo)包括穩(wěn)定周期運(yùn)動(dòng)期望水平速度4和期望最高點(diǎn)豎直高度；第2步檢測(cè)第一次觸地前的初始水平速度矣;第3步在第n次觸地之前計(jì)算得到第n次觸地角0 TD ；第4步實(shí)時(shí)檢測(cè)系統(tǒng)狀態(tài)，得到當(dāng)前時(shí)刻SLIP模型的狀態(tài)信息，包括質(zhì)心A的水平速度i、豎直方向速度j、腿的擺角9、腿長(zhǎng)度r、豎直方向腳力F。和當(dāng)前系統(tǒng)時(shí)間t ；第5步判斷SLIP模型是否觸地，觸地時(shí)刻判定條件為足端腳力F。發(fā)生由等于零到大于零的突變，且系統(tǒng)豎直速度j < 0 ;如果觸地執(zhí)行第7步，如果沒(méi)有觸地執(zhí)行第6步；第6步通過(guò)PID控制，在第n次觸地之前將SLIP模型控制到預(yù)定的觸地角，即使腿的擺角等于觸地角eTD;第7步讀取觸地時(shí)刻系統(tǒng)相關(guān)狀態(tài)參數(shù)，包括觸地時(shí)刻的水平速度，豎直速度yTD，等效腿長(zhǎng)rTD，以及實(shí)際觸地角度e TD，并計(jì)算觸地時(shí)刻系統(tǒng)總能量En，即為第n次觸地之如系統(tǒng)總能量；第8步判斷SLIP模型是否壓縮至底部，如果壓縮至底部執(zhí)行第9步，否則執(zhí)行第4步；
第9步進(jìn)行系統(tǒng)能量補(bǔ)償，使系統(tǒng)達(dá)到期望系統(tǒng)總能量Ed 第10步判斷SLIP模型是否離地，如果離地執(zhí)行第11步，否則執(zhí)行第9步；第11步讀取離地時(shí)刻系統(tǒng)相關(guān)狀態(tài)參數(shù)，包括離地時(shí)刻的水平速度,豎直速度jiO，等效腿長(zhǎng)rw，以及實(shí)際觸地角度e L0,并計(jì)算離地時(shí)刻系統(tǒng)總能量En+1，即為第n次觸地之后系統(tǒng)總能量；第12步計(jì)算第n次觸地過(guò)程中系統(tǒng)損失的能I At -，其計(jì)算公式為
權(quán)利要求
1.ー種具有環(huán)境適應(yīng)性的足式機(jī)器人穩(wěn)定性控制方法，該方法包括下述步驟 第I步系統(tǒng)參數(shù)初始化，包括系統(tǒng)基本信息、轉(zhuǎn)角PID控制的比例増益系數(shù)kP、積分時(shí)間常數(shù)h和微分時(shí)間常數(shù)kD ;令n=l ;令第一次觸地相補(bǔ)償?shù)哪芰緼E1+為零；初始化控制目標(biāo)，并計(jì)算得到穩(wěn)定周期運(yùn)動(dòng)期望系統(tǒng)總能量Ed ;所述系統(tǒng)基本信息包括質(zhì)量m、等效彈簧剛度k、等效阻尼c和初始等效腿長(zhǎng)
2.根據(jù)權(quán)利要求I所述的具有環(huán)境適應(yīng)性的足式機(jī)器人穩(wěn)定性控制方法，其特征在于，第3步中， 第η次觸地角Θ TD的計(jì)算過(guò)程為 設(shè)Λ S即為實(shí)際觸地點(diǎn)Ptd與中性點(diǎn)Pn之間的調(diào)整距離み為觸地點(diǎn)是中性點(diǎn)時(shí)觸地過(guò)程運(yùn)動(dòng)水平距離的一半,
3.根據(jù)權(quán)利要求I或2所述的具有環(huán)境適應(yīng)性的足式機(jī)器人穩(wěn)定性控制方法，其特征在于，觸地時(shí)刻系統(tǒng)總能量Εη，即為第η次觸地之前系統(tǒng)總能量的計(jì)算公式為
4.根據(jù)權(quán)利要求I或2所述的具有環(huán)境適應(yīng)性的足式機(jī)器人穩(wěn)定性控制方法，其特征在于，第9步中，補(bǔ)償能量的方法如下 在壓縮至底部時(shí)刻增加沿彈簧方向的沖量Fn · dt，計(jì)算欲補(bǔ)償能量為Δ￡時(shí)的驅(qū)動(dòng)カFn，其計(jì)算公式為
5.根據(jù)權(quán)利要求I或2所述的具有環(huán)境適應(yīng)性的足式機(jī)器人穩(wěn)定性控制方法，其特征在于，第9步中，補(bǔ)償能量的方法如下 在壓縮至底部時(shí)刻開始直到系統(tǒng)離地時(shí)刻持續(xù)增加沿彈簧方向的補(bǔ)償恒力Fn，計(jì)算欲補(bǔ)償能量為時(shí)的驅(qū)動(dòng)カFn，其計(jì)算公式為
6.根據(jù)權(quán)利要求I或2所述的具有環(huán)境適應(yīng)性的足式機(jī)器人穩(wěn)定性控制方法，其特征在于，第9步中，補(bǔ)償能量的方法如下 在壓縮至底部時(shí)刻開始直到系統(tǒng)離地時(shí)刻改變系統(tǒng)等效剛度，在原等效剛度k的基礎(chǔ)上増加補(bǔ)償剛度Ak，其計(jì)算公式為
7.根據(jù)權(quán)利要求I或2所述的具有環(huán)境適應(yīng)性的足式機(jī)器人穩(wěn)定性控制方法，其特征在于，第6步中，PID控制的輸入為當(dāng)前擺角，輸出為關(guān)節(jié)カ矩。
8.ー種具有環(huán)境適應(yīng)性的足式機(jī)器人穩(wěn)定性控制系統(tǒng)，其特征在干，該系統(tǒng)包括系統(tǒng)狀態(tài)檢測(cè)模塊和穩(wěn)定控制模塊； 穩(wěn)定控制模塊由觸地角控制模塊，PID控制器，能量補(bǔ)償模塊和補(bǔ)償カ控制器組成；觸地角控制模塊用于進(jìn)行計(jì)算飛行時(shí)的觸地角，并提供給PID控制器；PID控制器根據(jù)觸地角控制模塊提供的觸地角控制腿到對(duì)應(yīng)的擺角；能量補(bǔ)償模塊計(jì)算系統(tǒng)從彈簧壓縮至最低點(diǎn)到起飛期間需要補(bǔ)償?shù)哪芰?；補(bǔ)償カ控制器通過(guò)改變彈簧剛度，改變作用在等效質(zhì)量上的作用カ實(shí)現(xiàn)系統(tǒng)能量的補(bǔ)償；系統(tǒng)狀態(tài)檢測(cè)模 塊用于實(shí)時(shí)檢測(cè)系統(tǒng)控制所需的系統(tǒng)狀態(tài)量，并提供給穩(wěn)定控制模塊。
全文摘要
一種具有環(huán)境適應(yīng)性的足式機(jī)器人穩(wěn)定性控制方法及系統(tǒng)，該控制方法通過(guò)利用上一觸地過(guò)程相關(guān)參數(shù)信息與期望達(dá)到的控制目標(biāo)進(jìn)行比較，對(duì)飛行相水平運(yùn)動(dòng)速度和系統(tǒng)總能量實(shí)行反饋控制，預(yù)測(cè)控制觸地角度并進(jìn)行系統(tǒng)能量補(bǔ)償控制，最終實(shí)現(xiàn)足式機(jī)器人SLIP等效模型在不同地面環(huán)境下的期望穩(wěn)定周期運(yùn)動(dòng)。系統(tǒng)包括系統(tǒng)狀態(tài)檢測(cè)模塊和穩(wěn)定控制模塊。本發(fā)明不需要建立具體的機(jī)器人動(dòng)力學(xué)模型，不需要計(jì)算精確的不動(dòng)點(diǎn)觸地角度，通過(guò)反饋控制實(shí)現(xiàn)控制收斂，控制方法簡(jiǎn)單，計(jì)算迅速，很好的解決了現(xiàn)有方法控制實(shí)時(shí)性不足、適應(yīng)性不夠等問(wèn)題。且具有較好的未知環(huán)境適應(yīng)性，為足式機(jī)器人穩(wěn)定性控制提供了一種較好的解決方案。
文檔編號(hào)G05D1/02GK102736628SQ20121019283
公開日2012年10月17日 申請(qǐng)日期2012年6月12日 優(yōu)先權(quán)日2012年6月12日
發(fā)明者劉清宇, 周博, 姜偉, 程品, 羅欣, 陳學(xué)東, 陳霈, 韓斌 申請(qǐng)人:華中科技大學(xué)