本發(fā)明涉及機(jī)器人穩(wěn)定性領(lǐng)域，具體涉及一種考慮外力沖擊干擾和阻尼的多足機(jī)器人能量裕度計(jì)算方法。

背景技術(shù)：

近年來(lái)，隨著機(jī)器人技術(shù)日益成熟，在探險(xiǎn)、焊接、太空、搬運(yùn)、服務(wù)方面得到非常廣泛的應(yīng)用。多足機(jī)器人是一個(gè)復(fù)雜的機(jī)電一體化系統(tǒng)。作為被控對(duì)象，其本身是一個(gè)高度非線化、多變量、多參數(shù)耦合的復(fù)雜系統(tǒng)，使對(duì)其穩(wěn)定性控制變的尤為困難。因此機(jī)器人穩(wěn)定性控制問(wèn)題成為近年來(lái)研究的熱點(diǎn)問(wèn)題。

機(jī)器人在野外行走，六足機(jī)器人相對(duì)二足、四足機(jī)器人而言，具有更豐富的運(yùn)動(dòng)步態(tài)。且六足機(jī)器人具有冗余的腿，機(jī)器人在復(fù)雜路面行走時(shí)，即使當(dāng)一條腿或若干腿失效時(shí)，只要有三條腿能保持支撐狀態(tài)，機(jī)器人能繼續(xù)保持一定的運(yùn)動(dòng)能力。因此本發(fā)明以六足機(jī)器人在崎嶇地面上行走作為研究對(duì)像。

目前針對(duì)六足機(jī)器人在崎嶇地面上動(dòng)態(tài)穩(wěn)定性研究，通常采用是零力矩(ZMP)判定、壓力中心法(COP)，然而其只考慮了機(jī)器人的足端反力，考慮因素過(guò)少，且不能反映機(jī)器人的抗干擾能力。傳統(tǒng)的能量穩(wěn)定邊界法沒(méi)有考慮到機(jī)器人的機(jī)械震蕩屬性。而李雅普諾夫穩(wěn)定性理論難于對(duì)時(shí)變、非線性化做出瞬時(shí)響應(yīng)。因此，目前需要一種機(jī)器人穩(wěn)定性判定的方法能實(shí)時(shí)監(jiān)測(cè)機(jī)器人的穩(wěn)定性，使機(jī)器人能快速做出調(diào)整，保持穩(wěn)定的行走。

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

針對(duì)現(xiàn)有的穩(wěn)定性判定方法存在考慮影響因素太少或時(shí)效性差的問(wèn)題，本發(fā)明提出一種考慮外力沖擊干擾和阻尼的多足機(jī)器人能量裕度計(jì)算方法。該算法主要依據(jù)重心位置、虛擬彈性阻尼、合力矩得出，最終用于機(jī)器人在復(fù)雜路面行走時(shí)因外力沖擊和自身慣性力矩作用，判定是否發(fā)生傾覆，以確保機(jī)器人穩(wěn)定的行走。最終提出一種能適用于多種地形，且時(shí)效的穩(wěn)定性計(jì)算方法。

本發(fā)明解決其技術(shù)問(wèn)題所采用的技術(shù)方案是：

一種考慮外力沖擊干擾和阻尼的多足機(jī)器人能量裕度計(jì)算方法，包括如下步驟：

步驟一：把機(jī)器人的翻轉(zhuǎn)動(dòng)作轉(zhuǎn)換為包含彈簧阻尼特性的機(jī)械旋轉(zhuǎn)系統(tǒng)，計(jì)算機(jī)器人的能量裕度表達(dá)式；

步驟二：建立機(jī)器人在笛卡爾坐標(biāo)系下的D-H數(shù)學(xué)模型；

步驟三：結(jié)合步驟二解析機(jī)器人能量裕度函數(shù)所需有關(guān)參數(shù)。

進(jìn)一步，所述的步驟一通過(guò)以下方式實(shí)現(xiàn)：

1.1)根據(jù)牛頓第二定律列出機(jī)器人的動(dòng)力學(xué)微分方程：

其中，將繞邊線翻轉(zhuǎn)時(shí)的機(jī)器人，視為一個(gè)機(jī)械旋轉(zhuǎn)系統(tǒng)，該系統(tǒng)由轉(zhuǎn)動(dòng)慣量為J_i、扭轉(zhuǎn)剛度為K_S和粘性阻尼系數(shù)為C的阻尼器組成；

1.2)求解機(jī)器人繞邊線旋轉(zhuǎn)的實(shí)時(shí)重心處合力矩，即驅(qū)動(dòng)力矩：

式中，M_Gi為實(shí)時(shí)重心處重力相對(duì)旋轉(zhuǎn)邊線產(chǎn)生的力矩；M_I為實(shí)時(shí)重心處的等效慣性力矩；M_ri為著地腳因支撐力產(chǎn)生的力矩；f_i為各足端豎直向上的力；θ_i為直線AB與水平面夾角，δ_i為重心繞邊線旋轉(zhuǎn)角度；θ為斜線傾角；r_i為各足端到旋轉(zhuǎn)邊線的距離；e_i為旋轉(zhuǎn)邊線的單位法向量，以支撐多邊形逆時(shí)針?lè)较驗(yàn)檎较颍?/p>

1.3)系數(shù)K_S、C測(cè)定，根據(jù)上述的機(jī)械旋轉(zhuǎn)系統(tǒng)整理得

兩邊進(jìn)行拉氏變換，求得其傳遞函數(shù)為：

因此求得

式中，T_C、T_S分別為當(dāng)機(jī)器人受到一個(gè)脈沖力時(shí)，機(jī)械旋轉(zhuǎn)系統(tǒng)的震蕩周期與穩(wěn)定時(shí)間；

1.4)求解機(jī)器人考慮外力沖擊干擾和阻尼的能量裕度

機(jī)器人繞邊線旋轉(zhuǎn)到最高點(diǎn)的勢(shì)能變化：

機(jī)器人繞邊線旋轉(zhuǎn)到最高點(diǎn)的動(dòng)能變化：

當(dāng)w₀為正時(shí)：

當(dāng)w₀為負(fù)時(shí)：

假設(shè)機(jī)器人繞邊線旋轉(zhuǎn)到最高點(diǎn)時(shí)w_i＝0，因此求得機(jī)器人當(dāng)前位置因外界干擾不發(fā)生完全側(cè)翻，所能抵抗的能量為：

E_i＝V_i-V₀+K_i-K₀

求得：

上述公式中，w₀、M_Ri'分別為實(shí)時(shí)重心位置繞邊線旋轉(zhuǎn)的角速度與合力矩，方向延旋轉(zhuǎn)邊線，以支撐多邊形逆時(shí)針?lè)较驗(yàn)檎较颍?/p>

根據(jù)能量穩(wěn)定裕量的概念可知，定義能量穩(wěn)定裕度為SE_m，設(shè)最小穩(wěn)定裕度SE，當(dāng)SEm≥SE時(shí)，機(jī)器人能穩(wěn)定工作。

再進(jìn)一步，所述的步驟二通過(guò)以下方式實(shí)現(xiàn)：

在機(jī)器人的各關(guān)節(jié)處與軀體中心，建立笛卡爾坐標(biāo)系，設(shè)坐標(biāo)系C到坐標(biāo)系O的之間的旋轉(zhuǎn)變換為線性變換為^OP_C，因此求得C到O坐標(biāo)系的齊次變換為：

當(dāng)空間有n個(gè)坐標(biāo)系時(shí)，若已知相鄰坐標(biāo)系之間的齊次變換矩陣，則：

M_0n＝M₀₁·M₀₂·…M_n-1n

其中M_ij為相鄰坐標(biāo)系的齊次變換，由此建立機(jī)器人各部分在坐標(biāo)系O中的位置。

更進(jìn)一步，所述的步驟三通過(guò)以下方式實(shí)現(xiàn)：

4.1)重心位置參數(shù)，

假設(shè)各部件均為勻質(zhì)材料，因此其重心均在其幾何中心處，根據(jù)步驟二中齊次變換矩陣，求各部件重心在世界坐標(biāo)系的位置；

以軀體重心在世界坐標(biāo)系中的位置求解為例：

式中，表示軀體重心在坐標(biāo)系O_C中的位置；

同理求得各部件重心在O的位置，根據(jù)各重心位置坐標(biāo)代入相關(guān)公式求得實(shí)時(shí)重心位置；

4.2)求解實(shí)時(shí)重心繞支撐邊線旋轉(zhuǎn)到最高位置的豎直位移；

設(shè)構(gòu)成傾翻邊線的兩足端位置坐標(biāo)：

A(x_A,y_A,z_A)，B(x_B,y_B,z_B)

首先，由幾何關(guān)系式，求直線AB的表達(dá)式與重心CG到直線AB的距離H_i；

再次，求重心繞AB旋轉(zhuǎn)到最高點(diǎn)的豎直位移；

S_NEi＝ECG'cosθ＝H_i(1-cosδ_i)cosθ_i

式中θ_i為直線AB與水平面夾角，δ_i為重心繞邊線到最高點(diǎn)的旋轉(zhuǎn)角度；

最后把解析出的相關(guān)參數(shù)代入到能量裕度函數(shù)中，求得機(jī)器人穩(wěn)定裕度值，用于穩(wěn)定性判定，保證機(jī)器人穩(wěn)定工作。

本發(fā)明的有益效果主要表現(xiàn)在：

1、本發(fā)明考慮了機(jī)器人穩(wěn)定行走的動(dòng)態(tài)影響因素，包括角速度、力矩等。并在求解重心位置的能量裕度基礎(chǔ)上，考慮機(jī)器人自身的彈簧阻尼特性，此方法能最大程度的計(jì)算出機(jī)器人能量穩(wěn)定裕度。

2、考慮外力沖擊干擾和阻尼的多足機(jī)器人能量裕度計(jì)算方法，能適用于多足機(jī)器人在復(fù)雜地形上行走時(shí)的動(dòng)態(tài)穩(wěn)定性分析，使機(jī)器人具有一定抵抗外力沖擊的能力。

附圖說(shuō)明

圖1是本發(fā)明的考慮外力沖擊干擾和阻尼的多足機(jī)器人能量裕度計(jì)算方法，所應(yīng)用的六足機(jī)器人結(jié)構(gòu)示意圖。

圖2是六足機(jī)器人繞邊線等效的機(jī)械旋轉(zhuǎn)系統(tǒng)示意圖。

圖3是六足機(jī)器人在斜面行走示意圖。

圖4是六足機(jī)器人的單腿D-H模型。

圖5是本發(fā)明的考慮外力沖擊干擾和阻尼的多足機(jī)器人能量裕度計(jì)算方法的流程圖。

具體實(shí)施方式

下面結(jié)合附圖對(duì)本發(fā)明作進(jìn)一步描述。

參照?qǐng)D1～圖5，一種考慮外力沖擊干擾和阻尼的多足機(jī)器人能量裕度計(jì)算方法，以六足機(jī)器人為例，包括機(jī)器人軀體和六條相互對(duì)獨(dú)立的步行腿，所述計(jì)算方法包括以下步驟：

步驟一：把機(jī)器人的翻轉(zhuǎn)動(dòng)作轉(zhuǎn)換為包含彈簧阻尼特性的機(jī)械旋轉(zhuǎn)系統(tǒng)，計(jì)算機(jī)器人此時(shí)的能量裕度表達(dá)式；

1.1)根據(jù)牛頓第二定律可列出機(jī)器人的動(dòng)力學(xué)微分方程：

其中，將繞邊線翻轉(zhuǎn)時(shí)的機(jī)器人，視為一個(gè)機(jī)械旋轉(zhuǎn)系統(tǒng)，該系統(tǒng)由轉(zhuǎn)動(dòng)慣量為J_i、扭轉(zhuǎn)剛度為K_S和粘性阻尼系數(shù)為C的阻尼器組成。

1.2)求解機(jī)器人繞邊線旋轉(zhuǎn)的實(shí)時(shí)重心處合力矩，即驅(qū)動(dòng)力矩：

式中，M_Gi為實(shí)時(shí)重心處重力相對(duì)旋轉(zhuǎn)邊線產(chǎn)生的力矩；M_I為實(shí)時(shí)重心處的等效慣性力矩；M_ri為著地腳因支撐力產(chǎn)生的力矩；結(jié)合圖3中所示，f_i為各足端豎直向上的力；θ_i為直線AB與水平面夾角，δ_i為重心繞邊線旋轉(zhuǎn)角度；θ為斜線傾角；r_i為各足端到旋轉(zhuǎn)邊線的距離；e_i為旋轉(zhuǎn)邊線的單位法向量，以支撐多邊形逆時(shí)針?lè)较驗(yàn)檎较颉?/p>

1.3)系數(shù)K_S、C測(cè)定方法，根據(jù)上述的機(jī)械旋轉(zhuǎn)系統(tǒng)整理得

兩邊進(jìn)行拉氏變換，求得其傳遞函數(shù)為：

因此求得：

式中，T_C、T_S分別為當(dāng)機(jī)器人受到一個(gè)脈沖力時(shí)，機(jī)械旋轉(zhuǎn)系統(tǒng)的震蕩周期與穩(wěn)定時(shí)間。

1.4)求解機(jī)器人的考慮外力沖擊干擾和阻尼的能量裕度

機(jī)器人繞邊線旋轉(zhuǎn)到最高點(diǎn)的動(dòng)能變化：

當(dāng)w₀為正時(shí)：

當(dāng)w₀為負(fù)時(shí)：

假設(shè)機(jī)器人繞邊線旋轉(zhuǎn)到最高點(diǎn)時(shí)w_i＝0，因此機(jī)器人求得當(dāng)前位置因外界干擾不發(fā)生完全側(cè)翻，所能抵抗的能量為：

E_i＝V_i-V₀+K_i-K₀

求得：

上述公式中，w₀，M_Ri'分別為實(shí)時(shí)重心位置繞邊線旋轉(zhuǎn)的角速度與合力矩，方向延旋轉(zhuǎn)邊線，以支撐多邊形逆時(shí)針?lè)较驗(yàn)檎较颍?/p>

根據(jù)能量穩(wěn)定裕量的概念可知，定義能量穩(wěn)定裕度為SE_m，其值越大，表示機(jī)器人越穩(wěn)定。然而為了確保機(jī)器人在外力沖擊作用下安全可靠的工作，設(shè)最小穩(wěn)定裕度SE，當(dāng)SEm≥SE時(shí)，機(jī)器人能穩(wěn)定工作。

有益于機(jī)器人在行走過(guò)程中，根據(jù)實(shí)時(shí)檢測(cè)的關(guān)節(jié)轉(zhuǎn)角與機(jī)身角速度等有關(guān)變量，計(jì)算當(dāng)前能量裕度值，判定機(jī)器人是否能穩(wěn)定行走。并建立適當(dāng)?shù)淖赃m應(yīng)控制算法，根據(jù)環(huán)境情況和系統(tǒng)狀態(tài)，優(yōu)化關(guān)節(jié)驅(qū)動(dòng)參數(shù)，保證機(jī)器人穩(wěn)定工作。

步驟二：根據(jù)機(jī)器人的運(yùn)動(dòng)學(xué)，建立機(jī)器人在笛卡爾坐標(biāo)系下的D-H數(shù)學(xué)模型。由于機(jī)器人的六條腿相互獨(dú)立，且完全相同。因此根據(jù)機(jī)器人單腿的D-H數(shù)學(xué)模型可推導(dǎo)出整機(jī)的D-H模型。

如圖4所示，首先分別在機(jī)器人單腿各關(guān)節(jié)處，以及機(jī)器人軀體中心，建立笛卡爾坐標(biāo)系。再進(jìn)行兩個(gè)坐標(biāo)之間的齊次變換，以軀體坐標(biāo)到世界坐標(biāo)系之間齊次變換為例：

上式為坐標(biāo)系OC到ΣO旋轉(zhuǎn)變換，其中α、β、γ分別表示機(jī)器人在世界坐標(biāo)系O下的機(jī)身姿態(tài),且cα＝cosα，sα＝sinα，cβ,sβ,cγ,sγ依次類推。上述公式中的角度，分別由陀螺儀、關(guān)節(jié)舵機(jī)直接反饋得出。

^OP_C表示坐標(biāo)系O_C原點(diǎn)在O中相對(duì)位置。

上式為坐標(biāo)系OC到O的平移變換

求得兩坐標(biāo)之間的齊次變換:

當(dāng)空間有n個(gè)坐標(biāo)系時(shí)，若已知相鄰坐標(biāo)系之間的齊次變換矩陣，則：

M_0n＝M₀₁·M₀₂·…M_n-1n

其中M_ij為相鄰坐標(biāo)系的齊次變換，因此求出機(jī)器人各部分在坐標(biāo)系O中的位置；

步驟三：本發(fā)明的考慮外力沖擊干擾和阻尼的多足機(jī)器人能量裕度計(jì)算方法，與機(jī)器人的實(shí)時(shí)重心相關(guān)，為了計(jì)算方便，假設(shè)各部件均為勻質(zhì)材料，即重心均在其幾何中心處，由此得出在其相應(yīng)的笛卡爾坐標(biāo)系中的位置，如圖4所示；

3.1)通過(guò)步驟二中求出的齊次變換矩陣，得出各部件重心在世界坐標(biāo)系位置。

式中，表示軀體重心在坐標(biāo)系O_C中的位置

同理求得各部件重心在O的位置。代入以下公式求得機(jī)器人實(shí)時(shí)重心：

其中，W為機(jī)器人的總量，單位kg；n為機(jī)器人的足數(shù)，x_o,y_o,z_o為機(jī)身重心在坐標(biāo)系O中位置，x_j,y_j,z_j為各部件重心在O中位置坐標(biāo)

3.2)結(jié)合圖4求解實(shí)時(shí)重心繞支撐邊線翻轉(zhuǎn)時(shí)到最高位置時(shí)的豎直位移S_NEi。

首先假設(shè)構(gòu)成傾翻邊界的兩足端位置坐標(biāo)：

A(x_A,y_A,z_A)，B(x_B,y_B,z_B)

求直線AB的表達(dá)式：

再次求重心CG到直線AB的距離為：

最后，通過(guò)直線與面的幾何關(guān)系，求重心繞AB旋轉(zhuǎn)到最高點(diǎn)的豎直位移：

S_NEi＝ECG'cosθ_i＝H_i(1-cosδ_i)cosθ_i

式中，H_i為實(shí)時(shí)重心到支撐邊線的垂直距離；CG’為CG繞直線AB旋轉(zhuǎn)所能達(dá)到的最高高度點(diǎn)，δ_i為重心繞邊線的旋轉(zhuǎn)角度；θ_i為直線AB與水平面的夾角。最后把解析出的相關(guān)參數(shù)代入到能量裕度函數(shù)中，求得機(jī)器人穩(wěn)定裕度值，用于穩(wěn)定性判定，保證機(jī)器人穩(wěn)定工作。