[發(fā)明專利]一種多足機(jī)器人平衡控制方法

有效

• 申請(qǐng)?zhí)枺?/td>: 201210198798.6
• 申請(qǐng)日: 2012-06-15
• 公開（公告）號(hào): CN102749919A
• 公開（公告）日: 2012-10-24
• 發(fā)明（設(shè)計(jì)）人: 陳學(xué)東;韓斌;羅欣;鐘建鋒;劉志江;陳思宇;李栓柱;嚴(yán)可涵
• 申請(qǐng)（專利權(quán)）人: 華中科技大學(xué)
• 主分類號(hào): G05D1/02
• 分類號(hào): G05D1/02
• 代理公司: 華中科技大學(xué)專利中心 42201
• 代理人: 曹葆青

1.一種多足機(jī)器人平衡控制方法，其特征在于，該方法包括下述步驟：

(1)獲取當(dāng)前的機(jī)器人各關(guān)節(jié)角度值和機(jī)身位置姿態(tài)值，以初始化機(jī)器人各關(guān)節(jié)角度、機(jī)身位姿參數(shù)；

(2)實(shí)時(shí)獲取各關(guān)節(jié)角度、機(jī)身位姿信息，并利用這些信息對(duì)機(jī)器人進(jìn)行正運(yùn)動(dòng)學(xué)計(jì)算，得到機(jī)身重心、腿部各部分重心在機(jī)器人世界坐標(biāo)系中的坐標(biāo)；

(3)利用獲取的各足端腳力信息、機(jī)器人各關(guān)節(jié)角度信息和機(jī)身位姿信息，構(gòu)建出當(dāng)前運(yùn)動(dòng)時(shí)刻的支撐平面；

(4)使用零力矩點(diǎn)方法判斷機(jī)器人的穩(wěn)定性，根據(jù)零力矩點(diǎn)的位置判定機(jī)器人是否失穩(wěn)并計(jì)算穩(wěn)定裕度d_m；

(5)根據(jù)機(jī)器人當(dāng)前的穩(wěn)定性情況采取相應(yīng)控制策略：若機(jī)器人穩(wěn)定，則實(shí)施預(yù)設(shè)的正常行走步態(tài)，并轉(zhuǎn)入步驟(2)；若機(jī)器人失穩(wěn)，進(jìn)入步驟(6)；

(6)計(jì)算調(diào)整腿末端點(diǎn)H_At的位置約束條件；

(7)取步驟(4)中計(jì)算的穩(wěn)定裕度d_m與預(yù)設(shè)的穩(wěn)定裕度閾值S進(jìn)行比較：若穩(wěn)定裕度為負(fù)值且|d_m|＜S，則判定為機(jī)器人失穩(wěn)程度較小，并進(jìn)入步驟(8)；若穩(wěn)定裕度為負(fù)值且|d_m|≥S，則判定為機(jī)器人失穩(wěn)程度較大，執(zhí)行步驟(9)；

(8)根據(jù)調(diào)整腿末端點(diǎn)位于機(jī)器人所受合力在支撐面上的投影線上或者投影延長線上的原則，計(jì)算調(diào)整腿末端點(diǎn)H_At的位置坐標(biāo)，并轉(zhuǎn)入步驟(10)；

(9)根據(jù)髖關(guān)節(jié)到傾翻軸線的垂足與調(diào)整腿末端點(diǎn)的連線，垂直于傾翻軸線的原則，計(jì)算調(diào)整腿末端點(diǎn)H_At的位置坐標(biāo)，并轉(zhuǎn)入步驟(10)；

(10)計(jì)算出調(diào)整腿末端的位置后經(jīng)機(jī)器人運(yùn)動(dòng)學(xué)反解得到機(jī)器人各關(guān)節(jié)角度值，然后向執(zhí)行機(jī)構(gòu)輸出各關(guān)節(jié)角度序列，執(zhí)行系統(tǒng)動(dòng)作使機(jī)器人到達(dá)控制器要求達(dá)到的位置。

2.根據(jù)權(quán)利要求1所述的多足機(jī)器人平衡控制方法，其特征在于，步驟(7)中，穩(wěn)定裕度用零力矩點(diǎn)到支撐多邊形的各邊距離的最小值來表示

3.根據(jù)權(quán)利要求1所述的多足機(jī)器人平衡控制方法，其特征在于，步驟(3)中，構(gòu)建支撐平面的過程為：在機(jī)器人運(yùn)動(dòng)過程中，首先根據(jù)機(jī)器人傳感系統(tǒng)獲得的腳力信息確定腳力最大的三個(gè)著地足，假設(shè)為足k、足l、足m，由著地點(diǎn)的位置信息求出當(dāng)前運(yùn)動(dòng)時(shí)刻的支撐平面的法向向量，所述支撐平面的法向向量^CV_A計(jì)算公式為：

^CV_A＝(x_l-k，y_l-k，z_l-k)×(x_m-k，y_m-k，z_m-k)????????????式I

???＝(y_l-kz_m-k-z_l-ky_m-k，z_l-kx_m-k-x_l-kz_m-k，x_l-ky_m-k-y_l-kx_m-k)

其中x_l-k＝x_l-x_k，y_l-k＝y(tǒng)_l-y_k，z_l-k＝z_l-z_k；x_m-k＝x_m-x_k，y_m-k＝y(tǒng)_m-y_k，z_m-k＝z_m-z_k^CP_Ak＝(x_k，y_k，z_k)，^CP_Al＝(x_l，y_l，z_l)，^CP_Am＝(x_m，y_m，z_m)為腳力最大的三個(gè)著地足k、l、m末端點(diǎn)在機(jī)身坐標(biāo)系∑C中的位置坐標(biāo)。