[發(fā)明專利]一種快速終端滑模阻抗控制算法

有效

• 申請?zhí)枺?/td>: 202010019659.7
• 申請日: 2020-01-08
• 公開（公告）號: CN111208730B
• 公開（公告）日: 2021-06-22
• 發(fā)明（設(shè)計(jì)）人: 李春泉;何永華;楊峰
• 申請（專利權(quán)）人: 南昌大學(xué)
• 主分類號: G05B13/04
• 分類號: G05B13/04
• 代理公司: 北京眾合誠成知識產(chǎn)權(quán)代理有限公司 11246
• 代理人: 袁紅梅

1.一種快速終端滑模阻抗控制算法，其特征在于：按以下步驟：

步驟1、將力反饋設(shè)備簡化為雙連桿模型，力反饋設(shè)備具有N個(gè)可旋轉(zhuǎn)的關(guān)節(jié)J₁、J₂…J_N，N≤7，關(guān)節(jié)J₁控制的是設(shè)備末端在水平面的運(yùn)動，控制算法所需要補(bǔ)償?shù)闹亓σ约澳┒嗽诠ぷ骺臻g中的運(yùn)動主要來自關(guān)節(jié)J₂…J_N，將力反饋設(shè)備簡化為雙連桿模型，并設(shè)定連桿為均勻質(zhì)桿，簡化后的雙連桿模型關(guān)節(jié)1記作J′₁，關(guān)節(jié)2記作J′₂；其中，J′₁由原力反饋設(shè)備的關(guān)節(jié)J₂…J_N-1合并而來，J′₂是原力反饋設(shè)備的關(guān)節(jié)J_N，并將原力反饋設(shè)備的關(guān)節(jié)J₁省去；

步驟2、求解力反饋設(shè)備模型的運(yùn)動學(xué)，具體求解的算式如下：

(a)正運(yùn)動學(xué)求解算式：

式中，l₁和l₂分別是簡化后模型關(guān)節(jié)J′₁的連桿長和關(guān)節(jié)J′₂的連桿長，θ₁是l₁與水平面間的夾角，θ₂是l₂和豎直平面之間的夾角，(x₁，x₂)是設(shè)備末端在二維平面上的位置坐標(biāo)；

(b)逆運(yùn)動學(xué)求解算式：

式中，r是設(shè)備末端到關(guān)節(jié)J′₁之間的距離；

式中，β是r與水平面的夾角；

再通過余弦定理得到：

式中，γ是l₁與r之間的夾角；

考慮力反饋設(shè)備的實(shí)際工作空間，γ＞0，因此θ₁的計(jì)算公式為：

θ₁＝γ+β

通過余弦定理，得到角α的計(jì)算公式：

α為桿長l₁和l₂之間的夾角

得到θ₂的計(jì)算公式：

定義x＝[x₁ x₂]，θ＝[θ₁ θ₂]，得雅可比矩陣公式：

將正運(yùn)動學(xué)公式帶入上式，可得：

雅可比矩陣的微分為：

步驟3、求解力反饋設(shè)備模型的動力學(xué)，具體求解的算式如下：連桿l₁動能為：

連桿l₂質(zhì)心C在x₁上的線速度為：

連桿l₂質(zhì)心C在x₂上的線速度為：

可得：

連桿l₂的動能：

系統(tǒng)的總動能為：

系統(tǒng)的總勢能：

將系統(tǒng)的總動能和系統(tǒng)的總勢能代入拉格朗日函數(shù)中，得到：

關(guān)節(jié)力矩的計(jì)算

簡化模型的關(guān)節(jié)J′₁的驅(qū)動力矩計(jì)算公式如下：

式中：

得到力矩τ₁的計(jì)算公式：

簡化模型的關(guān)節(jié)J′₂的驅(qū)動力矩計(jì)算公式如下：

式中：

得到力矩τ₂的計(jì)算公式：

整理力矩τ₁和τ₂的計(jì)算公式，最終得到力矩的表達(dá)式為：

步驟4、建立阻抗模型，力反饋設(shè)備末端的接觸力為Fe，將Fe與理想位置誤差x_c-x_d建立阻抗關(guān)系，描述為：

其中，x_c為末端位置的指令軌跡，x_d為理想的阻抗軌跡，x(0)＝x_c(0)，M_d、B_d、K_d為阻抗參數(shù)，分別是質(zhì)量、阻尼和剛度系數(shù)矩陣，通過上述的阻抗關(guān)系公式計(jì)算出需要跟蹤的理想阻抗軌跡，再根據(jù)動力學(xué)模型，設(shè)計(jì)力反饋設(shè)備末端的控制律F_x；

步驟5、設(shè)計(jì)快速終端滑模控制器，定義快速終端滑模切換面：

其中，x_d(t)為理想軌跡，和分別為理想速度和加速度，γ₁和γ₂為大于零的正定常數(shù)矩陣，α和β為正奇數(shù)，且β大于α；

定義中間參考變量：

對上式微分，可得：

則有：

將中間參考變量的微分式代入上式中，可得：

設(shè)計(jì)快速終端滑模控制器為：

其中，K＞0，ε＞0，tanh為雙曲正切函數(shù)。