基于電流的機(jī)器人故障檢測與安全保護(hù)研究*

宋吉來，徐　方，鄒風(fēng)山

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

?

基于電流的機(jī)器人故障檢測與安全保護(hù)研究*

宋吉來1,2,3，徐方1,2,3，鄒風(fēng)山3

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

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

關(guān)鍵詞：故障檢測；安全保護(hù)；機(jī)器人控制；動(dòng)力學(xué)控制

0引言

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

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

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

1機(jī)器人構(gòu)型

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

①每個(gè)方向的運(yùn)動(dòng)由一個(gè)電機(jī)驅(qū)動(dòng)；

②Z軸采用絲杠傳動(dòng)，為升降軸；

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

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

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

2運(yùn)動(dòng)學(xué)與動(dòng)力學(xué)模型

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

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

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

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

(1)

運(yùn)動(dòng)學(xué)逆解為：

(2)

2.2動(dòng)力學(xué)模型

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

(3)

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

(4)

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

L3=L4

(5)

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

(6)

(7)

(8)

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

3機(jī)器人的力/力矩檢測

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

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

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

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

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

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

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

磁鏈方程：

(9)

電壓方程：

(10)

轉(zhuǎn)矩方程：

(11)

這時(shí)的電流矢量隨負(fù)載狀態(tài)的變化，在q軸上移動(dòng)。

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

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

q=N-1qm

(12)

τ=Nτm

(13)

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

重寫完整動(dòng)力學(xué)方程為以下形式：

(14)

考慮電機(jī)轉(zhuǎn)子動(dòng)力學(xué)，可以得到轉(zhuǎn)子的轉(zhuǎn)矩平衡方程為：

(15)

(16)

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

(17)

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

(18)

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

(19)

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

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

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

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

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

5結(jié)論

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

[參考文獻(xiàn)]

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

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

[3] 劉志剛,趙曉燕.淺談數(shù)控機(jī)床機(jī)械故障診斷的方法[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

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

作者簡介：宋吉來(1983—)，男，遼寧營口人，中國科學(xué)院大學(xué)博士，研究方向?yàn)闄C(jī)器人運(yùn)動(dòng)控制與力控制，(E-mail)songjilai@siasun.com。

中圖分類號：TH16；TG659

文獻(xiàn)標(biāo)識碼：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