基于電流的力控制技術與安全保護研究*

鄒風山，趙 彬

(1. 沈陽新松機器人自動化股份有限公司， 沈陽 110168；2. 中國科學院 沈陽自動化研究所， 沈陽 110016；3. 東北財經(jīng)大學 工商管理學院，遼寧 大連 116025)

基于電流的力控制技術與安全保護研究*

鄒風山1,2，趙 彬1,3

(1. 沈陽新松機器人自動化股份有限公司， 沈陽 110168；2. 中國科學院 沈陽自動化研究所， 沈陽 110016；3. 東北財經(jīng)大學 工商管理學院，遼寧 大連 116025)

文章解決了電機電流進行機器人主動柔順控制的問題，同時對七自由度關節(jié)一體機器人進行拖曳式示教實驗。首先針對關節(jié)一體化機器人進行運動學分析，采用多層前向神經(jīng)網(wǎng)絡解決機器人運動學冗余軸反解問題，將復雜的非線性優(yōu)化問題轉化為簡單的代數(shù)方程組求解問題。然后由于機器人特殊的構型及關節(jié)耦合復雜，采用牛頓—歐拉動力學建模。將永磁同步電機的轉矩模型與機器人動力學模型結合，得到基于電流模型的力檢測。最后實現(xiàn)基于電機電流的機器人主動柔順控制，并在七自由度關節(jié)一體機器人上進行拖曳式示教實驗，實際運行結果驗證了算法的有效性。

電流力控制；神經(jīng)網(wǎng)絡；機器人動力學；拖曳示教

0 引言

隨著機器人技術的興起，為了能夠滿足批量化生產(chǎn)和實際工程的需求，獨立決策、機器人感知和最優(yōu)控制等功能越來越受到國內(nèi)外學者的關注。新一代機器人在提高生產(chǎn)效率的同時，要求具有安全性，能夠在發(fā)生危險時及時保護機器人自身和外部環(huán)境的安全[1]。并且往往具有簡單易用的示教功能，只需用人手把持機器人的“手腕”，直接拖動機器人進行示教工作，機器人內(nèi)部控制系統(tǒng)會自動記錄示教過程的運動軌跡。對工業(yè)機器人力控制的研究主要使用其力矩傳感器來實現(xiàn)，通過ATI力矩傳感器來獲得精確的力矩，再對機器人實現(xiàn)力位混合控制。而在這些新興技術背后是近年來隨著機械、控制、傳感三方面技術的進步，迸發(fā)出嶄新活力的機器人主動柔順控制這一經(jīng)典研究課題。

鑒于此，基于電機電流或轉矩傳感器的機器人力控制屬于關節(jié)轉矩力控制。它通過直接檢測和控制機器人的關節(jié)轉矩來達到控制機器人各關節(jié)動作，最終使機器人末端執(zhí)行器實現(xiàn)各種功能運動和操作任務的目的?？梢哉f，關節(jié)轉矩力控制技術的研發(fā)對新一代工業(yè)機器人、協(xié)作機器人簡易示教與碰撞保護，醫(yī)療康復機器人的主動運動與被動運動，工業(yè)機器人的軟浮動裝配，服務機器人手臂的碰撞保護等領域都具有重要的應用意義。

根據(jù)傳感方式的不同，多種策略被應用于機器人主動柔順控制。對此，國內(nèi)外學者做了比較深入的研究。文獻[2]中Daniel Kruse和John T. Wen將Smith-?str?m預測器應用到了機器人力控制技術中[2]。文獻[3]中Alexander Winkler和Jozef Suchy將末端效應器速度的變化作為力控制誤差的函數(shù)改善了力控制輪廓跟隨算法，可以將這種力控制應用于諸如拋光，去毛刺或磨削的表面精加工任務中[3]。F. M. Kulakov通過使用控制對象的詳細動態(tài)模型分析現(xiàn)有力/轉矩控制方法的結果，解決了彈性機器人力控制理論的泛化問題[4]。H. P. Qian，J. De Schutter通過阻尼和低通濾波在離散時間穩(wěn)定性中的作用實現(xiàn)了機器人的力控制[5]。

以往前人研究的力控制算法都是基于力矩傳感器實現(xiàn)的。雖然效果較好，但是力矩傳感器價格較高，不適用于批量生產(chǎn)。本文為了改善這一缺點，提出了基于電流的力控制算法。首先建立機器人運動學方程，運用神經(jīng)網(wǎng)絡技術對運動學進行訓練學習來更新權值，間接求解冗余軸數(shù)值解，其次構建動力學方程，并利用牛頓歐拉法對動力學進行簡化。再次建立機器人與環(huán)境相互作用的反應轉矩模型。最后在動力學控制、關節(jié)轉矩控制的基礎上，將位置控制與力控制結合，實現(xiàn)主動柔順控制，并進行拖曳式示教驗證，為機器人批量化生產(chǎn)奠定了基礎。

1 七自由度機器人機械結構

如圖1所示，為七自由度機器人機械構型。在機械設計上，有前期開發(fā)的自重30kg、負載5kg的手臂；直流版手臂采用自主研制的一體化關節(jié)；各驅動元件及末端執(zhí)行器的線纜均高度集成于一體化關節(jié)內(nèi)部而非裸露在手臂外側。

圖1 七自由度機器人機械構型

本文在該七自由度機器人關節(jié)轉矩力控制技術可以實現(xiàn)的功能包括：①機器人拖曳式示教；②機器人碰撞保護；③機器人軟浮動技術；④機器人安全柔順控制等。

2 機器人運動學建模

七自由度機械手比以往的普通工業(yè)六軸機械手多了一個自由度，該自由度的加入使得機器人會產(chǎn)生無數(shù)解。冗余軸的加入會給機器人模型推導帶來一定困難，使用以往機械運動學算法難以解決，本文采用神經(jīng)網(wǎng)絡算法，改善了以往運動學算法的不足。并且可以較快的得到最優(yōu)解。為后續(xù)研究奠定了一定基礎。

2.1 正運動學建模

表1 機械臂的連桿DH參數(shù)

(1)

(2)

2.2 逆運動學建模

建立如圖2所示的機器人連桿坐標系框圖，進而求解逆運動學方程。

圖2 七自由度機器人連桿坐標系框圖

2.2.1 求解第四軸θ4

這里，先引入肘部關節(jié)翻轉與否標識elbow；

推導出：

(3)

θ4求解完畢。

鑒于此，采用神經(jīng)網(wǎng)絡算法，可以不需要考慮機器人無數(shù)解的情況，采用最優(yōu)解來得到運動學算法。

2.2.2 求解第一軸θ1

圖3 三層前向神經(jīng)網(wǎng)絡結構

將肘部關節(jié)位置代入公式(4)中，

(4)

如圖所示，θ1=-α；

(5)

θ1求解完畢。

2.2.3 求解第二軸θ2

如圖所示，因為θ2=-γ，顯然可以得到公式如下：

(6)

θ2求解完畢。

2.2.4 求解第三軸θ3

已知：

兩邊元素(2，4)和元素(2，4)：

設常量：

利用arctan2可以解出唯一解，

(7)

θ3求解完畢。

2.2.5 求解腕關節(jié)

上面已經(jīng)求得θ1、θ2、θ3、θ4。

求解θ5：

(1)由于第一行，第二列相等可以得到：

(2)由于第三行，第二列相等可以得到：

(8)

考慮θ5的值受θ6的影響，如果θ6=0，則θ5為任意解。

求解θ7：

(1)由于第二行，第一列相等可以得到：

(2)由于第二行，第三列相等可以得到：

θ7=arctan2(s6s7,s6c7)

(9)

考慮θ7的值受θ6的影響，如果θ6=0，則θ7為任意解。在這個情況下，所以結果都是θ5和θ7的和或者差。

求解θ6：

(1)由于第二行，第三列相等可以得到：

(2)由于第二行，第二列相等可以得到：

(10)

先求θ4、θ6，由于周期性，必須進行跟蹤求解。

由于操作臂關節(jié)(翻轉)可以得到2組解：

(11)

θ4、θ5、θ6求解完畢。

3 工業(yè)機器人動力學建模

基于電流的力控制技術首先要解決的就是動力學建模[7-9]。本次開發(fā)的動力學模型是在加入EtherCAT的基礎上，RC每4ms周期計算1次動力學結果，發(fā)送給力控制補償器，力控制補償器根據(jù)其計算結果來控制位置指令輸出給驅動器。

3.1 機械參數(shù)

七自由度工業(yè)機器人的理論基本機械參數(shù)由軟件計算得到。慣性張量可以用3×3矩陣表示如下：

(12)

表2為七自由度關節(jié)一體化機器人的慣性張量參數(shù)。

表2 機器人的機械參數(shù)

3.2 牛頓-歐拉動力學

牛頓-歐拉外推法

外推：i：0—6

(13)

(14)

牛頓-歐拉內(nèi)推法

內(nèi)推：i：7—1

(15)

4 機器人力控制

4.1 基于電流的力控制

完整的機器人動力學方程為：

(16)

方程(16)代表以時間t為非獨立變量的系統(tǒng)非線性微分方程。方程的左邊各項含義為(n為機器人的關節(jié)數(shù)也是自由度數(shù))：

M∈Rn×n：關節(jié)空間慣性矩陣(對稱，正定)；

C∈Rn×n：哥氏力和向心力計算矩陣；

G∈Rn×1：重力項向量；

q∈Rn×1：廣義關節(jié)偏差向量；

τ∈Rn×1：廣義驅動轉矩向量；

τ=Nτm

(17)

并考慮電機角度與關節(jié)角度傳遞關系，得到計及電機轉子動力學的完整動力學模型為：

(18)

(19)

在機器人主動柔順控制的研究中，將基于電流模型的力檢測與力/位混合控制結合，設計基于模型的新的力/位混合控制結構如圖4所示。

圖4 電流力控制結構圖

圖4中關節(jié)轉矩的傳感采用電機電流，外部轉矩τext通過基于電流模型的力檢測獲得，并反饋給力閉環(huán)進行柔順控制。每個關節(jié)既有位置控制器，又有力控制器。為了根據(jù)約束條件改變每個自由度所要求的控制模式，圖中引入了選擇矩陣S和S′，其為3×3矩陣，是兩組互鎖開關。如要求第i個關節(jié)進行位置(或力)控制則矩陣S(或S′)對角線上的第i個元素為1，否則為0。

4.2 實驗

為驗證基于電流模型的力檢測和基于模型的力/位混合控制結構的有效性，采用關節(jié)一體化七自由度機器人為控制對象，進行拖曳式示教實驗。

圖5所示，為拖曳式示教中電機轉矩曲線圖?？梢钥闯?，拖曳式示教時由于手部施加給機器人的外作用力，電機轉動帶動機器人的運動方向與外力方向一致，電機轉矩比執(zhí)行模式小，此時電機轉動起到助力作用。在正反向運動的起始階段，電機電流較大，這是由于起始段機器人處于靜止狀態(tài)，反向的電流起到運動啟動的作用。實驗過程中機器人運行平穩(wěn)，滿足拖曳式示教的需求。

圖5 拖曳式示教中電機轉矩曲線圖

5 結論

本文針對采用力矩傳感器進行力控制的技術方案成本較高的缺點，提出了一種基于電機電流模型的力控制方法。本文采用多層前向神經(jīng)網(wǎng)絡解決機器人運動學冗余軸多解的問題，將復雜的非線性優(yōu)化問題轉化為簡單的代數(shù)方程組求解問題。然后利用牛頓—歐拉簡化計算系統(tǒng)模型。最后進行了拖曳式示教實驗，通過電流力控制方法簡化了主動柔順控制功能。經(jīng)過實驗驗證了該方法的有效性，為后續(xù)機器人算法研究和開拓機器人市場奠定了基礎。

[1] 姜昆鵬，陳敏,丁建軍. 六自由度并聯(lián)機構的控制系統(tǒng)及仿真試驗探究[J], 組合機床與自動化加工技術, 2016(9): 43-46.

[2] Daniel Kruse，John T. Wen. Application of the Smith-?str?m Predictor to robot force control[C]. 2015 IEEE International Conference on Automation Science and Engineering (CASE), 2015: 383-388.

[3] Alexander Winkler，Jozef Suchy. Force controlled contour following on unknown objects with an industrial robot[C]. Robotic and Sensors Environments (ROSE), 2013: 208-213.

[4] F M Kulakov. Generalization of robot force control theory for elastic robot[J]. roceedings of the IEEE/RSJ/GI International Conference on, 1994,11(1): 1610-1617.

[5] H P Qian，J De Schutter. The role of damping and low pass filtering in the stability of discrete time implemented robot force control[J]. Robotics and Automation, 1992，2(11): 1368-1373.

[6] Guivant J, Nebot E, and Baiker S. Autonomous navigation and map building using laser range sensors in outdoor applications[J]. Journal of Robotics Systems, 2000, 17(10) : 565-583.

[7] 史旭東，莊立東，郭顯鵬. 基于PSO的6R裝配機器人手臂剛度優(yōu)化[J].組合機床與自動化加工技術, 2016(10): 40-43.

[8] 趙彬. 基于機器視覺的工業(yè)機器人分揀技術研究[D]. 沈陽：沈陽工業(yè)大學，2013.

[9] Koren Y，Borenstein J. Potential field methods and their inherent limitations for mobile robot navigation[C].In: Proceedings of the 1991 IEEE International Conference on Robotics and Automation. Sacramento，California：IEEE，1991; 1398-1404.

(編輯 李秀敏)

Autonomous Mobile Robot Map Building, Localization and Path Planning

ZOU Feng-shan1,2，ZHAO Bin1,3

(1. SIASUN Robot & Automation Co., Ltd., Shenyang 110168，China; 2. Shenyang Institute of Automation, Chinese Academy of Science, Shenyang 110016, China)

Based on the motor current, the active compliant control of the robot is studied, and the drag-and-drop teaching experiment is carried out for the 7-DOF joint one-body robot. Firstly, the multi-layer feedforward neural network is used to solve the inverse kinematics problem of the robot kinematics, and the complex nonlinear optimization problem is transformed into a simple algebraic equation group for solving the kinematic analysis of the joint-integrated robot. Then, due to the special configuration of the robot and the complicated coupling of the joints, the Newton-Euler dynamics model is adopted. The torque model of the permanent magnet synchronous motor is combined with the robot kinetic model to obtain the force detection based on the current model. Finally, the active compliant control of the robot based on the motor current is realized, and the teaching experiment of the robot is carried out. The actual running result verifies the effectiveness of the method.

current force control; neural networks; robot dynamics; drag teaching

1001-2265(2017)08-0100-05

10.13462/j.cnki.mmtamt.2017.08.025

2016-11-03；

2016-11-22

國家科技重大專項：硅片集成傳輸系統(tǒng)研發(fā)和示范應用(2014ZX02103)；國家高技術研究發(fā)展863計劃(2012AA041405)；沈陽市工業(yè)科技攻關項目(F12-010-2-00)

鄒風山(1978—)，男，山東泰安人，沈陽新松機器人中央研究院院長，博士，研究方向為機器人控制與應用，(E-mail)zoufengshan123@sohu.com。

TH162;TG506

A