[發明專利]一種工業機器人軌跡跟蹤控制算法

在審

• 申請號: 202010439921.3
• 申請日: 2020-05-22
• 公開（公告）號: CN111673742A
• 公開（公告）日: 2020-09-18
• 發明（設計）人: 鄭雪芳
• 申請（專利權）人: 江蘇信息職業技術學院
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 南京天翼專利代理有限責任公司 32112
• 代理人: 任志艷

1.一種工業機器人軌跡跟蹤控制算法，其特征在于，采用了工業機器人非奇異終端滑模軌跡跟蹤控制算法，包括如下步驟：

步驟1：利用拉格朗日方程獲得SCARA四軸工業機器人的動力學模型：

式中，q、與分別代表4×1的關節變量、關節變量速度和關節變量加速度向量；M是4×1的慣性矩陣；C是4×4的科氏力與離心力矩陣；τ是4×1的關節力矩；τ_d是4×1的包含了關節負載、外界擾動、摩擦力矩與未建模特性的干擾力矩；

步驟2:設計快速非奇異終端滑模控制律：

其中滑模面s為：

滑膜面導數為；

式中，e＝q_r-q，是SCARA機器人在關節空間內的關節變量跟蹤誤差及其一階導數、二階導數；q_r、與是參考關節變量及其一階導數，β、k₁與k₂分別為待調節的控制增益矩陣，各指數系數分別滿足1＜γ＜2，0＜p＜1的條件；用飽和函數sat替代滑模控制中常見的分段函數sign；

步驟3：在Matlab/xPC的環境中對該控制策略的有效性進行試驗驗證。

2.根據權利要求1所述的一種工業機器人軌跡跟蹤控制算法，其特征在于，在步驟2中使用-Windup技術來抑制機器人系統的Windup現象，機器人系統最終設計的控制律為：

τ＝τ_a+K_wsign(τ_a) (5)

式中，τ_a為控制律經Anti-Windup技術與一階慣性濾波器1/(Ts+1)后的控制力矩，K_w為補償增益矩陣。

3.根據權利要求1或2所述的一種工業機器人軌跡跟蹤控制算法，其特征在于，在步驟2中還設計了優化末端執行器位姿參考軌跡，通過采用布谷鳥算法獲得機器人的最優關節變量軌跡；

假設SCARA機器人的各關節位姿矩陣為X，則其正向運動學方程可被描述為：

X＝f(q) (6)

對上式求關于時間的導數，可得機器人的微分運動學方程為：

式中，為線速度與角速度組合的速度矩陣，為雅克比矩陣；

根據式(7)可反解出關節變量速度，即：

式中，為雅克比矩陣的廣義逆；上式的解可使關節速度的模取得局部最小值；

進而，采用閉環偽逆法來獲取關節變量：

這樣對上式積分便可獲得關節變量，即通過給定路徑點的位姿矩陣X_r來計算出參考關節變量q_r。

4.根據權利要求3所述的一種工業機器人軌跡跟蹤控制算法，其特征在于，布谷鳥算法優化關節變量軌跡的具體步驟為：

設機器人末端執行器的當前位置為P_c＝(x_c,y_c,z_c)，目標位置為P_f＝(x_f,y_f,z_f)，則其位置誤差P_e為：

進而，選取如下約束條件使得各關節變量在時域內的變化最小：

f₁＝max{[q(t+1)-q(t)]²} (11)

式中，t為時間步長，f₁為約束條件；

因此，SCARA機器人軌跡規劃的目標函數為：

F＝αP_e+(1-α)f₁ (12)

式中，α為權重因子。

5.根據權利要求1所述的一種工業機器人軌跡跟蹤控制算法，其特征在于，所述的步驟1動力學模型中的維數4，推廣到任意維數都可行。