鄒風(fēng)山，趙 彬

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

基于電流的力控制技術(shù)與安全保護(hù)研究*

鄒風(fēng)山1,2，趙 彬1,3

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

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

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

0 引言

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

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

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

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

1 七自由度機(jī)器人機(jī)械結(jié)構(gòu)

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

圖1 七自由度機(jī)器人機(jī)械構(gòu)型

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

2 機(jī)器人運(yùn)動(dòng)學(xué)建模

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

2.1 正運(yùn)動(dòng)學(xué)建模

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

(1)

(2)

2.2 逆運(yùn)動(dòng)學(xué)建模

建立如圖2所示的機(jī)器人連桿坐標(biāo)系框圖，進(jìn)而求解逆運(yùn)動(dòng)學(xué)方程。

圖2 七自由度機(jī)器人連桿坐標(biāo)系框圖

2.2.1 求解第四軸θ4

這里，先引入肘部關(guān)節(jié)翻轉(zhuǎn)與否標(biāo)識(shí)elbow；

推導(dǎo)出：

(3)

θ4求解完畢。

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

2.2.2 求解第一軸θ1

圖3 三層前向神經(jīng)網(wǎng)絡(luò)結(jié)構(gòu)

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

(4)

如圖所示，θ1=-α；

(5)

θ1求解完畢。

2.2.3 求解第二軸θ2

如圖所示，因?yàn)棣?=-γ，顯然可以得到公式如下：

(6)

θ2求解完畢。

2.2.4 求解第三軸θ3

已知：

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

設(shè)常量：

利用arctan2可以解出唯一解，

(7)

θ3求解完畢。

2.2.5 求解腕關(guān)節(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為任意解。在這個(gè)情況下，所以結(jié)果都是θ5和θ7的和或者差。

求解θ6：

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

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

(10)

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

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

(11)

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

3 工業(yè)機(jī)器人動(dòng)力學(xué)建模

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

3.1 機(jī)械參數(shù)

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

(12)

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

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

3.2 牛頓-歐拉動(dòng)力學(xué)

牛頓-歐拉外推法

外推：i：0—6

(13)

(14)

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

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

(15)

4 機(jī)器人力控制

4.1 基于電流的力控制

完整的機(jī)器人動(dòng)力學(xué)方程為：

(16)

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

M∈Rn×n：關(guān)節(jié)空間慣性矩陣(對(duì)稱(chēng)，正定)；

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

G∈Rn×1：重力項(xiàng)向量；

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

τ∈Rn×1：廣義驅(qū)動(dòng)轉(zhuǎn)矩向量；

τ=Nτm

(17)

并考慮電機(jī)角度與關(guān)節(jié)角度傳遞關(guān)系，得到計(jì)及電機(jī)轉(zhuǎn)子動(dòng)力學(xué)的完整動(dòng)力學(xué)模型為：

(18)

(19)

在機(jī)器人主動(dòng)柔順控制的研究中，將基于電流模型的力檢測(cè)與力/位混合控制結(jié)合，設(shè)計(jì)基于模型的新的力/位混合控制結(jié)構(gòu)如圖4所示。

圖4 電流力控制結(jié)構(gòu)圖

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

4.2 實(shí)驗(yàn)

為驗(yàn)證基于電流模型的力檢測(cè)和基于模型的力/位混合控制結(jié)構(gòu)的有效性，采用關(guān)節(jié)一體化七自由度機(jī)器人為控制對(duì)象，進(jìn)行拖曳式示教實(shí)驗(yàn)。

圖5所示，為拖曳式示教中電機(jī)轉(zhuǎn)矩曲線圖?？梢钥闯觯弦肥绞窘虝r(shí)由于手部施加給機(jī)器人的外作用力，電機(jī)轉(zhuǎn)動(dòng)帶動(dòng)機(jī)器人的運(yùn)動(dòng)方向與外力方向一致，電機(jī)轉(zhuǎn)矩比執(zhí)行模式小，此時(shí)電機(jī)轉(zhuǎn)動(dòng)起到助力作用。在正反向運(yùn)動(dòng)的起始階段，電機(jī)電流較大，這是由于起始段機(jī)器人處于靜止?fàn)顟B(tài)，反向的電流起到運(yùn)動(dòng)啟動(dòng)的作用。實(shí)驗(yàn)過(guò)程中機(jī)器人運(yùn)行平穩(wěn)，滿足拖曳式示教的需求。

圖5 拖曳式示教中電機(jī)轉(zhuǎn)矩曲線圖

5 結(jié)論

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

[1] 姜昆鵬，陳敏,丁建軍. 六自由度并聯(lián)機(jī)構(gòu)的控制系統(tǒng)及仿真試驗(yàn)探究[J], 組合機(jī)床與自動(dòng)化加工技術(shù), 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裝配機(jī)器人手臂剛度優(yōu)化[J].組合機(jī)床與自動(dòng)化加工技術(shù), 2016(10): 40-43.

[8] 趙彬. 基于機(jī)器視覺(jué)的工業(yè)機(jī)器人分揀技術(shù)研究[D]. 沈陽(yáng)：沈陽(yáng)工業(yè)大學(xué)，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

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

鄒風(fēng)山(1978—)，男，山東泰安人，沈陽(yáng)新松機(jī)器人中央研究院院長(zhǎng)，博士，研究方向?yàn)闄C(jī)器人控制與應(yīng)用，(E-mail)zoufengshan123@sohu.com。

TH162;TG506

A