基于無源觀測器的機(jī)器人笛卡爾空間力/阻抗控制方法

孔繁旭， 呂鵬, 王洪光, 常勇

(1.中國科學(xué)院沈陽自動(dòng)化研究所， 沈陽 110016； 2.中國科學(xué)院機(jī)器人與智能制造創(chuàng)新研究院， 沈陽 110169)

現(xiàn)階段大多數(shù)的工業(yè)機(jī)器人都應(yīng)用于搬運(yùn)、焊接和噴漆等任務(wù)，可以基于運(yùn)動(dòng)學(xué)的控制思想，采用控制位姿的方法實(shí)現(xiàn)。在這些任務(wù)中，機(jī)器人末端都不與工作環(huán)境相接觸，位姿控制就可以滿足精度和安全要求。但是機(jī)器人在完成一些與環(huán)境存在力作用的任務(wù)時(shí)，比如打磨、裝配，單純的位置控制會(huì)由于位置誤差而引起過大的作用力，從而傷害零件或機(jī)器人。機(jī)器人在這類運(yùn)動(dòng)受限環(huán)境中運(yùn)動(dòng)時(shí)，往往需要配合力控制來使用[1-2]。近年來對(duì)實(shí)現(xiàn)力控制的不同策略進(jìn)行了廣泛研究[3-6]。力位混合控制是指機(jī)械臂在某些自由度上進(jìn)行力控制，在余下的自由度上進(jìn)行位置控制[7-9]。位置控制和力控制是解耦的，他們的控制方法可以分開設(shè)計(jì)，一般來說，位置控制需要比例-微分 (proportional-proportional,PD) 控制，力控制需要比例-積分(proportional-integral, PI)控制，因?yàn)槲恢每刂菩枰憫?yīng)速度快，而力控制需要誤差更小。力位混合控制算法的優(yōu)點(diǎn)是可以直接控制機(jī)器人末端的位置和作用力，而缺點(diǎn)是需要環(huán)境的精準(zhǔn)模型和控制模式切換時(shí)穩(wěn)定性較差。阻抗控制[10-12]是將機(jī)械臂末端等效成理想的質(zhì)量-彈簧-阻尼系統(tǒng)，建立末端位置或速度與接觸力間的動(dòng)態(tài)關(guān)系，通過調(diào)整目標(biāo)阻抗參數(shù)可以調(diào)整這種動(dòng)態(tài)關(guān)系。阻抗控制的缺點(diǎn)是由于缺少力的閉環(huán)控制無法精確地控制力。為了解決這個(gè)問題，文獻(xiàn)[13]提出了笛卡爾空間的力/阻抗控制器，該控制器將力控制與阻抗控制進(jìn)行融合，實(shí)現(xiàn)了較高的力控精度的同時(shí)還具有柔順性。由于力控制器與阻抗控制器采用并聯(lián)結(jié)構(gòu)所以不需要在兩種控制算法中進(jìn)行切換。力控器設(shè)計(jì)的難點(diǎn)是在力控制過程中末端工具與作業(yè)對(duì)象脫離接觸時(shí)可能會(huì)使機(jī)械臂的移動(dòng)速度快速增加并造成機(jī)械臂與周圍環(huán)境發(fā)生碰撞。提出了基于無源性的笛卡爾空間力/阻抗控制, 通過導(dǎo)入能量罐的概念，使力跟蹤控制器、阻抗控制器、能量罐和電機(jī)模型構(gòu)成一個(gè)無源系統(tǒng)從而保證系統(tǒng)的穩(wěn)定性。但是只通過能量罐的能量限制本身并不能阻止能量的突然釋放，這可能導(dǎo)致系統(tǒng)某部分不滿足無源性的能量突然增加, 文獻(xiàn)[14]中通過根據(jù)參考功率軌跡調(diào)節(jié)能量罐和系統(tǒng)之間的交換功率來解決這個(gè)問題。在此控制策略中使用笛卡爾空間動(dòng)力學(xué)模型，所以具有計(jì)算量大的缺點(diǎn)。文獻(xiàn)[15]中提出了無源觀測器的概念。無源觀測器包括機(jī)械臂和除關(guān)節(jié)空間PD控制器以外的先進(jìn)控制器。只需保持無源觀測器內(nèi)部系統(tǒng)為無源的，控制系統(tǒng)整體的穩(wěn)定性便可以保證。

基于此，首先分析笛卡爾空間力/阻抗控制系統(tǒng)的無源性。期望得到可能會(huì)破壞系統(tǒng)無源性的控制輸入。將文獻(xiàn)[15]中的無源觀測器導(dǎo)入到笛卡爾空間力/阻抗控制中，嘗試解決力控制過程中末端工具與作業(yè)對(duì)象脫離接觸時(shí)末端工具可能與周圍環(huán)境發(fā)生激烈碰撞的問題，本文提出的控制算法特點(diǎn)是計(jì)算量小并且不需要?jiǎng)恿W(xué)模型。

1 機(jī)器人控制系統(tǒng)的無源性

無源性的概念來自電路理論，主要考慮系統(tǒng)與外界的能量交換。在機(jī)械系統(tǒng)中當(dāng)系統(tǒng)的輸入力fin和輸出速度vout具有相同的維數(shù)時(shí)，如果始終滿足關(guān)系式

(1)

則該系統(tǒng)稱為無源的[16]，其中γ0為定數(shù)。

n自由度的機(jī)械臂動(dòng)力學(xué)方程表示為

(2)

機(jī)械臂總能量E包括動(dòng)能K和重力勢能P,其表達(dá)式分別為

(3)

g(q)=[?P(q)/?qT]T

(4)

(5)

=-γ02

(6)

將(6)式微分可得

(7)

由式(7)可得出機(jī)器人是嚴(yán)格輸出無源的。

當(dāng)嚴(yán)格輸出無源系統(tǒng)與線性時(shí)不變嚴(yán)格正實(shí)系統(tǒng)負(fù)反饋連接時(shí)系統(tǒng)是漸進(jìn)穩(wěn)定的[10]。線性時(shí)不變嚴(yán)格正實(shí)系統(tǒng)可選定為PD控制器，則有

(8)

式(8)中：uPD為PD控制器的輸出；qd為關(guān)節(jié)位置期望值；KP為比例系數(shù)，KD是微分系數(shù)，KP與KD均大于0。該控制器實(shí)現(xiàn)了關(guān)節(jié)空間的位置控制，系統(tǒng)控制框圖如圖1所示。

圖1 關(guān)節(jié)空間的位置控制Fig.1 Position control in joint space

文中所考慮的力/阻抗控制需要在笛卡爾空間內(nèi)實(shí)現(xiàn), 所以將圖1所示的關(guān)節(jié)空間控制轉(zhuǎn)換成笛卡爾空間控制，轉(zhuǎn)換結(jié)果如圖2所示。

圖2 笛卡爾空間的位置控制Fig.2 Position control in Cartesian space

FPD是笛卡爾空間PD控制器的輸出信號(hào)，笛卡爾空間PD控制器表示為

(9)

式(9)中：Xd為笛卡爾空間位置期望值；X為笛卡爾空間位置；Kpx與Kdx分別為比例系數(shù)和阻尼系數(shù)。

笛卡爾空間與關(guān)節(jié)空間的力與速度變換關(guān)系表示為

τin=JT(q)FPD

(10)

(11)

式中：J(q)是機(jī)器人雅克比矩陣；τin為關(guān)節(jié)空間的輸入力矩。

在關(guān)節(jié)空間下討論的無源性和穩(wěn)定性的結(jié)論在笛卡爾空間下依然成立，即如圖2所示的笛卡爾空間的機(jī)器人子系統(tǒng)是無源的，整體的控制系統(tǒng)是漸進(jìn)穩(wěn)定的。

2 笛卡爾空間力/阻抗控制和無源性分析

笛卡爾空間自由度n的機(jī)械臂動(dòng)力學(xué)模型為

(12)

笛卡爾空間力/阻抗控制器的計(jì)算式為

Fin=Fimp+FF

(13)

(14)

(15)

式中：Fimp是阻抗控制器的輸出，Kpx與Kdx分別是阻抗控制器的剛度和阻尼系數(shù)。FF是力控制器的輸出，F(xiàn)d是機(jī)械臂末端對(duì)環(huán)境的期望力，KFx、IFx和DFx是力控制器的PID參數(shù)。這里的阻抗控制器與上文提到的笛卡爾空間PD控制器是相同的。笛卡爾空間力/阻抗控制器由式(14)的阻抗控制器與式(15)的力控制器組成，控制框圖如圖3所示。

圖3 笛卡爾空間力/阻抗控制器Fig.3 Cartesian force/impedance control

圖4 力/阻抗控制器的無源性分析Fig.4 Passivity analysis of Cartesian force/impedance control

當(dāng)式(13)中FF=0時(shí)，笛卡爾空間力/阻抗控制變?yōu)樽杩箍刂?，由于機(jī)器人與外部環(huán)境組成的子系統(tǒng)是無源的，并且阻抗控制為線性時(shí)不變嚴(yán)格正實(shí)的，所以系統(tǒng)整體是穩(wěn)定的。當(dāng)式(13)中FF≠0時(shí)，系統(tǒng)整體的穩(wěn)定性無法確定, 因此認(rèn)為只有FF會(huì)破壞系統(tǒng)的無源性和穩(wěn)定性。在下文中會(huì)提出監(jiān)控系統(tǒng)無源性的方法，當(dāng)檢測到系統(tǒng)接近非無源狀態(tài)時(shí)會(huì)減小力控制器的輸出FF，從而使系統(tǒng)始終保持為無源的。

3 基于無源性觀測器的笛卡爾空間力/阻抗控制

圖5 無源性觀測器Fig.5 Passivity monitor

儲(chǔ)存在無源性觀測器的能量表達(dá)式為

(16)

式(16)中：Emon與Pmon分別表示進(jìn)入無源性觀測器的能量與功率。

由式(1)的無源性的定義可知，當(dāng)Emon滿足

Emon≥E0

(17)

時(shí)，無源性觀測器為無源的。其中E0為表示無源性觀測器初始能量的定數(shù)。

通過上文的分析可知只有力控制輸出FF會(huì)破壞無源性觀測器的無源性。當(dāng)無源性觀測器接近非無源的狀態(tài)時(shí)，可以通過引入整形函數(shù)α(Emon)和β(Pmon)減小FF，從而達(dá)到恢復(fù)無源性的目的。引入整形函數(shù)后的力控制輸出設(shè)計(jì)F′F為

(18)

α=

(19)

(20)

式中：Eu與El(Eu=E0>El)是合適的定數(shù)。當(dāng)Emon在El與Eu之間時(shí)，說明無源性觀測器有從無源轉(zhuǎn)變?yōu)榉菬o源的傾向，此時(shí)設(shè)定0<α<1到達(dá)減小FF的效果。當(dāng)Emon小于El時(shí)，說明無源性觀測系統(tǒng)已經(jīng)接近非無源的狀態(tài)，此時(shí)設(shè)定α=0到達(dá)使FF為零的效果。β起到了當(dāng)系統(tǒng)有快速從無源轉(zhuǎn)換為非無源的傾向時(shí)減小FF的作用。

基于無源性觀測器的笛卡爾空間力/阻抗控制就是對(duì)笛卡爾空間力/阻抗控制器中的力控制器引入整形函數(shù)的控制策略。

通過檢測儲(chǔ)存在無源性觀測器中的能量調(diào)節(jié)會(huì)破壞系統(tǒng)的無源性的力控制輸出FF，從而使無源性觀測器始終保持為無源的。因?yàn)闊o源性觀測器與線性時(shí)不變嚴(yán)格正實(shí)的PD控制負(fù)反饋連接所以系統(tǒng)整體是穩(wěn)定的。

圖6 仿真模型Fig.6 Simulation model

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

使用Matlab/Simulink的Simscape Multibody 工具箱進(jìn)行模型的搭建，仿真模型如圖6所示。雙連桿機(jī)械臂的長度分別是l1=0.32 m,l2=0.29 m。在桿2的末端固定了一個(gè)半徑10 mm的小球。關(guān)節(jié)1與關(guān)節(jié)2的角度分別用q1和q2表示。板1與板2在Y方向上有4 mm的落差。

實(shí)驗(yàn)過程分成3個(gè)過程，過程1是雙連桿機(jī)械臂從初始位置q1=0 rad,q2=0 rad運(yùn)動(dòng)到q1=0.785 rad,q2=-2.04 rad的位置，此時(shí)采用的控制策略是式(8)所示的關(guān)節(jié)空間位置控制。過程2是使用笛卡爾空間力/阻抗控制器進(jìn)行控制。阻抗控制器的運(yùn)動(dòng)指令為沿著X方向，以0.05 m/s的速度運(yùn)動(dòng)。力控制器的期望力為(Fdx,Fdy,Fdz)=(0, -40,0)N。在力控制器的作用下雙連桿機(jī)械臂會(huì)向Y負(fù)方向運(yùn)動(dòng), 直到連桿2的末端小球與板1接觸為止，末端小球會(huì)給板-40 N的下壓力。過程3中所使用的控制器與過程2相同，由于板1與板2之間在Y方向上有高度差，機(jī)械臂末端運(yùn)動(dòng)到板1與板2的交界處時(shí)會(huì)產(chǎn)生較大的沖擊力。仿真實(shí)驗(yàn)中對(duì)比了力/阻抗控制器與基于無源性觀測器的力/阻抗控制器的機(jī)械臂末端的受力曲線。

力/阻抗控制器參數(shù)選擇為Kpx=1 000 N/m,Kdx=20 N·s/m,KFx=0.1,IFx=5 Hz，DFx=0。無源性觀測器的參數(shù)選擇為，Eu=E0=1 J,El=0 J,Pl=-2.0 W。

圖7所示為使用力/阻抗控制器時(shí)機(jī)械臂末端在Y方向的受力和速度。

圖7 笛卡爾空間力/阻抗控制器仿真結(jié)果Fig.7 Simulation results of cartesian force/impedance control

從圖7(a)中可知過程2中在力控制作用下機(jī)械臂末端在0.536 s時(shí)與板1接觸, 機(jī)械臂末端在板1與板2的交界處受到的沖擊力的最大值為550 N。圖7(b)中可知在板1與板2的交界處機(jī)械臂末端Y方向的移動(dòng)速度vy的最大值為-0.51 m/s。

圖8所示為基于無源性觀測器的力/阻抗控制結(jié)果。圖8(a)為機(jī)械臂末端在Y方向的受力Fyext，圖8(b)為機(jī)械臂末端Y方向的移動(dòng)速度vy，圖8(c)為儲(chǔ)存在無源性觀測器的能量Emon，圖8(d)為無源性觀測器的能量儲(chǔ)存功率Pmon，圖8(e)自變量是Emon的整形函數(shù)α的曲線，圖8(f)為自變量是Pmon的整形函數(shù)β的曲線。

圖8 基于無源觀測器的笛卡爾空間力/ 阻抗控制器仿真結(jié)果Fig.8 Simulation results of passivity monitor based cartesian force/impedance control

從圖8(a)中可得末端小球在板1與板2的交界處受到的沖擊力的最大值為僅為62 N。由圖8(b)可知在板1與板2交界處機(jī)械臂末端最大移動(dòng)速度的為-0.053 m/s。由圖8(c)可知儲(chǔ)存在無源性觀測系統(tǒng)的能量Emon在過程2中是遞增的，機(jī)械臂運(yùn)動(dòng)到板1與板2的交界處時(shí)Emon減小。由圖8(e)可知α=1,說明由于Emon>E0，所以無源觀測器內(nèi)的系統(tǒng)是無源的。由圖8(d)可知在板1與板2的交界處無源觀測器的功率為-4 W，說明無源觀測器內(nèi)部的系統(tǒng)有快速成為非無源的傾向。無源觀測器的功率為-4 W，如圖8(f)所示β=0.5，F(xiàn)′F=0.5FF。說明無源性觀測系統(tǒng)起到了減小力控制器的輸出的作用。

5 結(jié)論

(1)首先分析了笛卡爾空間力/阻抗控制器的無源性，得出只有力控制器的輸出會(huì)破壞系統(tǒng)的無源性的結(jié)論。

(2)針對(duì)笛卡爾空間力/阻抗控制器設(shè)計(jì)了無源性觀測器。無源觀測器的輸入和輸出分別是阻抗控制器的輸出和機(jī)械臂末端速度。通過儲(chǔ)存于無源性觀測器的能量判斷系統(tǒng)是否是無源的，在無源性的判斷上由于不需要使用機(jī)器人動(dòng)力學(xué)模型，所以本算法具有較好的可行性及一定的工程參考性。無源性觀測器也可以針對(duì)其他的先進(jìn)力控算法進(jìn)行設(shè)計(jì)所以具有一定的泛用性。

(3)仿真結(jié)果表明使用無源性觀測器的力/阻抗控制器可以解決機(jī)器人末端工具與作業(yè)對(duì)象脫離接觸時(shí)速度激增的問題提高了系統(tǒng)的安全性。