專利名稱：仿人機(jī)器人穩(wěn)定行走的上身姿態(tài)控制的方法和裝置的制作方法
技術(shù)領(lǐng)域：
本發(fā)明涉及自動化控制領(lǐng)域，特別涉及一種仿人機(jī)器人穩(wěn)定行走的上身姿態(tài)控制的方法 和裝置。
背景技術(shù)：
仿人機(jī)器人(以下簡稱機(jī)器人)，又稱雙足機(jī)器人，跟人類一樣，是靠兩條腿的行走實現(xiàn) 移動的。它的雙腿結(jié)構(gòu)跟人類似，較傳統(tǒng)的輪式機(jī)器人和履帶式機(jī)器人有更好的機(jī)動性，尤 其是在凹凸不平的地面、樓梯以及與地面僅有離散不連續(xù)接觸點(diǎn)的場合更體現(xiàn)出優(yōu)越性。
為了使機(jī)器人行走，需要預(yù)先設(shè)定機(jī)器人的行走軌跡(動態(tài)步態(tài))。機(jī)器人的動態(tài)步態(tài)是 一種固有的、周期的運(yùn)動，是依據(jù)雙足機(jī)器人整體動力學(xué)產(chǎn)生的。由于約束條件的耦合性和 動力學(xué)方程的復(fù)雜性，動態(tài)步態(tài)計算需要一個優(yōu)化過程。但是，動態(tài)步態(tài)一般只能通過離線 計算方法來實現(xiàn)，也就是說，動態(tài)步態(tài)一般是在假設(shè)雙足機(jī)器人模型和周圍環(huán)境已知的情況下 生成的。實際上，雙足機(jī)器人行走的現(xiàn)實環(huán)境不可能與設(shè)定的環(huán)境和條件完全相同， 一旦機(jī) 器人周圍環(huán)境發(fā)生變化，如果機(jī)器人機(jī)械地按照預(yù)先規(guī)劃好的動態(tài)步態(tài)完全執(zhí)行，不對規(guī)劃 的動態(tài)步態(tài)進(jìn)行實時修正和控制，很可能會產(chǎn)生不穩(wěn)定甚至摔倒等異?，F(xiàn)象。因此，必須根 據(jù)當(dāng)前的環(huán)境信息和機(jī)器人當(dāng)前的狀態(tài)，對規(guī)劃的動態(tài)步態(tài)進(jìn)行修正，克服環(huán)境的改變與不 確定性，使機(jī)器人能在實際環(huán)境中穩(wěn)定行走。
現(xiàn)有技術(shù)中，對雙足機(jī)器人進(jìn)行穩(wěn)定控制往往采用基于ZMP (Zero Moment Point,零力 矩點(diǎn))補(bǔ)償?shù)目刂品椒?，見日本專利出版物NO. 1993-305586，主要是通過改變機(jī)器人的上身 和修正機(jī)器人足部位置來實現(xiàn)ZMP補(bǔ)償。
在實現(xiàn)本發(fā)明的過程中，發(fā)明人發(fā)現(xiàn)現(xiàn)有技術(shù)至少存在以下問題
基于ZMP補(bǔ)償?shù)目刂品椒ǎ瑱C(jī)器人模型精度制約補(bǔ)償效果，需要對機(jī)器人的數(shù)學(xué)模型進(jìn) 行計算，不易求解，而且計算量大，補(bǔ)償時延大，不利于實時性控制。

發(fā)明內(nèi)容
為了簡化計算，減小時延，擴(kuò)大適用范圍，本發(fā)明實施例提供了一種仿人機(jī)器人穩(wěn)定行走的上身姿態(tài)控制的方法和裝置。所述技術(shù)方案如下
一種仿人機(jī)器人穩(wěn)定行走的上身姿態(tài)控制的方法，所述方法包括 判斷當(dāng)前時刻機(jī)器人的腳的狀態(tài)，所述腳的狀態(tài)為支撐狀態(tài)或擺動狀態(tài)； 根據(jù)所述腳的狀態(tài)計算當(dāng)前時刻所述機(jī)器人的髖關(guān)節(jié)的修正值；
根據(jù)所述修正值計算所述髖關(guān)節(jié)的伺服參考值，所述伺服參考值用于驅(qū)動所述髖關(guān)節(jié)調(diào) 節(jié)所述機(jī)器人的上身姿態(tài)。
一種仿人機(jī)器人穩(wěn)定行走的上身姿態(tài)控制的裝置，所述裝置包括
判斷模塊，用于判斷當(dāng)前時刻機(jī)器人的腳的狀態(tài)，所述腳的狀態(tài)為支撐狀態(tài)或擺動狀態(tài)； 第一計算模塊，用于根據(jù)所述判斷模塊判斷的腳的狀態(tài)，計算當(dāng)前時刻所述機(jī)器人的髖 關(guān)節(jié)的修正值；
第二計算模塊，用于根據(jù)所述第一計算模塊得到的修正值計算所述髖關(guān)節(jié)的伺服參考值， 所述伺服參考值用于驅(qū)動所述髖關(guān)節(jié)調(diào)節(jié)所述機(jī)器人的上身姿態(tài)。 本發(fā)明實施例提供的技術(shù)方案的有益效果是
通過調(diào)節(jié)髖關(guān)節(jié)來調(diào)整上身姿態(tài)，以達(dá)到上身平衡的目的，不需要機(jī)器人具體的數(shù)學(xué)模 型，適用范圍廣，計算簡單，時延小，能夠?qū)崟r保持機(jī)器人上身的動態(tài)平衡。


圖1是本發(fā)明實施例1提供的仿人機(jī)器人穩(wěn)定行走的上身姿態(tài)控制的方法流程圖； 圖2是本發(fā)明實施例1提供的機(jī)器人步行控制器結(jié)構(gòu)示意圖3是本發(fā)明實施例1提供的仿人機(jī)器人穩(wěn)定行走的上身姿態(tài)控制的方法詳細(xì)流程圖； 圖4是本發(fā)明實施例2提供的仿人機(jī)器人穩(wěn)定行走的上身姿態(tài)控制的裝置結(jié)構(gòu)示意圖； 圖5是本發(fā)明實施例2提供的判斷模塊結(jié)構(gòu)示意圖； 圖6是本發(fā)明實施例2提供的第一計算模塊結(jié)構(gòu)示意圖。
具體實施例方式
為使本發(fā)明的目的、技術(shù)方案和優(yōu)點(diǎn)更加清楚，下面將結(jié)合附圖對本發(fā)明實施方式作進(jìn) 一步地詳細(xì)描述。 實施例1
參見圖1，本實施例提供了一種仿人機(jī)器人穩(wěn)定行走的上身姿態(tài)控制的方法，該方法通 過調(diào)節(jié)髖關(guān)節(jié)實現(xiàn)仿人機(jī)器人的穩(wěn)定行走，包括步驟101:判斷當(dāng)前時刻機(jī)器人的腳的狀態(tài)，腳的狀態(tài)為支撐狀態(tài)或擺動狀態(tài)； 步驟102:根據(jù)腳的狀態(tài)計算當(dāng)前時刻該機(jī)器人的髖關(guān)節(jié)的修正值；
步驟103:根據(jù)該修正值計算該髖關(guān)節(jié)的伺服參考值，該伺服參考值用于驅(qū)動該髖關(guān)節(jié) 調(diào)節(jié)該機(jī)器人的上身姿態(tài)。
本實施例通過調(diào)節(jié)髖關(guān)節(jié)來調(diào)整上身姿態(tài)，以達(dá)到仿人機(jī)器人穩(wěn)定行走的目的，不需要 具體的機(jī)器人數(shù)學(xué)模型，適用范圍廣，計算簡單，補(bǔ)償時延小，能夠?qū)崟r保持機(jī)器人上身的 動態(tài)平衡。
圖2所示的仿人機(jī)器人步行控制器結(jié)構(gòu)圖，仿人機(jī)器人包括上身1、髖關(guān)節(jié)2、膝關(guān)節(jié)3、 踝關(guān)節(jié)4、腳5、腳踝處的力傳感器6及其連接件?？刂破鞑捎秒p結(jié)構(gòu)，由起前饋?zhàn)饔玫膭討B(tài)
步態(tài)值《。(0和起局部反饋?zhàn)饔玫馁空礎(chǔ)《W構(gòu)成，由二者得出的伺服參考值《(0輸入伺 服驅(qū)動器，控制仿人機(jī)器人行走。其中，動態(tài)步態(tài)值《。( )是在已知仿人機(jī)器人模型和周圍環(huán) 境的情況下預(yù)先生成的髖關(guān)節(jié)的角度，修正值A(chǔ)《(0是當(dāng)仿人機(jī)器人的上身實際姿態(tài)與上身 規(guī)劃姿態(tài)出現(xiàn)偏差時，為了克服環(huán)境不確定性生成的髖關(guān)節(jié)角度的實時修正量，伺服參考值 《(r)是髖關(guān)節(jié)的驅(qū)動角度。下面結(jié)合圖2和圖3，詳細(xì)說明仿人機(jī)器人穩(wěn)定行走的上身姿態(tài) 控制的方法。
步驟201:腳踝處的力傳感器6獲取機(jī)器人的腳5與地面之間接觸力F/Q。,。
步驟202:根據(jù)獲取的接觸力^。。,判斷腳的狀態(tài)，若接觸力i^。。,大于零，則當(dāng)前時刻腳
的狀態(tài)為支撐狀態(tài)，否則，當(dāng)前時刻腳的狀態(tài)為擺動狀態(tài)。
步驟203:若是支撐狀態(tài)，(步驟203a)計算上身1實際姿態(tài)角與上身1規(guī)劃姿態(tài)角之間
的偏差，對偏差積分得到修正值，(步驟203b)動態(tài)步態(tài)值加上該修正值所得的值作為髖關(guān)
節(jié)2的伺服參考值。修正值具體可以采用如下公式進(jìn)行計算
<formula>formula see original document page 6</formula> (1)
其中，A《^( )為f時刻機(jī)器人的上身實際姿態(tài)角與上身規(guī)劃姿態(tài)角之間的偏差。上身
實際姿態(tài)角，是通過陀螺儀測得的當(dāng)前時刻機(jī)器人上身的實際傾角；上身規(guī)劃姿態(tài)角，是預(yù) 先設(shè)定的機(jī)器人上身的傾角，可以作為已知量。X。是系數(shù)，且0<&<1。
伺服參考值為
<formula>formula see original document page 6</formula> (2) 其中，動態(tài)步態(tài)值《。(f)是預(yù)先規(guī)劃好的，可以作為已知量，再加上(1)中得到的當(dāng)前 時刻t的修正值A(chǔ)《(0就可以計算出伺服參考值《(0 。
另外，髖關(guān)節(jié)角度，是髖關(guān)節(jié)從初始零位轉(zhuǎn)過的角度。髖關(guān)節(jié)角度既有大小，又有方向
性。初始零位，是機(jī)器人行走之前的初始狀態(tài)位。在本實施例中，動態(tài)步態(tài)值和伺服參考值
都屬于髖關(guān)節(jié)角度。在具體定義機(jī)器人的上身姿態(tài)傾角時，可以設(shè)定豎直方向為參考方向，上身和參考方向 的角度偏差定義為上身姿態(tài)傾角。上身姿態(tài)傾角也是既有大小，又有方向性。在本實施例中， 上身實際姿態(tài)角和上身規(guī)劃姿態(tài)角都屬于上身姿態(tài)傾角。
步驟204:若是擺動狀態(tài)，(步驟204a)對前期積累的偏差按一定比例進(jìn)行積分得到修正 值，(步驟204b)動態(tài)步態(tài)值加上該修正值所得的值作為髖關(guān)節(jié)2的伺服參考值。修正值具 體可以采用如下公式進(jìn)行計算
A,-(u貝(卜:r) (3)
其中，T為計算機(jī)控制的采樣周期，i^為預(yù)先設(shè)定的比例積分系數(shù)，0<《〃<1，其 它符號的含義與式(1)及(2)相同，這里不再贅述。 伺服參考值為
(4)
其中，各符號的含義與式(2)相同，這里不再贅述。
步驟205:伺服驅(qū)動器根據(jù)伺服參考值《(f)驅(qū)動髖關(guān)節(jié)，調(diào)整機(jī)器人上身姿態(tài)。
例如地面出現(xiàn)異常，機(jī)器人上身比規(guī)劃姿態(tài)前傾，此時，應(yīng)該髖關(guān)節(jié)一個修正量A《(f)，
使機(jī)器人適當(dāng)后傾，從而保持機(jī)器人上身的平衡。對于后傾、左傾、右傾等異常情況的處理，
均與前傾相同，這里不再贅述。
分別對機(jī)器人的左右腳，執(zhí)行步驟201-205，就可以實時地調(diào)整機(jī)器人的上身姿態(tài)，保
持機(jī)器人上身的動態(tài)平衡。
本實施例通過調(diào)節(jié)髖關(guān)節(jié)來調(diào)整上身姿態(tài)，以達(dá)到上身穩(wěn)定的目的，不需要機(jī)器人具體 的數(shù)學(xué)模型，適用范圍廣，計算簡單，補(bǔ)償時延小，能夠?qū)崟r保持機(jī)器人上身的動態(tài)平衡。
實施例2
參見圖4， 一種仿人機(jī)器人穩(wěn)定行走的上身姿態(tài)控制的裝置，其特征在于，所述裝置包
括
判斷模塊401，用于判斷當(dāng)前時刻機(jī)器人的腳的狀態(tài)，腳的狀態(tài)為支撐狀態(tài)或擺動狀態(tài)； 第一計算模塊402，用于根據(jù)該判斷模塊401判斷的腳的狀態(tài)，計算當(dāng)前時刻該機(jī)器人 的髖關(guān)節(jié)的修正值；
第二計算模塊403，用于根據(jù)該第一計算模塊402得到的修正值計算該髖關(guān)節(jié)的伺服參 考值，該伺服參考值用于驅(qū)動該髖關(guān)節(jié)調(diào)節(jié)該機(jī)器人的上身姿態(tài)。 具體地，
參見圖5，該判斷模塊401包括
獲取單元401a，用于獲取該機(jī)器人的腳與地面之間接觸力；
判斷單元401b,用于判斷若該獲取單元401a獲取的接觸力大于零，當(dāng)前時刻該腳的狀 態(tài)為支撐狀態(tài)，否則，當(dāng)前時刻該腳的狀態(tài)為擺動狀態(tài)。參見圖6，該第一計算模塊402包括
第一計算單元402a，用于若當(dāng)前時刻該腳的狀態(tài)為支撐狀態(tài)時，該修正值 J^JA《^(0^， A《^(0為f時刻該機(jī)器人的上身實際姿態(tài)角與上身規(guī)劃姿態(tài)角之
間的偏差，該上身實際姿態(tài)角是通過陀螺儀測得的當(dāng)前時刻該機(jī)器人上身的實際傾角，該上 身規(guī)劃姿態(tài)角是預(yù)先設(shè)定的該機(jī)器人上身的傾角，^是系數(shù)，且0<&<1。
第二計算單元402b，用于若當(dāng)前時刻該腳的狀態(tài)為擺動狀態(tài)時，該修正值 △《W = (1-&)A《(f-r)，其中，T為計算機(jī)控制采樣周期，尺,為系數(shù)，0<尺,<1。
該第二計算模塊403還用于
將動態(tài)步態(tài)值加上該修正值所得的值作為該伺服參考值，該動態(tài)步態(tài)值為預(yù)先設(shè)定的該 機(jī)器人的髖關(guān)節(jié)的角度。
本實施例提供的裝置通過調(diào)節(jié)髖關(guān)節(jié)來調(diào)整上身姿態(tài)，以達(dá)到上身穩(wěn)定的目的，不需要 具體的機(jī)器人數(shù)學(xué)模型，適用范圍廣，計算簡單，補(bǔ)償時延小，能夠?qū)崟r保持機(jī)器人上身的 動態(tài)平衡。
以上實施例提供的技術(shù)方案中的全部或部分內(nèi)容可以通過軟件編程實現(xiàn)，其軟件程序存 儲在可讀取的存儲介質(zhì)中，存儲介質(zhì)例如計算機(jī)中的硬盤、光盤或軟盤。
以上所述僅為本發(fā)明的較佳實施例，并不用以限制本發(fā)明，凡在本發(fā)明的精神和原則之 內(nèi)，所作的任何修改、等同替換、改進(jìn)等，均應(yīng)包含在本發(fā)明的保護(hù)范圍之內(nèi)。
權(quán)利要求
1. 一種仿人機(jī)器人穩(wěn)定行走的上身姿態(tài)控制的方法，其特征在于，所述方法包括判斷當(dāng)前時刻機(jī)器人的腳的狀態(tài)，所述腳的狀態(tài)為支撐狀態(tài)或擺動狀態(tài)；根據(jù)所述腳的狀態(tài)計算當(dāng)前時刻所述機(jī)器人的髖關(guān)節(jié)的修正值；根據(jù)所述修正值計算所述髖關(guān)節(jié)的伺服參考值，所述伺服參考值用于驅(qū)動所述髖關(guān)節(jié)調(diào)節(jié)所述機(jī)器人的上身姿態(tài)。
2. 如權(quán)利要求l所述的仿人機(jī)器人穩(wěn)定行走的上身姿態(tài)控制的方法，其特征在于，所述 判斷當(dāng)前時刻機(jī)器人的腳的狀態(tài)包括獲取所述機(jī)器人的腳與地面之間接觸力；若所述接觸力大于零，當(dāng)前時刻所述腳的狀態(tài)為支撐狀態(tài)，否則，當(dāng)前時刻所述腳的狀 態(tài)為擺動狀態(tài)。
3. 如權(quán)利要求l所述的仿人機(jī)器人穩(wěn)定行走的上身姿態(tài)控制的方法，其特征在于，所述 根據(jù)所述腳的狀態(tài)計算所述機(jī)器人的髖關(guān)節(jié)的修正值包括若當(dāng)前時刻所述腳的狀態(tài)為支撐狀態(tài)時，所述修正值厶《(0= ^[i^A^^)A ，為f時刻所述機(jī)器人的上身實際姿態(tài)角與上身規(guī)劃姿態(tài)角之間的偏差，所述上身實際姿態(tài)角是 通過陀螺儀測得的當(dāng)前時刻所述機(jī)器人上身的實際傾角，所述上身規(guī)劃姿態(tài)角是預(yù)先設(shè)定的 所述機(jī)器人上身的傾角，^是系數(shù)，0<^<1;若當(dāng)前時刻所述腳的狀態(tài)為擺動狀態(tài)時，所述修正值A(chǔ)《w-(i-^)A《(z-:r)，其中，T為計算機(jī)控制采樣周期，A是系數(shù)，0<&<1。
4. 如權(quán)利要求l所述的仿人機(jī)器人穩(wěn)定行走的上身姿態(tài)控制的方法，其特征在于，所述 根據(jù)所述修正值計算所述髖關(guān)節(jié)的伺服參考值包括-將動態(tài)步態(tài)值加上所述修正值所得的值作為所述伺服參考值，所述動態(tài)步態(tài)值為預(yù)先設(shè) 定的所述機(jī)器人的髖關(guān)節(jié)的角度。
5. —種仿人機(jī)器人穩(wěn)定行走的上身姿態(tài)控制的裝置，其特征在于，所述裝置包括 判斷模塊，用于判斷當(dāng)前時刻機(jī)器人的腳的狀態(tài)，所述腳的狀態(tài)為支撐狀態(tài)或擺動狀態(tài); 第一計算模塊，用于根據(jù)所述判斷模塊判斷的腳的狀態(tài)，計算當(dāng)前時刻所述機(jī)器人的髖關(guān)節(jié)的修正值；第二計算模塊，用于根據(jù)所述第一計算模塊得到的修正值計算所述髖關(guān)節(jié)的伺服參考值， 所述伺服參考值用于驅(qū)動所述髖關(guān)節(jié)調(diào)節(jié)所述機(jī)器人的上身姿態(tài)。
6. 如權(quán)利要求5所述的仿人機(jī)器人穩(wěn)定行走的上身姿態(tài)控制的裝置，其特征在于，所述 判斷模塊包括獲取單元，用于獲取所述機(jī)器人的腳與地面之間接觸力；判斷單元，用于判斷若所述獲取單元獲取的接觸力大于零，當(dāng)前時刻所述腳的狀態(tài)為支 撐狀態(tài)，否則，當(dāng)前時刻所述腳的狀態(tài)為擺動狀態(tài)。
7. 如權(quán)利要求5所述的仿人機(jī)器人穩(wěn)定行走的上身姿態(tài)控制的裝置，其特征在于，所述 第一計算模塊包括第一計算單元，用于若當(dāng)前時刻所述腳的狀態(tài)為支撐狀態(tài)時，所述修正值 △《(0= J[x/a^^)&，其中a《^(0為f時刻所述機(jī)器人的上身實際姿態(tài)角與上身規(guī)劃姿態(tài)角之間的偏差，所述上身實際姿態(tài)角是通過陀螺儀測得的當(dāng)前時刻所述機(jī)器人上身的實 際傾角，所述上身規(guī)劃姿態(tài)角是預(yù)先設(shè)定的所述機(jī)器人上身的傾角，A是系數(shù)，且0<^^<1; 第二計算單元，用于若當(dāng)前時刻所述腳的狀態(tài)為擺動狀態(tài)時，所述修正值△《(0 = (i-&)A6Uf-r)，其中t為計算機(jī)控制采樣周期，^是系數(shù)，0<&<1。
8. 如權(quán)利要求5所述的仿人機(jī)器人穩(wěn)定行走的上身姿態(tài)控制的裝置，其特征在于，所述 第二計算模塊還用于將動態(tài)步態(tài)值加上所述修正值所得的值作為所述伺服參考值，所述動態(tài)步態(tài)值為預(yù)先設(shè) 定的所述機(jī)器人的髖關(guān)節(jié)的角度。
全文摘要
本發(fā)明公開了一種仿人機(jī)器人穩(wěn)定行走的上身姿態(tài)的控制方法和裝置，屬于自動化控制領(lǐng)域。所述方法包括判斷當(dāng)前時刻機(jī)器人的腳的狀態(tài)，所述腳的狀態(tài)為支撐狀態(tài)或擺動狀態(tài)；根據(jù)所述腳的狀態(tài)計算當(dāng)前時刻所述機(jī)器人的髖關(guān)節(jié)的修正值；根據(jù)所述修正值計算所述髖關(guān)節(jié)的伺服參考值。所述裝置包括判斷模塊、第一計算模塊和第二計算模塊。本發(fā)明通過調(diào)節(jié)髖關(guān)節(jié)來調(diào)整上身姿態(tài)，以達(dá)到機(jī)器人行走穩(wěn)定的目的，不需要具體的機(jī)器人數(shù)學(xué)模型，適用范圍廣，計算簡單，補(bǔ)償時延小，實時保持機(jī)器人上身動態(tài)平衡的效果。
文檔編號G05D1/08GK101414189SQ200810171980
公開日2009年4月22日 申請日期2008年10月28日 優(yōu)先權(quán)日2008年10月28日
發(fā)明者余張國, 張偉民, 陳學(xué)超, 高峻峣, 強(qiáng) 黃 申請人:北京理工大學(xué)