基于電流的機器人故障檢測與安全保護研究*

宋吉來，徐　方，鄒風山

(1.中國科學院 沈陽自動化研究所，沈陽　110016；2.中國科學院大學，北京　100049；3.沈陽新松機器人自動化股份有限公司，沈陽　110068)

?

基于電流的機器人故障檢測與安全保護研究*

宋吉來1,2,3，徐方1,2,3，鄒風山3

(1.中國科學院 沈陽自動化研究所，沈陽110016；2.中國科學院大學，北京100049；3.沈陽新松機器人自動化股份有限公司，沈陽110068)

摘要：提出一種基于電流的機器人故障檢測與安全保護方法。當機器人運行中發(fā)生齒輪、抱閘等故障或與外界環(huán)境發(fā)生碰撞時，電機電流會發(fā)生突變，因此可以根據(jù)這種異常變化判斷故障和碰撞的發(fā)生。通過直接檢測電機電流，利用電機模型與機器人動力學模型，估計外作用力，對故障或碰撞的發(fā)生間接測量，實現(xiàn)保護功能。結(jié)合動力學控制、關(guān)節(jié)轉(zhuǎn)矩控制，并采用硅片傳輸機器人進行碰撞保護實驗，實際運行結(jié)果驗證了該方法的有效性。

關(guān)鍵詞：故障檢測；安全保護；機器人控制；動力學控制

0引言

隨著科學技術(shù)的發(fā)展，機器人、數(shù)控機床等設(shè)備越來越多的應(yīng)用在生產(chǎn)、加工、制造等領(lǐng)域。在這些應(yīng)用中，故障檢測與安全保護已然成為這一領(lǐng)域研究的主要問題[1-2]。當機器人與人或其它設(shè)備協(xié)同工作時，一次偶然的碰撞會影響整個生產(chǎn)線的效率，甚至對機器人及周圍環(huán)境設(shè)備(特別是人)造成很大的傷害，避免這樣的碰撞發(fā)生或及時判斷出發(fā)生碰撞對于機器人、數(shù)控設(shè)備或人的保護是非常必要的。

根據(jù)傳感方式的不同，多種策略被應(yīng)用于機器人和數(shù)控設(shè)備的故障診斷與安全保護控制[3]。一種策略是采用六維力/力矩傳感器，其安裝在機器人腕部，通過假設(shè)末端執(zhí)行器為剛性連接，作用在機器人末端的外力可以被計算出來。然而，這種高靈敏度的力/力矩傳感器價格比較高，常應(yīng)用于高精度力控制場合，如高精度裝配及拋光等領(lǐng)域，限制了其應(yīng)用范圍。第二種策略是采用關(guān)節(jié)轉(zhuǎn)矩傳感器，其安裝在機器人關(guān)節(jié)處[4]。如果關(guān)節(jié)轉(zhuǎn)矩為τ，那么末端執(zhí)行器所受外力可以通過τ=JTF計算得到，其中J為機器人的雅可比矩陣。這種方法對機器人的安全控制是很方便的，但傳感器的安裝增加了關(guān)節(jié)的柔性，加大了建模與控制的難度，甚至可能引起振動。第三種方法是結(jié)合電機模型與控制，采用狀態(tài)觀測器或擾動觀測器。Sun和Mills描述了一種采用轉(zhuǎn)矩控制模型的轉(zhuǎn)矩觀測器，其基于電機電流和位置信息[5]。Ohnishi設(shè)計了一種擾動觀測器，以補償電機的擾動轉(zhuǎn)矩[6]。同時，基于擾動觀測器和摩擦模型[7-8]，只需電機電流和位置信息可以得到機器人與環(huán)境作用的反應(yīng)轉(zhuǎn)矩，而不需要額外的力/力矩傳感器。

當機器人運行中發(fā)生齒輪抱閘等故障或與外界環(huán)境發(fā)生碰撞時，電機電流會異常變化。本文提出一種基于電流的機器人故障診斷與安全保護方法，直接檢測電機電流，利用電機模型與機器人動力學模型，估計外作用力，對故障或碰撞的發(fā)生間接測量，實現(xiàn)保護功能。將電機模型引入到力/位混合控制，結(jié)合動力學控制、關(guān)節(jié)轉(zhuǎn)矩控制，在硅片傳輸機器人上進行碰撞保護實驗。基于電機電流的機器人故障檢測與安全保護，不需要額外的傳感器，成本低廉，不降低機器人自身剛性，具有廣泛的適用性。

1機器人構(gòu)型

圖1是典型SCARA結(jié)構(gòu)的硅片傳輸機器人外形圖。這種結(jié)構(gòu)的機器人被廣泛應(yīng)用于潔凈、真空環(huán)境下的半導體生產(chǎn)工業(yè)中。其結(jié)構(gòu)特點包括：

①每個方向的運動由一個電機驅(qū)動；

②Z軸采用絲杠傳動，為升降軸；

③SCARA類型手臂具有兩個連桿、三個旋轉(zhuǎn)關(guān)節(jié)、三組皮帶和帶輪。

圖1　SCARA結(jié)構(gòu)硅片傳輸機器人

圖1中，機器人的運動包括三個自由度，升降、旋轉(zhuǎn)和伸縮，運動空間為圓柱形空間。升降和旋轉(zhuǎn)分別由單電機驅(qū)動。旋轉(zhuǎn)運動為R軸方向，由單電機驅(qū)動。沿半徑方向的伸縮運動為S軸方向，由于關(guān)節(jié)、皮帶和帶輪的連接關(guān)系，S軸的伸縮也由一個電機驅(qū)動，關(guān)節(jié)之間的耦合關(guān)系，使得機器人伸縮時末端工具始終保持沿S軸方向的直線運動。

2運動學與動力學模型

2.1運動學模型

硅片傳輸機器人的連桿參數(shù)如表1。

表1　硅片傳輸機器人連桿參數(shù)

根據(jù)DH法[4]以及連桿參數(shù)和角度關(guān)系，得到運動學正解為：

(1)

運動學逆解為：

(2)

2.2動力學模型

SCARA構(gòu)型硅片傳輸機器人的動力學計算采用牛頓-歐拉法[4]，其包括計算速度和加速度的向外迭代以及計算力和力矩的向內(nèi)迭代兩部分。牛頓-歐拉方程的遞推形式為：

(3)

SCARA構(gòu)型手臂滿足角度耦合關(guān)系：

(4)

以及連桿長度關(guān)系：

L3=L4

(5)

應(yīng)用連桿參數(shù)和角度的耦合關(guān)系簡化動力學解，可以得到機器人三個驅(qū)動軸動力學解為：

(6)

(7)

(8)

其中，m是各連桿質(zhì)量的總和。mi為第i個連桿的質(zhì)量，L為第3,4連桿的長度，Lmi為第i個連桿的質(zhì)心位置。

3機器人的力/力矩檢測

3.1電機轉(zhuǎn)矩模型

在研究永磁同步電機數(shù)學模型時，常作如下假設(shè)[9]：

(1)忽略空間諧波，設(shè)三相繞組對稱，氣隙磁通按正弦分布；

(2)忽略磁路飽和，各繞組自感和互感恒定；

(3)忽略鐵心、磁滯等損耗；

(4)不考慮頻率、溫度變化對繞組電阻影響。

永磁同步電機動態(tài)方程由磁鏈方程、電壓方程和轉(zhuǎn)矩方程組成。為分析及控制簡單，通常采用忽略鐵損時的d、q軸數(shù)學模型。保持d軸電流為0的id=0控制時，永磁同步電機的數(shù)學模型簡化為如下方程：

磁鏈方程：

(9)

電壓方程：

(10)

轉(zhuǎn)矩方程：

(11)

這時的電流矢量隨負載狀態(tài)的變化，在q軸上移動。

3.2基于模型的力/力矩檢測

當機器人采用減速器傳動時，關(guān)節(jié)角度和轉(zhuǎn)矩滿足：

q=N-1qm

(12)

τ=Nτm

(13)

式中，N∈Rn×n是每個關(guān)節(jié)減速比的對角陣，q和qm分別為關(guān)節(jié)角度和電機角度，τ和τm分別為關(guān)節(jié)轉(zhuǎn)矩和電機轉(zhuǎn)矩。

重寫完整動力學方程為以下形式：

(14)

考慮電機轉(zhuǎn)子動力學，可以得到轉(zhuǎn)子的轉(zhuǎn)矩平衡方程為：

(15)

(16)

由此可得作用在關(guān)節(jié)上的外部轉(zhuǎn)矩τext：

(17)

代入電機轉(zhuǎn)矩模型和關(guān)節(jié)轉(zhuǎn)矩關(guān)系，得到：

(18)

代入雅可比矩陣的力傳遞關(guān)系，得到基于模型的機器人末端6×1維廣義外力Fext為：

(19)

機器人在運行中與外界環(huán)境發(fā)生碰撞或者出現(xiàn)齒輪、抱閘等故障時，電機電流會異常變化，基于模型的力/力矩檢測值τext、Fext也會突變，當τext、Fext大于限定值時則判定故障的發(fā)生。此時可以通過動力下電等方式，實現(xiàn)安全保護功能。

4實驗

采用基于模型的力/力矩檢測方法，在硅片傳輸機器人上進行碰撞實驗。圖2為機器人在碰撞過程中的電機轉(zhuǎn)矩曲線，可以看出，碰撞發(fā)生時電機轉(zhuǎn)矩迅速增加，最大變化量約0.4N·m，可以在電機轉(zhuǎn)矩達到最大值前檢測到碰撞并實施有效保護。圖3為采用模型的力/力矩檢測進行碰撞判斷，并在碰撞發(fā)生時動力下電實施安全保護的電機轉(zhuǎn)矩曲線?？梢钥闯?，電機轉(zhuǎn)矩在10ms左右增加約0.1N·m時，便可進行有效的碰撞檢測并進行保護，轉(zhuǎn)換到機器人末端的力保護精度約為0.1N。傳統(tǒng)的采用六維力傳感器的方式，傳感器檢測的精度一般為滿量程的百分之一，以傳感器量程10N為例，末端的力保護精度約為0.1N。對比可見，基于模型的力/力矩檢測方法在檢測和保護精度上與采用六維力傳感器的方式相當，檢測時間在10ms左右也滿足系統(tǒng)需求，但其成本低廉，只采用機器人本身的電流和位置傳感器，且不會因傳感器的安裝降低機器人自身的剛性，具有一定的優(yōu)越性。

圖2　碰撞過程電機轉(zhuǎn)矩曲線圖

圖3　碰撞保護電機轉(zhuǎn)矩曲線圖

5結(jié)論

本文提出一種基于電機電流的機器人故障檢測與安全保護方法，該方法不需要使用額外的傳感器，僅根據(jù)電機電流以及系統(tǒng)模型來進行碰撞檢測。將電機轉(zhuǎn)矩模型和機器人動力學模型結(jié)合得到外力檢測的力/力矩模型。最后采用硅片傳輸機器人進行碰撞保護實驗，實驗結(jié)果表明該方法與傳統(tǒng)的采用六維力傳感器的方式檢測精度相當，可以在10ms量級的時間內(nèi)判斷出系統(tǒng)的異常外作用力，能夠及時檢測系統(tǒng)故障并保障機器人與設(shè)備的安全。

[參考文獻]

[1]MazzocchiT,DiodatoA,CiutiG,etal.Smartsensorizedpolymericskinforsaferobotcollisionandenvironmentalinteraction[C].IntelligentRobotsandSystems(IROS),2015IEEE/RSJInternationalConferenceon.IEEE,2015:837-843.

[2] 李夢群,馬維金,王愛玲,等.現(xiàn)代數(shù)控機床故障診斷及維修[M].北京:國防工業(yè)出版社,2009.

[3] 劉志剛,趙曉燕.淺談數(shù)控機床機械故障診斷的方法[J].裝備制造技術(shù),2007(4):67-68.

[4]PhongLD,ChoiJ,KangS.Externalforceestimationusingjointtorquesensorsforarobotmanipulator.RoboticsandAutomation(ICRA),2012IEEEInternationalConferenceon.IEEE,2012:4507-4512.

[5]SunD,MillsJK.Developmentofpartialmodel-basedtorquecontrolofACinductionmotors.IEEETransactionsonRobotandAutomation,2001,17(1):100-107.

[6]OhnishiK,ShibataM,MurakamiT.Motioncontrolforadvancedmechatronics.IEEE/ASMETransactionsonMechatronics,1996,1(1):56-67.

[7]MurakamiT,YuF,OhnishiK.Torquesensorlesscontrolinmultidegree-of-freedommanipulator.IEEETransactionsonInducedElectronics.1993,40(2):259-265.

[8]O’SullivanTM,BinghamCM,SchofieldN.Observer-basedtuningoftwo-inertiaservo-drivesystemswithintegratedSAWtorquetransducers.IEEETransactionsonInducedElectronics.2007,54(2):108-109.

[9]ZhangY,ZhuJ.Directtorquecontrolofpermanentmagnetsynchronousmotorwithreducedtorquerippleandcommutationfrequency.PowerElectronics,IEEETransactionson,2011,26(1):235-248.

(編輯李秀敏)

文章編號：1001-2265(2016)07-0056-04

DOI:10.13462/j.cnki.mmtamt.2016.07.016

收稿日期：2016-03-08;修回日期：2016-03-18

*基金項目：國家科技重大專項：硅片集成傳輸系統(tǒng)研發(fā)和示范應(yīng)用(2014ZX02103005)

作者簡介：宋吉來(1983—)，男，遼寧營口人，中國科學院大學博士，研究方向為機器人運動控制與力控制，(E-mail)songjilai@siasun.com。

中圖分類號：TH16；TG659

文獻標識碼：A

Robot Fault Detection and Safety Protection Based on Current

SONG Ji-lai1,2,3,XU Fang1,2,3,ZOU Feng-shan3

(1.ShenyangInstituteofAutomation,ChineseAcademyofSciences,Shenyang110016,China;2.UniversityofChineseAcademyofSciences,Beijing110049,China)

Abstract：An approach of robot fault detection and safety protection based on current is presented. When the robot operation, the occurrence of gear,holding brake fault or collision with the external environment,the motor current will change suddenly,so this abnormal changes can be used to determine fault and collision occurs.By directly detecting the motor current,followed by use of the motor model and the robot dynamics model to estimate external force,and indirect measuring fault or collision,collision protection can be achieved.Finally,combined with dynamics control and joint control torque,collision protection experiments are carried out on clean-room robot,and actual operating results demonstrate the effectiveness of this method.

Key words：fault detection;safety protection;robot control;dynamic control