潔凈機(jī)器人反應(yīng)轉(zhuǎn)矩觀測與碰撞保護(hù)研究

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

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

潔凈機(jī)器人反應(yīng)轉(zhuǎn)矩觀測與碰撞保護(hù)研究

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

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

提出一種基于反應(yīng)轉(zhuǎn)矩觀測器的機(jī)器人碰撞保護(hù)方法；機(jī)器人的反應(yīng)轉(zhuǎn)矩由基于模型的干擾觀測器估值得到，模型的建立包括電機(jī)系統(tǒng)轉(zhuǎn)矩模型與機(jī)器人系統(tǒng)動力學(xué)模型兩部分；由于潔凈機(jī)器人特殊的構(gòu)型及關(guān)節(jié)耦合關(guān)系，機(jī)器人的動力學(xué)建模被大大簡化，同時對簡化后的模型采用最小二乘法對慣性參數(shù)和摩擦參數(shù)進(jìn)行辨識，提高了模型的精度；根據(jù)永磁同步電機(jī)的轉(zhuǎn)矩模型與機(jī)器人動力學(xué)模型，可以得到基于電機(jī)電流的機(jī)器人關(guān)節(jié)轉(zhuǎn)矩；機(jī)器人與外界環(huán)境接觸時，關(guān)節(jié)轉(zhuǎn)矩的增加量即為反應(yīng)轉(zhuǎn)矩；通過設(shè)計反應(yīng)轉(zhuǎn)矩觀測器并采用力/位混合控制結(jié)構(gòu)，實現(xiàn)基于電流的機(jī)器人主動柔順控制功能，并在潔凈機(jī)器人進(jìn)行碰撞保護(hù)實驗，實際運(yùn)行結(jié)果驗證了該方法的有效性。

機(jī)器人控制；機(jī)器人動力學(xué)；碰撞保護(hù)；關(guān)節(jié)轉(zhuǎn)矩控制；系統(tǒng)辨識

0　引言

隨著科技的進(jìn)步與發(fā)展，機(jī)器人的應(yīng)用在廣度和深度兩方面都發(fā)生了較大的變革。新一代機(jī)器人已不只是工作在安全圍欄中，而是能夠與人類在工廠、社區(qū)、家庭、醫(yī)院、酒店、銀行等開放式場所中協(xié)同作業(yè)、和諧共生［1］。在這些與人類接觸的應(yīng)用中，安全性已然成為這一領(lǐng)域研究的主要問題。與其相關(guān)的研究內(nèi)容包括人機(jī)協(xié)同作業(yè)、拖曳式示教、力量輔助、碰撞保護(hù)等。而在這些應(yīng)用背后是近年來隨著機(jī)械、控制、傳感三方面技術(shù)的進(jìn)步，迸發(fā)出嶄新活力的機(jī)器人主動柔順控制這一經(jīng)典研究問題。潔凈機(jī)器人是用于半導(dǎo)體制程設(shè)備中進(jìn)行晶圓傳輸和搬運(yùn)工作的特種應(yīng)用機(jī)器人［2］。其運(yùn)行過程中，如果發(fā)生碰撞將會損壞機(jī)器人自身或晶圓，且影響整個生產(chǎn)線的效率，甚至對工作人員的安全造成損害，因此潔凈機(jī)器人碰撞保護(hù)的研究對于保障機(jī)器人自身和環(huán)境安全具有重要意義。

根據(jù)傳感方式的不同，機(jī)器人碰撞保護(hù)方法分為兩大類。第一類采用外部傳感器，主要包括外部感知傳感器、六維力/力矩傳感器和關(guān)節(jié)轉(zhuǎn)矩傳感器等。外部感知傳感器常用的有可感知皮膚、接近傳感器、視覺傳感器等。采用六維力/力矩傳感器，靈敏度比較高，但昂貴的價格限制了應(yīng)用范圍。采用關(guān)節(jié)轉(zhuǎn)矩傳感器是近年來興起的一種新的機(jī)器人柔順控制方式［5］，其對機(jī)器人的安全控制是很方便的，但傳感器的安裝增加了關(guān)節(jié)的柔性，加大了建模與控制的難度，甚至可能引起振動。第二類為無傳感器的碰撞檢測方法［6］，通過檢查機(jī)器人本身具有的編碼器、電流等信息來判斷碰撞的發(fā)生。Kazuhiro Kosuge提出一種基于自適應(yīng)控制律的碰撞檢測方法，其通過自適應(yīng)控制估計系統(tǒng)模型參數(shù)，比較實際力矩與模型給定力矩來判斷碰撞的發(fā)生［7］。鄒風(fēng)山在研究真空機(jī)器人位置誤差變化的基礎(chǔ)上，采用位置信息，建立基于位置偏差模糊推理的碰撞檢測系統(tǒng)，提高了系統(tǒng)碰撞檢測的準(zhǔn)確性和靈敏度［4］。

機(jī)器人與外界環(huán)境接觸時，由于末端約束力的作用，機(jī)器人各個關(guān)節(jié)上會增加相應(yīng)的關(guān)節(jié)轉(zhuǎn)矩，可稱之為反應(yīng)轉(zhuǎn)矩。機(jī)器人與外界環(huán)境碰撞時，反應(yīng)轉(zhuǎn)矩會突然增加，因此可以根據(jù)反應(yīng)轉(zhuǎn)矩判斷碰撞的發(fā)生。本文針對潔凈機(jī)器人的特點，提出一種采用電流檢測式關(guān)節(jié)轉(zhuǎn)矩傳感，直接檢測電機(jī)電流，利用電機(jī)模型與機(jī)器人動力學(xué)模型，設(shè)計反應(yīng)轉(zhuǎn)矩觀測器，對碰撞發(fā)生間接測量，實現(xiàn)碰撞保護(hù)功能?；陔娏鞯姆磻?yīng)轉(zhuǎn)矩檢測響應(yīng)速度快，具有較高的實時性，且不需要額外的傳感器，成本低廉，也不會對機(jī)器人的結(jié)構(gòu)剛度產(chǎn)生任何影響，具有廣泛的適用性。

1　動力學(xué)模型

1.1機(jī)械結(jié)構(gòu)與連桿參數(shù)

潔凈機(jī)器人工作空間為圓柱形，可實現(xiàn)豎直方向的上下運(yùn)動，沿豎直軸的旋轉(zhuǎn)運(yùn)動，以及平面關(guān)節(jié)三連桿結(jié)構(gòu)用來實現(xiàn)水平方向的伸縮運(yùn)動。圖1是典型SCARA結(jié)構(gòu)的潔凈機(jī)器人外形圖。豎直方向的運(yùn)動由絲杠傳動，水平方向運(yùn)動由皮帶和帶輪傳動。

圖1　SCARA結(jié)構(gòu)潔凈機(jī)器人

采用DH法建立潔凈機(jī)器人的坐標(biāo)系如圖2。

圖2　SCARA機(jī)器人坐標(biāo)系

由于SCARA型手臂特殊的結(jié)構(gòu)形式，其角度耦合和連桿長度滿足特殊的耦合關(guān)系。

角度耦合關(guān)系為：

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

連桿參數(shù)如表1。

表1　SCARA機(jī)器人連桿參數(shù)

1.2動力學(xué)模型

在不考慮摩擦和環(huán)境作用力的情況下，機(jī)器人關(guān)節(jié)空間典型的動力學(xué)方程為：

M（q）¨q+C（q，.q）.q+G（q）=τ（3）

其中：q為關(guān)節(jié)變量。方程的左邊各項含義為（n為機(jī)器人的關(guān)節(jié)數(shù)也是自由度數(shù)）：

M∈Rn×n：關(guān)節(jié)空間慣性矩陣（對稱，正定）；C∈Rn×n：哥氏力和向心力計算矩陣；G∈Rn×1：重力項向量；q∈Rn×1：廣義關(guān)節(jié)偏差向量；τ∈Rn×1：廣義驅(qū)動轉(zhuǎn)矩向量；

根據(jù)機(jī)器人動力學(xué)的拉格朗日方法，各矩陣和向量可以分別計算得到。

其中：mi為第i個連桿的質(zhì)量，JL，i和JR，i為連桿i雅可比矩陣的平動項和轉(zhuǎn)動項，Ii為第i個連桿的慣性矩陣。

C矩陣：C矩陣也是由連桿的動能方程導(dǎo)出，其有多種求法，一種特殊的方法為：G向量：重力向量G由勢能方程導(dǎo)出。

考慮到SCARA結(jié)構(gòu)機(jī)器人的特殊構(gòu)型，整個機(jī)器人的旋轉(zhuǎn)運(yùn)動由2號電機(jī)驅(qū)動，第3，4，5軸的伸縮運(yùn)動由3號電機(jī)驅(qū)動，其水平方向運(yùn)動的重力項為0。考慮連桿參數(shù)和角度的耦合關(guān)系，動力學(xué)方程可以化簡。

1.3動力學(xué)參數(shù)辨識

考慮摩擦和環(huán)境作用力，完整的機(jī)器人動力學(xué)方程為：M（q）¨q+C（q，.q）.q+Fv.q+Fssgn（.q）+g（q）=τ-JT（q）Fext（8）

其中：Fext為環(huán)境作用力，J為機(jī)器人的雅可比矩陣。根據(jù)動力學(xué)方程的性質(zhì)可以對其化簡，并利用系統(tǒng)辨識方法進(jìn)行參數(shù)辨識。

連桿參數(shù)的向量形式為：πi=［mimilcixmilciymilciz

I∧ixxI∧ixyI∧ixzI∧iyyI∧iyzI∧izz］T（9）

根據(jù)動力學(xué)模型的特性，動力學(xué)方程可以被線性化為如下形式：

簡記為：

τ=Y（q，.q，¨q）π（11）

測量以下變量：

1）直接測量q和.q（采用編碼器）；

2）間接測量¨q（構(gòu)建濾波器）；

3）間接測量τ（基于電機(jī)電流）；

得到N次數(shù)據(jù)：

采用最小二乘辨識算法得到辨識結(jié)果：

2　反應(yīng)轉(zhuǎn)矩觀測與碰撞保護(hù)

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

反應(yīng)轉(zhuǎn)矩為機(jī)器人與外界環(huán)境接觸時所增加的關(guān)節(jié)轉(zhuǎn)矩，而關(guān)節(jié)轉(zhuǎn)矩的檢測采用電流式關(guān)節(jié)轉(zhuǎn)矩傳感，因而需要建立交流永磁同步電機(jī)的轉(zhuǎn)矩模型。

永磁同步電機(jī)模型的建立過程中做以下基本假設(shè)［8］：

1）轉(zhuǎn)子永磁磁場在氣隙空間分布為正弦波，定子電樞繞組中的感應(yīng)電勢也為正弦波；

2）忽略定子鐵心飽和，認(rèn)為磁路線性，電感參數(shù)不變；

3）不計鐵心渦流與磁滯等損耗；

4）轉(zhuǎn)子上沒有阻尼繞組。

三相永磁電機(jī)可以在空間矢量坐標(biāo)系下分析，有如下矢量方程。

磁鏈方程：ψs=Lsis+ψf=Lsσis+Lmis+ψf（14）式中，Ls是定子自感，包括漏感Ls和勵磁電感Lm兩部分，勵磁電感Lm=1.5 Lab，Lab是三相坐標(biāo)系下的兩相互感。ψf是轉(zhuǎn)子磁鏈?zhǔn)噶浚譮=ψfejθe，θe是轉(zhuǎn)子角度，也就是轉(zhuǎn)子磁場定向下的定向電角度。

電壓方程：

其中，

轉(zhuǎn)矩方程：電磁轉(zhuǎn)矩表示為磁鏈和電流的外積，恒幅值變換下表示為：

轉(zhuǎn)子磁場定向以后，轉(zhuǎn)子磁鏈ψf只有d軸分量，穩(wěn)態(tài)下d軸電流為零。根據(jù)式（14）、（15）和（16）分別可得下列方程。

磁鏈方程：電壓方程：轉(zhuǎn)矩方程：

其中：τp稱為永磁轉(zhuǎn)矩，τr稱為磁阻轉(zhuǎn)矩，對于表貼磁體，Ld=Lq，磁阻轉(zhuǎn)矩為0，從而：

保持磁鏈幅度不變，通過控制q軸電流可以獨立控制電磁轉(zhuǎn)矩。

2.2反應(yīng)轉(zhuǎn)矩觀測器設(shè)計

機(jī)器人與外界環(huán)境作用過程中，反應(yīng)轉(zhuǎn)矩可以視為負(fù)載力矩擾動，這種干擾具有多變性、不確定性和隨機(jī)性，無法預(yù)見和準(zhǔn)確測量。而擾動觀測器（DOB）不需要建立干擾的準(zhǔn)確的數(shù)學(xué)模型，而且它本身的結(jié)構(gòu)簡單，避免了大量的計算，能夠良好地滿足實時要求。

圖3為擾動觀測器的基本結(jié)構(gòu)框圖。在實際的對模型的構(gòu)建的過程中，得到的模型總是會與實際系統(tǒng)有某些差別。DOB會把實際系統(tǒng)和名義系統(tǒng)的輸出之間的差異作為一種干擾，并將這種干擾輸入到原來的模型中。圖中，P（s）表示實際控制系統(tǒng)的傳函，Pn（s）是名義的數(shù)學(xué)模型，u代表的是系統(tǒng)的輸入，d是影響系統(tǒng)的外部的干擾，^d為d的估算值。

圖3　擾動觀測器基本結(jié)構(gòu)圖

名義模型的逆矩陣P-1n（s）是沒有辦法進(jìn)行物理實現(xiàn)的，解決措施是在干擾觀測器的兩個輸入端同時串聯(lián)一個濾波器Q（s），使得Q（s）的相對階次能夠大于或者等于名義模型的相對階次，使名義模型成為了物理上可以實現(xiàn)的函數(shù)。Q（s）可以選擇為一階低通濾波器。

機(jī)器人完整動力學(xué)方程 （8）可以改寫為反應(yīng)轉(zhuǎn)矩形式：

M（q）¨q+C（q，.q）.q+F（q，.q）+g（q）=τ-τext（21）式中，F(xiàn)（q，.q）為機(jī)器人關(guān)節(jié)處的摩擦力矩，τext為施加在關(guān)節(jié)上的外作用力矩，即反應(yīng)轉(zhuǎn)矩。

減速器傳動的機(jī)器人，關(guān)節(jié)角度和轉(zhuǎn)矩滿足：

q=N-1qm（22）

τ=Nτm（23）式中，N∈Rn×n是每個關(guān)節(jié)減速比的對角陣。

考慮電機(jī)轉(zhuǎn)子動力學(xué)，轉(zhuǎn)子的轉(zhuǎn)矩平衡方程重寫為：

Jm¨qm+Fm（qm，.qm）+N-1（M（q）¨q+C（q，.q）.q+

F（q，.q）+g（q）+τext）=τm（24）

式中，Jm為電機(jī)轉(zhuǎn)子的慣量，F(xiàn)m（qm，.qm）為電機(jī)轉(zhuǎn)子處的摩擦力矩。帶入關(guān)節(jié)角度關(guān)系，得到：

（M（q）+N2Jm）¨q+C（q，.q）.q+F（q，.q）+g（q）+τext=τ（25）

推導(dǎo)過程中，將電機(jī)轉(zhuǎn)子處的摩擦歸入摩擦項F（q，.q）。式（25）為計及電機(jī)轉(zhuǎn)子動力學(xué)在內(nèi)的機(jī)器人完整動力學(xué)模型。

由此可得作用在關(guān)節(jié)上的反應(yīng)轉(zhuǎn)矩τext：

τext=τ- ［（M（q）+N2Jm）¨q+C（q，.q）.q+ F（q，.q）+g（q）］（26）

加入一階低通濾波器，代入電機(jī)轉(zhuǎn)矩模型和關(guān)節(jié)轉(zhuǎn)矩傳遞關(guān)系，得到：

實際工程之后，式（27）中一些項的參數(shù)不能精確獲得或者本身的模型不夠精確，因此只能用相應(yīng)項的估計值代替，由此得到最終的反應(yīng)轉(zhuǎn)矩觀測器為：

2.3力/位混合控制與碰撞保護(hù)

力/位混合控制基于機(jī)器人位置子空間與力子空間的互補(bǔ)性、正交性，在位置子空間進(jìn)行位置控制，在力子空間進(jìn)行力控制。Raibert和Craig于1981年提出了力/位混合控制［9］?；诜磻?yīng)轉(zhuǎn)矩觀測器的機(jī)器人力/位混合控制結(jié)構(gòu)如圖4所示，該控制結(jié)構(gòu)可用于碰撞保護(hù)、拖曳式示教等多種力控制應(yīng)用。圖中τext被視為擾動量，τ、qd為系統(tǒng)輸入，q為系統(tǒng)輸出，τe∧xt為的估值，當(dāng)反應(yīng)轉(zhuǎn)矩觀測器的輸出∧超出一定閾值，則判τextτext斷碰撞發(fā)生。關(guān)節(jié)轉(zhuǎn)矩的傳感采用電機(jī)電流和系統(tǒng)模型。每個關(guān)節(jié)既有位置控制器，又有力控制器。為了根據(jù)約束條件改變每個自由度所要求的控制模式，圖中引入了選擇矩陣S和S′，其為3×3矩陣，是兩組互鎖開關(guān)。如要求第i個關(guān)節(jié)進(jìn)行位置（或力）控制則矩陣S（或S′）對角線上的第i個元素為1，否則為零。

圖4　基于反應(yīng)轉(zhuǎn)矩觀測器的力/位混合控制結(jié)構(gòu)圖

3　實驗

潔凈機(jī)器人連桿結(jié)構(gòu)對稱，慣性矩陣的主要項為Ixx，Iyy和Izz，其他項的值比較小可以被忽略。通過矩陣相乘，Ixx和Iyy項也被消掉，動力學(xué)計算值僅與Izz項相關(guān)。通過最小二乘法參數(shù)辨識可以得到每個連桿的Izz項，如表2所示。

機(jī)器人的摩擦辨識通過測量勻速運(yùn)動過程中的關(guān)節(jié)轉(zhuǎn)矩，并減去動力學(xué)所引起的轉(zhuǎn)矩項得到。摩擦辨識結(jié)果如表3所示。

表2　機(jī)器人連桿慣性參數(shù)Izz

表3　機(jī)器人連桿慣性參數(shù)Izz

圖5為機(jī)器人在碰撞保護(hù)功能使能和禁止情況下發(fā)生碰撞時的電機(jī)轉(zhuǎn)矩曲線。在碰撞發(fā)生后，第一個任務(wù)是快速而準(zhǔn)確的判斷出碰撞的發(fā)生，判斷出系統(tǒng)發(fā)生碰撞后的一個重要問題是如何控制機(jī)器人，保證將碰撞造成的傷害降到最低。本文采用驅(qū)動器動力下電的方式，使用軟件輸出為驅(qū)動器下動力電或切斷驅(qū)動器動力電源。如圖5中曲線所示，機(jī)器人發(fā)生碰撞后，通過反映轉(zhuǎn)矩判斷碰撞發(fā)生，并下動力電，電機(jī)電流迅速將為0值。該方法可以有效的保護(hù)機(jī)器人自身和外部環(huán)境的安全。

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

4　結(jié)論

本文提出一種基于反應(yīng)觀測器的機(jī)器人的碰撞檢測方法，該方法不需要使用額外的傳感器，僅根據(jù)電機(jī)電流以及系統(tǒng)模型來進(jìn)行碰撞檢測。系統(tǒng)模型包括電機(jī)轉(zhuǎn)矩模型和機(jī)器人動力學(xué)模型兩部分，在機(jī)器人模型化簡后采用最小二乘法進(jìn)行系統(tǒng)參數(shù)辨識，提高系統(tǒng)的準(zhǔn)確性與靈敏度。同時通過設(shè)計反應(yīng)轉(zhuǎn)矩觀測器和力/位混合控制結(jié)構(gòu)，實現(xiàn)機(jī)器人的主動柔順控制功能。最后針對潔凈機(jī)器人進(jìn)行碰撞保護(hù)實驗，實驗結(jié)果表明該方法可以在非常短的時間內(nèi)判斷出碰撞的發(fā)生，采取有效地措施，保障機(jī)器人與外界環(huán)境的安全。

［1］Yamada Y，Hirasawa Y，Huand S，et al.Fail-safe human/robot contact in the safety space［A］.International Workshop on Robot and Human Communication［C］.IEEE，1996：59-64.

［2］叢明，沈?qū)毢?，于?面向IC制造的凈化機(jī)器人［J］.高技術(shù)通訊，2007，17（4）：395-400.

［3］梁燕華，金鴻章.模糊控制策略在避碰運(yùn)動系統(tǒng)中的應(yīng)用［J］.系統(tǒng)仿真學(xué)報，2008，20（6）：1554-1558.

［4］鄒風(fēng)山，曲道奎，徐方，等.基于位置誤差的真空機(jī)器人碰撞保護(hù)研究［J］.組合機(jī)床與自動化加工技術(shù)，2009，9（9）：17 23.

［5］Spong M W.Modeling and control of elastic joint robots［J］.Journal of Dynamics Systems，Measurement and Control，Transactions of the ASME，1987，109（4）：310-319.

［6］De Luca A，F(xiàn)errajoli L.Exploiting Robot Redundancy in Collision Detection and Reaction［A］.2008 IEEE International Conference on Intelligent Robots and Systems［C］.2008：3299-3305.

［7］Morinaga S，Kosuge K.Collision detection system for manipulator based on adaptive impedance control law［A］.IEEE International Conference on Robotics and Automation［C］.Taipei，ROC，2003：1080-1085.

［8］Pillay P，Krishnan R.Modeling，Simulation，and Analysis of Permanent-magnet Motor Drives，Part I：The Permanent-magnet Synchronous Motor Drive［J］.IEEE Transactions on Induced Application.1989，25（2）：265-273.

［9］Raibert M H，Craig J J.Hybrid Position/Force Control of Manipulators［J］.Transactions of ASME Journal of Dynamics Systems，Measurement，and Control.1981，102：126-133.

Reactive Torque Observation and Collision Protection for Clean-room Manipulator

Song Jilai1，2，3，Xu Fang1，2，3，Zou Fengshan3
（1.Shenyang Institute of Automation，Chinese Academy of Sciences，Shenyang110016，China；2.University of Chinese Academy of Sciences，Beijing110049，China；3.SIASUN Robot& Automation Co，Ltd Shenyang，Shenyang110068，China）

An approach of robot collision protection based on reactive torque observation is presented.Reactive torque is estimated by the disturbance observer based on the model of the system，which including motor torque model and robot dynamic model.Because of the special structure and joint coupling relationship of clean-room robot，the dynamics of robot is greatly simplified，and simplified model is identified by the least squares method which improved the precision of the model.According to the torque of permanent magnet synchronous motor and dynamic of the robot，joint torque based on motor current can be getted.When robot contact with the external environment，the increasion of the joint torque is the reaction torque，which is used to implement the active compliance control combined with disturbance observer and the force/position hybrid control.At last，collision protection experiment is carried out on the clean-room robot，and the experimental results verify the effectiveness of the proposed method.

robot control；robot dynamics；collision protection；joint torque control；system identification

1671-4598（2016）05-0246-05

10.16526/j.cnki.11-4762/tp.2016.05.069

TP273

A

2016-02-22；

2016-03-23。

國家科技重大專項（2014ZX02103005）。

宋吉來（1982-），男，遼寧營口人，博士研究生，主要從事機(jī)器人控制方向的研究。