[發明專利]考慮外力沖擊干擾和阻尼的多足機器人能量裕度計算方法

有效

• 申請號: 201611138545.4
• 申請日: 2016-12-12
• 公開（公告）號: CN106737669B
• 公開（公告）日: 2019-10-18
• 發明（設計）人: 艾青林;徐冬;簡燕梅
• 申請（專利權）人: 杭州宇芯機器人科技有限公司
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 杭州斯可睿專利事務所有限公司 33241
• 代理人: 王利強

1.一種考慮外力沖擊干擾和阻尼的多足機器人能量裕度計算方法，其特征在于：包括如下步驟：

步驟一：把機器人的翻轉動作轉換為包含彈簧阻尼特性的機械旋轉系統，計算機器人的能量裕度表達式；

步驟二：建立機器人在笛卡爾坐標系下的D-H數學模型；

步驟三：結合步驟二解析機器人能量裕度函數所需有關參數；

所述的步驟一通過以下方式實現：

1.1)根據牛頓第二定律列出機器人的動力學微分方程：

其中，將繞邊線翻轉時的機器人，視為一個機械旋轉系統，該系統由轉動慣量為J_i、扭轉剛度為K_S和粘性阻尼系數為C的阻尼器組成；

1.2)求解機器人繞邊線旋轉的實時重心處合力矩，即驅動力矩：

式中，M_Gi為實時重心處重力相對旋轉邊線產生的力矩；M_I為實時重心處的等效慣性力矩；M_ri為著地腳因支撐力產生的力矩；f_i為各足端豎直向上的力；θ_i為直線AB與水平面夾角，構成傾翻邊界的兩足端位置坐標：A(x_A,y_A,z_A)，B(x_B,y_B,z_B)，直線AB的表達式：

δ_i為重心繞邊線到最高點的旋轉角度；θ為斜線傾角；r_i為各足端到旋轉邊線的距離；e_i為旋轉邊線的單位法向量，以支撐多邊形逆時針方向為正方向；

1.3)系數K_S、C測定，根據機械旋轉系統整理得

兩邊進行拉氏變換，求得其傳遞函數為：

因此求得

式中，T_C、T_S分別為當機器人受到一個脈沖力時，機械旋轉系統的震蕩周期與穩定時間；

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

機器人繞邊線旋轉到最高點的勢能變化：

機器人繞邊線旋轉到最高點的動能變化：

當w₀為正時：

當w₀為負時：

假設機器人繞邊線旋轉到最高點時w_i＝0，因此求得機器人當前位置因外界干擾不發生完全側翻，所能抵抗的能量為：

E_i＝V_i-V₀+K_i-K₀

求得：

上述公式中，w₀、M_Ri'分別為實時重心位置繞邊線旋轉的角速度與合力矩,方向延旋轉邊線，以支撐多邊形逆時針方向為正方向；

根據能量穩定裕量的概念可知，定義能量穩定裕度為SE_m，設最小穩定裕度SE，當SEm≥SE時，機器人能穩定工作。

2.如權利要求1所述的考慮外力沖擊干擾和阻尼的多足機器人能量裕度計算方法：其特征在于：所述的步驟二通過以下方式實現：

在機器人的各關節處與軀體中心，建立笛卡爾坐標系，設機器人坐標系C到世界坐標系O的之間的旋轉變換為線性變換為^OP_C，因此求得機器人坐標系C到世界坐標系O的齊次變換為：

當空間有n個坐標系時，若已知相鄰坐標系之間的齊次變換矩陣，則：

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

其中M_ij為相鄰坐標系的齊次變換，由此建立機器人各部分在世界坐標系O中的位置。

3.如權利要求2所述的考慮外力沖擊干擾和阻尼的多足機器人能量裕度計算方法：其特征在于：所述的步驟三通過以下方式實現：

4.1)重心位置參數，

各部件均為勻質材料，因此其重心均在其幾何中心處，根據步驟二中齊次變換矩陣，求各部件重心在世界坐標系O的位置；

以軀體重心在世界坐標系O中的位置求解：

式中，表示軀體重心在機身坐標系O_C中的位置；

同理求得各部件重心在世界坐標系O的位置，根據各重心位置坐標代入以下公式求得實時重心位置：

其中，W為機器人的總量，單位kg；n為機器人的足數，x_o,y_o,z_o為機身重心在世界坐標系O中位置坐標，x_j,y_j,z_j為各部件重心在世界坐標系O中位置坐標；

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

設構成傾翻邊線的兩足端位置坐標：

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

首先，由幾何關系式，求直線AB的表達式與重心CG到直線AB的距離H_i；

再次，求重心繞AB旋轉到最高點的豎直位移；

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

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

最后把解析出的相關參數代入到能量穩定裕度的公式中，求得機器人穩定裕度值，用于穩定性判定，保證機器人穩定工作。