基于外力估計的并聯(lián)機器人柔順控制策略研究

倪 濤 孫 旭 李 東 趙亞輝 張泮虹 鄧英杰

(1.燕山大學車輛與能源學院， 秦皇島 066004; 2.吉林大學機械與航空航天工程學院， 長春 130022;3.燕山大學機械工程學院， 秦皇島 066004)

0 引言

隨著科學技術的進步，機器人領域發(fā)展迅速[1-3]，目前，由于具有承載大負載、位置控制精度高、同時末端的工件慣性小等優(yōu)點，并聯(lián)機器人應用越來越廣泛，因此很多學者對并聯(lián)機器人的控制技術進行了研究。

機器人的阻抗控制[4-5]為經典的機器人柔順控制策略，其目標是在機器人末端執(zhí)行器的位置和環(huán)境接觸力之間建立期望的動態(tài)關系[6]。阻抗控制按其實現(xiàn)方式可分為基于力的阻抗控制和基于位置的阻抗控制?；谖恢玫淖杩箍刂撇呗訹7]由內部位置控制環(huán)和外部力反饋環(huán)組成，接觸力信息用于修改末端執(zhí)行器的期望位置，從而實現(xiàn)末端執(zhí)行器位置的柔順控制?；诹Φ淖杩箍刂撇呗訹8]由內部力控制環(huán)和外部位置反饋環(huán)組成，位置軌跡的偏差用于修改末端執(zhí)行器的指令力，從而實現(xiàn)機器人末端力的柔順控制的作用。BROENINK等[9]設計了一套基于實時檢測的力傳感器值的阻抗控制系統(tǒng)用于控制機器人完成作業(yè)。CHAN等[10]提出的阻抗控制策略根據給定的控制移動路徑和從環(huán)境獲得的力傳感器值，通過設定的阻抗方程不斷地縮小路徑偏差和接觸力偏差，實現(xiàn)柔順控制。由于大多數機器人容易配備高精度的位置伺服控制器，基于位置的控制策略可以避免控制器的重新設計。因此基于位置的阻抗控制策略成為一種更受關注的方法。

在力反饋控制方面，近年來國內外學者重點研究基于外力估計控制方式代替使用力傳感器測量的方法，并取得了一定成果。YANG等[11]提出了一種不需要力傳感器，只需要獲得位置信息的控制策略，該方法通過前置反饋力矩，利用逆運動學推導出機器人的實時控制力矩，并且通過多自由度的機械臂的仿真和實驗驗證了該控制器可以應用于對未知環(huán)境的類人學習。LE等[12]結合了時間延時估計，并同時利用狀態(tài)估計，利用機器人的關節(jié)力矩估計末端執(zhí)行器受力。ZHANG等[13]提出一種通過關節(jié)位置誤差和關節(jié)控制回路的諧波估計外力的方法，基于該方法關聯(lián)各關節(jié)的力矩與角度偏差，利用存在的力矩估計外力的方式計算外力。GUTIERREZ-GILES等[14]提出了一種基于廣義比例積分技術的速度/力觀測器，關節(jié)速度和接觸力僅通過位置測量進行估算，然后用于力/位置控制方案。STOLT等[15]提出了一種不需要力傳感器的控制方法，通過利用關節(jié)位置偏差直接估計所受外力。

本文針對機器人在作業(yè)過程中，能夠實現(xiàn)位置精度控制及柔順控制效果，將采用并聯(lián)機器人基于位置的阻抗控制策略，并在阻抗控制實現(xiàn)過程中，利用基于動力學模型的外力估計方法，檢測機器人與環(huán)境接觸時的作用力并估計其值。

1 理論模型建立

1.1 平臺逆運動學分析

Stewart平臺結構如圖1所示，電動缸下端與固定平臺的連接點所在的平面為固定平面，且固定平面的中心點與世界坐標系B的原點O重合，電動缸的上端與動平臺的連接點所在的平面為動平面，且動平面的中心點與動坐標系P的坐標原點重合。定義坐標系B的y軸為下平臺短邊b1b2的中垂線，坐標系P的y軸為上平臺長邊a1a2的中垂線，向量ai(i=1,2,…,6)表示上連接點相對于動坐標系的位置，向量bi(i=1,2,…,6)表示固定平面的6個連接點在世界坐標系B中的位置。

圖1 Stewart平臺結構簡圖Fig.1 Structure diagram of Stewart platform

由Stewart平臺結構中的空間位置關系則可推算出上平臺6個連接點在坐標系P下的位置，下平臺6個連接點在坐標系B下的位置[16]。定義廣義坐標向量q=(x,y,z,φ,θ,φ)，其中(x,y,z)表示動坐標系P的原點相對于世界坐標系B的位置，(φ、θ、φ)表示動坐標系P的原點在世界坐標系B中的空間姿態(tài)。定義平臺6個電動缸的上連接點與下連接點之間的距離向量為l=(l1,l2,l3,l4,l5,l6)。

采用ZXZ歐拉角的旋轉順序，依次旋轉角度φ、θ、φ，則可得到動坐標系P相對于世界坐標系B的齊次旋轉變化矩陣BRp，計算公式為

(1)

式中c表示余弦函數，s表示正弦函數。

利用此變化矩陣與各參數之間的關系可推算出平臺6個電動缸的上連接點與下連接點之間的距離向量li(i=1,2,…,6)，電動缸速度矢量和動平臺速度之間的關系，以及電動缸的線速度與末端執(zhí)行器的廣義速度之間的關系[17-18]。

電動缸速度矢量和動平臺速度的關系表達式為

(2)

式中x——動坐標系原點相對于世界坐標系的位置

ω——上平臺相對于坐標系B的角速度

電動缸的線速度與末端執(zhí)行器的廣義速度的計算關系為

(3)

其中

(4)

電動缸的線加速度可由電動缸的線速度求導得到。計算公式為

(5)

其中

(6)

1.2 平臺逆動力學分析

1.2.1單個電動缸受力分析

如圖2所示，以電動缸上下兩部分質心為坐標原點分別建立坐標系o1x1y1z1和o2x2y2z2。其中x1、x2方向沿著軸線方向；y1、y2方向沿著萬向節(jié)旋轉的軸單位向量vi方向；z1、z2的方向由右手定則確定。

圖2 支鏈動力學分析圖Fig.2 Branched chain dynamics analysis diagram

(7)

(8)

其中

ci=ui×vi

(9)

(10)

(11)

式中ni、ui——沿x1、z1軸的單位矢量

ωi——第i個電動缸角速度

αi——第i個電動缸角加速度

ai1——第i個電動缸伸縮桿質心加速度

ai2——第i個電動缸缸筒質心加速度

m1——第i個電動缸伸縮桿質量

m2——第i個電動缸缸筒質心質量

la——電動缸伸縮桿質心到電動缸上連接點的距離

lb——電動缸缸筒質心到電動缸下連接點的距離

li——第i個電動缸上連接點與下連接點之間的距離

g——重力加速度

(12)

1.2.2動平臺受力分析

圖3 上平臺受力圖Fig.3 Upper platform stress diagram

在世界坐標系B中，根據動平臺受力平衡建立方程

(13)

在世界坐標系B中，根據動平臺力矩平衡建立方程

(14)

式中Ip——動平臺自身的轉動慣量

pd——動平臺的綜合質心在動坐標系P的位置矢量

xn——動平臺質心在世界坐標系B中的位置矢量

mp——動平臺質量

pai——點ai相對于動坐標系的位置

α——上平臺相對于坐標系B的角加速度

聯(lián)立式(1)～(14)可推導出Stewart平臺逆動力學方程，計算公式為

(15)

(16)

確定了支撐桿和移動平臺之間的相互作用力就可以計算出電動缸的驅動力，利用受力平衡列出方程，可求出作用于電動缸軸向力，計算公式為

(17)

電動缸的伸縮桿驅動力矢量為

F=(f1,f2, …,f6)T

(18)

動平臺受力和力矩矢量為

τ=(fx,fy,fz,τφ,τθ,τφ)T

(19)

則τ與F關系式為

τ=J-TF

(20)

1.3 伺服運動系統(tǒng)動力學分析

利用平臺逆運動學模型可以計算出各個電動缸的伸縮長度以及伸縮速度，需要建立電動缸伸縮長度與電機轉角之間的數學模型才能控制平臺的運動。電動缸運動系統(tǒng)的控制原理圖如圖4所示。

圖4 電動缸數學模型Fig.4 Mathematical model of electric cylinder

圖4中左邊為電機數學模型，利用電壓平衡理論，建立方程

(21)

式中ua——電機電樞電壓

ia——電機電樞電流

La——電機電樞電感

Ra——電樞電阻θm——電機旋轉角

Va——電機反電動勢

Ke——反電動勢系數

交流電機的電磁轉矩方程為

Te=Kti

(22)

式中Te——電機電磁轉矩

Kt——電機轉矩系數i——電機電流

包含電機摩擦的電機動力學方程為

(23)

式中Jm——電機轉動慣量

Tmf——電機摩擦力矩

Tm——電機軸端輸出力矩

如圖4所示，右邊框圖為滾珠絲杠的數學模型，考慮到滾珠絲杠轉動時的摩擦，滾珠絲杠動力學方程為

(24)

式中JL——滾珠絲杠的轉動慣量

TLf——滾珠絲杠的摩擦力矩

TL——滾珠絲杠的負載力矩

θL——滾珠絲杠的轉動角度

對式(21)～(24)進行拉氏反變換得到

(25)

聯(lián)立式(25)中的4個等式，可以得到以電壓Ua為輸入，電機旋轉角θm為輸出的伺服運動系統(tǒng)的傳遞函數，其計算式為

(26)

滾珠絲杠的直線位移與電機旋轉角有一定的轉換關系，數學表達式為

(27)

式中l(wèi)——滾珠絲杠的伸縮長度

P——導程

可以將電機旋轉角轉換為電動缸的伸縮量,對Stewart平臺運動位姿和運動速度的控制就可以轉換為對電機旋轉角和轉速的控制。

2 基于外力估計的阻抗控制

2.1 基于動力學模型的外力估計

現(xiàn)有的機器人外力檢測方法可以分為兩種：①利用機器人自身關節(jié)處安裝的傳感器來檢測機器人受到的外力。②將六維力傳感器[19-20]裝在機器人的末端關節(jié)來檢測機器人受到環(huán)境的作用力。由于精度較高的力傳感器價格很高，因此提出一種無傳感器外力估計的方法，利用Stewart平臺的動力學模型和電機的電流反饋值來估算外力作用時機器人關節(jié)力的變化值。電流環(huán)處于電機控制系統(tǒng)的最底層，它具有很高的靈敏度，因此利用伺服電機的電流反饋值來估算外力的方法可行。

該方法需要利用Stewart并聯(lián)機器人的動力學模型計算出動平臺正常運動時6個電動缸的軸力，將電動缸的軸力轉換為電機的力矩再加上電動缸的摩擦產生的力矩得到理論力矩。采集電機正常運動時的實際力矩并與理論力矩對比，就可以估算外力產出的力矩，而伺服電機的電流和力矩之間存在數學關系，因此可以利用電機的電流反饋值來估算外力。

已知包含摩擦力的Stewart并聯(lián)機器人的動力學方程為

(28)

τm——電機的理論力矩

Ml(l)——關節(jié)空間的慣性矩陣

Gl(l)——重力/扭矩分量矩陣

η——滾珠絲杠的傳動效率

當有外力作用于Stewart平臺末端時，實際電機的力矩為

τc=τm+τext

(29)

其中τext表示外力作用于Stewart機器人末端時引起的電機力矩變化值。若Fc為Stewart平臺與環(huán)境接觸時實際受力，則τc與Fc的轉換關系就是滾珠絲杠的軸力和力矩之間的關系

τc=2FcπPη

(30)

則可知

τext=τc-τm=2FcπPη-τm

(31)

所以外力作用導致每個電動缸的軸力增加值可以估算為

(32)

式中Kt——電機轉矩系數

Δi——電機電流變化值

機器人在運動過程中會有一定的機械振動，再加上外界噪聲的干擾，導致電機采集到的反饋電流波動較大。如果電機電流受外界噪聲影響較大會導致機器人感知外力的能力變弱，也可能將噪聲引起的電流變化誤以為是外力作用引起的。針對這個問題，需要將實際測得的電流通過濾波的方法，減少噪聲的干擾，從而讓機器人更準確地感知外力作用，就可以減小外力估計的誤差。

目前，卡爾曼濾波方法[21-22]和維納濾波方法[23]是最為常用的濾波處理方法。維納濾波方法的使用環(huán)境為頻域空間，在處理波動較小的隨機信號中使用較多，但是這種方法需要很大的計算量，對計算機的內存要求也高，所以實際應用時有較大局限性?？柭鼮V波器與大多數濾波器不同的是它是一種純粹的時域濾波器，直接可以在時域下設計并實現(xiàn)。它利用線性系統(tǒng)的狀態(tài)方程，不僅能估計和處理一維的穩(wěn)定信號，還能對非平穩(wěn)的、多維的、隨機的信號進行估計。此外，還能實現(xiàn)實時的遞推濾波，容易使用編程語言實現(xiàn)，所以被廣泛應用于信號的降噪處理與最優(yōu)估計。

采用卡爾曼濾波法對伺服電機的電流反饋信號進行濾波處理。通過對實測的電流進行降噪處理，電機的電流變化會穩(wěn)定在一個較小的范圍，這樣外力引起的電流變化會更明顯，Stewart平臺就可以很容易感知到外力的作用。

卡爾曼濾波的基本原理就是信息的兩個更新過程，一個是時間的更新過程，一個是觀測狀態(tài)的更新過程，卡爾曼濾波器的5個核心公式為：

(1)預測狀態(tài)方程，根據第k-1次狀態(tài)獲得第k次系統(tǒng)狀態(tài)的估計值，即

Xk|k-1=AXk-1|k-1+BUk

(33)

(2)預測協(xié)方差方程，獲得預測值Xk|k-1的協(xié)方差矩陣Fk|k-1，即

Fk|k-1=AFk-1|k-1AT+Q

(34)

(3)計算增益方程，根據k時刻協(xié)方差矩陣的預測值計算卡爾曼增益，即

(35)

(4)最優(yōu)值的更新(卡爾曼濾波的輸出)，利用狀態(tài)變量的預測值和系統(tǒng)實測值計算k時刻狀態(tài)變量的最優(yōu)值，即

Xk|k=Xk|k-1+Kk(Zk-HkXk|k-1)

(36)

(5)計算最新的協(xié)方差方程，求出當前時刻的協(xié)方差，即

Fk|k=(I-KkHk)Fk|k-1

(37)

由圖5可以看出，電機的實測電流經卡爾曼濾波處理后波動幅度穩(wěn)定在一個較小的范圍。這樣可以通過電機的電流反饋檢測外力是否作用于Stewart平臺末端，當檢測到外力時機器人將停止運動或者進行位姿修正。

圖5 電機電流濾波處理Fig.5 Motor current filtering processing

2.2 基于位置的阻抗控制

阻抗控制是一種適用于機械設備相互作用任務的機器人控制方法，可以使機器人的末端執(zhí)行器與環(huán)境的剛性接觸變?yōu)槿嵝越佑|，能夠實現(xiàn)位置和力的動態(tài)交互。對Stewart平臺采用基于位置的阻抗控制方法，并設計阻抗控制器?；谖恢玫淖杩箍刂频臄祵W模型為

(38)

式中M——慣性矩陣, 為對角矩陣

C——阻尼系數矩陣，為對角矩陣

K——剛度矩陣，為對角矩陣

Fd——Stewart平臺與環(huán)境接觸的期望力

Fc——Stewart平臺與環(huán)境接觸時實際受力

在頻域下式(38)表達式為

X(s)-Xr(s)=Y(s)ΔF(s)

(39)

其中

(40)

式(40)為阻抗控制器，阻抗控制器其實就是一個線性的二階系統(tǒng)。二階系統(tǒng)的固有頻率ωm、阻尼比ξ、增益Z為

(41)

基于位置的阻抗控制方案主要由并聯(lián)機器人正逆運動學模型、外力估計模型、力反饋阻抗控制模型組成，此方案需要電機工作在位置模式下,電動缸運動過程中的摩擦采用庫侖-粘性摩擦模型[24-26]。首先給定機器人期望末端位姿，利用逆運動學計算出電動缸的長度和運動過程中的速度，將電動缸長度和運動速度換算為電機轉角和角速度后發(fā)送給驅動器。平臺在運動過程中如果受到外界作用力，利用電機的電流反饋可估算出機器人與環(huán)境的接觸力，將機器人與環(huán)境的交互作用力代入導納控制器可以得到機器人位姿的調整量。圖6為基于位置的阻抗控制系統(tǒng)的控制框圖,圖中qd為機器人期望末端位姿，qc為機器人實際末端位姿，lc為電動缸實際長度，參數K、M、C是定義在末端的任務空間，s代表微分，1/s代表積分。

圖6 基于位置的阻抗控制系統(tǒng)框圖Fig.6 Block diagram of position based impedance control system

2.3 阻抗參數選取

Stewart平臺為空間六自由度并聯(lián)機器人，包括3個平移和3個旋轉，所以其矩陣為6×6的對角矩陣。矩陣M、C、K形式為

(42)

(43)

(44)

由于阻抗控制模型實際上是一個二階系統(tǒng)，所以當二階系統(tǒng)處于臨界阻尼狀態(tài)或過阻尼狀態(tài)時阻抗參數較理想。首先利用單自由度系統(tǒng)選取阻抗參數，m、b、k是矩陣M、C、K對角線中的元素，由式(41)可知，系統(tǒng)剛度k的倒數就是系統(tǒng)增益，所以可以根據期望的增益先確定阻抗參數k。由式(41)可知系統(tǒng)的固有頻率只與k和m有關，在確定了k后，m越大系統(tǒng)的頻率就越小，也就是系統(tǒng)到達穩(wěn)定狀態(tài)的周期就越大。為了使系統(tǒng)快速到達穩(wěn)定狀態(tài)可以減小系統(tǒng)的慣性系數m。在確定系統(tǒng)的慣性系數m和剛度k后，可以根據二階系統(tǒng)的阻尼比選擇阻尼系數。

3 仿真

利用Simulink搭建阻抗控制模型，加入到動力學仿真模型中，綜合考慮系統(tǒng)的性能，初步選擇M、C、K對角線中的元素m均為20、b均為300、k均為500進行阻抗控制仿真。

仿真時，設置仿真時間為15 s，采樣時間T為0.02 s。設定Stewart平臺沿Z軸方向的期望軌跡曲線為0.2sin(0.25πt)+1.6，則此時期望速度為0.05πcos(0.25πt)，期望加速度為-0.012 5π2·sin(0.25πt)。仿真時間為6 s時施加一沿X軸正方向的外力100 N和沿Z軸負方向的外力50 N，仿真時間為8 s時撤去外力作用，仿真分析外力作用對平臺運動軌跡的影響。

由圖7可以看出，在沒有外力作用時，Stewart仿真平臺會沿著期望設定的軌跡運動，在施加外力后，Stewart上平臺沿Z軸的實際軌跡會偏離期望軌跡曲線，偏離程度與外力有關。當撤去外力后平臺會逐漸恢復到期望設定的軌跡進行運動。

圖7 Z軸軌跡變化曲線Fig.7 Trajectory change curves of Z axis

圖8為Stewart上平臺位移變化曲線，可以看出在受到外力作用后上平臺會順著外力的作用方向移動一段距離。撤去沿Z軸負向的外力后，平臺將沿Z軸以設定的軌跡運動；撤去沿X軸正向的外力后，平臺在X軸的位置將恢復到原點。

圖8 平臺末端位移變化曲線Fig.8 End position change curves of platform

仿真結果表明，設計的Stewart平臺基于位置的阻抗控制模型是正確的，且該控制策略能夠使平臺在外力作用時實現(xiàn)柔順控制的效果，撤去外力作用后能夠恢復到原來的運動狀態(tài)。

4 實驗

利用實驗室搭建的實驗平臺系統(tǒng)，對提出的Stewart平臺基于位置的阻抗控制算法進行實驗。圖9為實驗系統(tǒng)構成圖。

圖9 并聯(lián)機器人實驗系統(tǒng)構成圖Fig.9 General drawing of parallel robot experimental system

如圖10所示，設定平臺沿Z軸正方向的期望軌跡運動，在平臺向上運動過程中人為施加外力作用于末端執(zhí)行器，此時電動缸的電流會發(fā)生明顯改變，通過電流的變化檢測到外力作用，將外力的作用解算為每個電動缸的缸長變化量進行平臺末端位姿的修正。

圖10 Stewart平臺阻抗控制實驗圖Fig.10 Stewart platform impedance control experiment diagrams

如圖11所示，為了驗證Stewart平臺基于位置的阻抗控制的順應性運動,首先在世界坐標系中施加一個外力，通過上位機控制程序采集電機的力矩變化值和動平臺坐標原點在世界坐標系下的位置。

圖11 動平臺外力作用圖Fig.11 External force action diagram of moving platform

由圖12可以看出，施加一個外力后電機的力矩發(fā)生了明顯變化，再次施加一個較大的外力后，電機力矩會增大，在撤去外力后電機力矩逐漸恢復到0左右。與此同時，在第1個階段動平臺產生了沿X軸正向和沿Y軸負向的運動。在外力增大后，動平臺沿X軸正向產生的位移和沿Y軸負向產生的位移也會增大，此時為順應軌跡段。在撤去外力作用后，動平臺的位置又會恢復到起始位置，在圖中對應的是恢復軌跡段。由圖12可得，動平臺在外力作用方向產生了順應性運動，驗證了阻抗控制理論模型的可行性。

圖12 Stewart平臺順應運動驗證Fig.12 Stewart platform compliance motion verification

由圖13可以看出，在位置控制模式下，動平臺沿Z軸的位置跟蹤誤差為0.2～0.5 mm。從圖14可以看出，動平臺沿X軸的位置跟蹤誤差為0.1～0.4 mm，由于跟蹤距離較小，實際跟蹤曲線近似斜直線。

圖14 Stewart平臺沿X軸的位置跟蹤Fig.14 Position tracking of Stewart platform along X axis

由實驗結果可知，基于位置的阻抗控制方法的位置控制精度高，而且平臺受到外力作用后的柔順控制響應速度也快。

5 結束語

針對并聯(lián)機器人的柔順控制問題，提出了無需力傳感器的阻抗控制策略。闡述了Stewart并聯(lián)機器人的動力學模型，并以此為基礎提出了利用動力學模型和電流反饋來估計環(huán)境接觸力的方法。設計了基于位置的阻抗控制策略，分析了阻抗參數的選取規(guī)則。最后進行了Stewart平臺阻抗控制的仿真與實驗，結果表明基于外力估計的并聯(lián)機器人阻抗控制方法能夠實現(xiàn)精確、穩(wěn)定、快速的柔順控制效果。