沿任意傾斜面的機(jī)器人力/位置控制方法研究

李正義 唐小琦 熊 爍 葉伯生

華中科技大學(xué)國(guó)家數(shù)控系統(tǒng)工程技術(shù)研究中心，武漢， 430074

0 引言

機(jī)器人與未知環(huán)境間的力/位置控制主要研究減小未知環(huán)境的幾何參數(shù)和動(dòng)力學(xué)參數(shù)(慣量、阻尼、剛度)對(duì)控制系統(tǒng)性能的影響及如何提高機(jī)器人力/位置控制的自適應(yīng)能力等，其中關(guān)于機(jī)器人與任意傾斜面間的力/位置控制以其環(huán)境模型相對(duì)簡(jiǎn)單受到眾多學(xué)者青睞。文獻(xiàn)[1-2]給出了基于機(jī)器人關(guān)節(jié)角位移、角速度的傾斜面法向矢量在線(xiàn)自適應(yīng)計(jì)算公式，但算法收斂較慢；文獻(xiàn)[3-4]根據(jù)機(jī)器人關(guān)節(jié)角位移和機(jī)器人末端接觸力設(shè)計(jì)了能同時(shí)進(jìn)行斜面法向方向估計(jì)和實(shí)現(xiàn)力/位置控制的算法，接觸面切向速度恒定，但算法復(fù)雜、難以工程實(shí)現(xiàn)；文獻(xiàn)[5]結(jié)合力覺(jué)、視覺(jué)和位移傳感器實(shí)現(xiàn)了任意斜面上的機(jī)器人力/位置控制；文獻(xiàn)[6]對(duì)比研究了未知環(huán)境參數(shù)(阻尼、剛度)估計(jì)的幾種方法，指出各種方法應(yīng)用于機(jī)器人力/位置控制的優(yōu)缺點(diǎn)；文獻(xiàn)[7]提出利用遞歸最小二乘(recursive least squares,RLS)算法估計(jì)環(huán)境阻尼和剛度參數(shù)，并設(shè)計(jì)了基于神經(jīng)網(wǎng)絡(luò)的PI控制器實(shí)時(shí)補(bǔ)償環(huán)境參數(shù)估計(jì)誤差。總之，機(jī)器人與環(huán)境間的力/位置控制算法較復(fù)雜、難以工程實(shí)現(xiàn)，目前大都處于仿真階段，設(shè)計(jì)易于實(shí)現(xiàn)的機(jī)器人與環(huán)境間的力/位置控制方案有著重要意義。

本文設(shè)計(jì)了機(jī)器人自適應(yīng)阻抗控制方法以實(shí)現(xiàn)機(jī)器人末端執(zhí)行器沿任意傾斜面的力/位置控制，主要內(nèi)容包括利用機(jī)器人末端與接觸面碰撞接觸激發(fā)環(huán)境的動(dòng)態(tài)特性，由RLS算法估計(jì)環(huán)境的阻尼、剛度；接觸穩(wěn)定后，根據(jù)接觸力矩調(diào)整機(jī)器人末端姿態(tài)到期望值；在機(jī)器人末端沿斜面滑動(dòng)過(guò)程中，由于機(jī)器人末端位移相對(duì)值變化小，接觸力測(cè)量噪聲大，設(shè)計(jì)了基于位移和接觸力反饋的模糊控制器以實(shí)時(shí)修正阻抗控制模型的阻尼和剛度參數(shù)，使其滿(mǎn)足環(huán)境的阻尼、剛度變化和補(bǔ)償估計(jì)誤差的需要。

1 機(jī)器人阻抗控制模型

Hogan[8]提出了機(jī)器人阻抗控制方法，該方法通過(guò)將機(jī)器人末端力/位置控制等效為“彈簧-質(zhì)量-阻尼”模型，建立了機(jī)器人末端位移與接觸力的關(guān)系，并通過(guò)任意調(diào)節(jié)慣性、阻尼、剛度參數(shù)來(lái)調(diào)整機(jī)器人末端位置與接觸力的關(guān)系，阻抗控制克服了機(jī)器人力/位置混合控制中力控制與位置控制頻繁切換的缺點(diǎn)。機(jī)器人阻抗控制理想模型為[7]

(1)

E(t)=Fd-Fe(t)

式中，X(t)為機(jī)器人末端當(dāng)前位置；Xd為機(jī)器人末端期望位置；X(t)和Xd的一階、二階導(dǎo)數(shù)表示對(duì)應(yīng)的速度、加速度；Md為機(jī)器人阻抗模型理想慣性矩陣；Bd為阻抗模型理想阻尼矩陣；Kd為阻抗模型理想剛度矩陣(為實(shí)現(xiàn)解耦控制，Bd、Kd和Md都取為l×l正定對(duì)角矩陣，l為機(jī)器人工作空間維數(shù))；Fd為機(jī)器人末端期望接觸力；Fe(t)為實(shí)際接觸力。

機(jī)器人阻抗控制就是通過(guò)調(diào)整參數(shù)Md、Bd和Kd實(shí)現(xiàn)機(jī)器人末端期望的位移和接觸力控制。機(jī)器人阻抗控制中的環(huán)境常近似為“彈簧-阻尼”系統(tǒng)[7]：

(2)

2 環(huán)境參數(shù)對(duì)機(jī)器人阻抗控制的影響

機(jī)器人阻抗控制應(yīng)用于機(jī)器人與環(huán)境間力/位置控制時(shí)，環(huán)境參數(shù)未知或變化使控制系統(tǒng)的動(dòng)靜態(tài)性能變差，分析環(huán)境的剛度、阻尼對(duì)機(jī)器人與環(huán)境間力/位置控制性能的影響是應(yīng)用機(jī)器人阻抗控制的基礎(chǔ)[6]。

2.1 穩(wěn)定誤差影響

當(dāng)機(jī)器人末端與環(huán)境接觸并處于穩(wěn)定狀態(tài)時(shí)，對(duì)于機(jī)器人阻抗控制模型(式(1))有

Fe-Fd=Kd(X-Xd)

對(duì)于環(huán)境模型(式(2))，則有

Fe=Ke(X-Xe)

為實(shí)現(xiàn)Fd=Fe，要求

Xd=Xe+Fd/Ke

定義機(jī)器人實(shí)際控制目標(biāo)軌跡Xr為“參考軌跡”，取

Xr=Xe+Fd/Ke

用Xr替換式(1)中的Xd，得

易知當(dāng)時(shí)間t→∞時(shí)，機(jī)器人末端位移：

X(t)→Xr=Xe+Fd/Ke

機(jī)器人末端與環(huán)境間接觸力：

F(t)=Ke(X-Xe)→Fd

即同時(shí)實(shí)現(xiàn)了機(jī)器人末端期望的接觸力和位置控制，但實(shí)際應(yīng)用機(jī)器人阻抗控制時(shí)，Ke的值大多是未知的或是變化的，需在線(xiàn)計(jì)算Ke。

2.2 動(dòng)態(tài)性能影響

將式(2)代入式(1)并令Xe=Xd，將機(jī)器人與環(huán)境間相互作用等效為二階系統(tǒng)，此二階系統(tǒng)的阻尼系數(shù)ξ和無(wú)阻尼振蕩角頻率wn滿(mǎn)足[7]：

其中，md、kd、bd、ke和be分別為矩陣Md、Kd、Bd、Ke和Be中對(duì)應(yīng)的元素，表示某一方向上的等效質(zhì)量、剛度和阻尼。由于wn和ξ決定了二階系統(tǒng)動(dòng)態(tài)性能，因此，當(dāng)環(huán)境參數(shù)(ke,be)變化導(dǎo)致(wn,ξ)變化時(shí)，為保持設(shè)定的機(jī)器人與環(huán)境間力/位置控制系統(tǒng)動(dòng)態(tài)性能不變，阻抗控制模型參數(shù)(md,kd,bd)必須根據(jù)環(huán)境變化實(shí)時(shí)調(diào)整。調(diào)整的方法一種是實(shí)時(shí)測(cè)量或根據(jù)環(huán)境模型計(jì)算出(ke,be)的值，修改(md,kd,bd)以補(bǔ)償環(huán)境的變化；另一種方法是設(shè)計(jì)智能控制器，根據(jù)機(jī)器人末端位移和接觸力反饋實(shí)時(shí)調(diào)整(md,kd,bd)，降低環(huán)境變化對(duì)系統(tǒng)動(dòng)態(tài)性能的影響。本文設(shè)計(jì)的力/位置控制方法在機(jī)器人與環(huán)境作用不同階段分別采用這兩種方法提高系統(tǒng)對(duì)環(huán)境參數(shù)未知或變化的自適應(yīng)能力。

3 沿傾斜面的機(jī)器人力/位置控制設(shè)計(jì)

機(jī)器人沿傾斜面的力/位置控制主要是機(jī)器人末端沿接觸面法向方向的接觸力跟蹤控制和沿接觸面切向方向的軌跡控制，實(shí)際任務(wù)(拋光、打磨)中一般給定接觸面法向期望接觸力、機(jī)器人末端姿態(tài)與傾斜面間的夾角和沿接觸面切向的軌跡，因此，應(yīng)用機(jī)器人阻抗控制實(shí)現(xiàn)沿傾斜面的力/位置控制時(shí)，需解決機(jī)器人末端姿態(tài)、環(huán)境阻尼、剛度未知的問(wèn)題。本文在機(jī)器人末端與環(huán)境碰撞接觸過(guò)程中利用RLS算法估計(jì)環(huán)境參數(shù)(Be,Ke)，接觸穩(wěn)定后由接觸力矩計(jì)算出機(jī)器人末端姿態(tài)調(diào)整旋轉(zhuǎn)矩陣；在機(jī)器人末端沿傾斜面滑動(dòng)階段，設(shè)計(jì)模糊控制器實(shí)時(shí)調(diào)整系統(tǒng)參數(shù)(Bd,Kd)，實(shí)現(xiàn)機(jī)器人力/位置控制對(duì)環(huán)境參數(shù)(Be,Ke)變化的自適應(yīng)性。

3.1 環(huán)境參數(shù)(Be,Ke)初值估計(jì)

式中，z為復(fù)變量；T為采樣時(shí)間。

對(duì)應(yīng)差分方程為

k=1,2,…

將其改寫(xiě)為

Y(k)=φT(k)θ(k)

θ(k)=[A1(k)A2(k)]T

φ(k)=[δx(k) δx(k-1)]T

Y(k)=fe(k)+fe(k-1)

則由RLS算法得θ(k)的計(jì)算式如下：

θ(k)=θ(k-1)+G(k)(Y(k)-φT(k)θ(k-1))

其中,G(k)為增益因子；λ為遺忘因子，一般取0.9；P(k)為參數(shù)估計(jì)協(xié)方差矩陣，其遞歸計(jì)算式為

在迭代計(jì)算過(guò)程中，當(dāng)θ(k)值變化小于設(shè)定閾值時(shí)，認(rèn)為θ(k)值是穩(wěn)定的，得到θ(k)值后由a1、a2的計(jì)算式得(be,ke)估計(jì)值分別為

依照類(lèi)似的方法可得其他方向上的(be,ke)參數(shù)，若為提高(be,ke)估計(jì)的準(zhǔn)確性，可讓碰撞接觸過(guò)程中系統(tǒng)處于欠阻尼狀態(tài)[6]，獲得笛卡兒空間三個(gè)相互垂直方向上的(be,ke)后，組合即為環(huán)境的阻尼和剛度矩陣(Be,Ke)。

3.2 機(jī)器人末端姿態(tài)調(diào)整

圖1 機(jī)器人與斜面接觸示意圖

機(jī)器人末端受力如圖1所示，vn為機(jī)器人末端速度，只要在初始接觸時(shí)調(diào)整好機(jī)器人末端相對(duì)傾斜面的姿態(tài)，以后機(jī)器人末端沿傾斜面滑動(dòng)時(shí)可不用調(diào)整姿態(tài)，因此，本文采用在機(jī)器人末端與傾斜面初始穩(wěn)定接觸時(shí)根據(jù)接觸力矩調(diào)整機(jī)器人末端姿態(tài)。Fn為傾斜面法向壓力，F(xiàn)f為摩擦力，由于機(jī)器人末端與傾斜面初始穩(wěn)定接觸時(shí)沿傾斜面運(yùn)動(dòng)趨勢(shì)較弱，可忽略Ff，則在當(dāng)前姿態(tài)下，F(xiàn)n在機(jī)器人末端產(chǎn)生力矩為Mn(k)，設(shè)機(jī)器人末端在期望姿態(tài)下Fn可產(chǎn)生力矩Md，定義em(k)=Md-Mn(k)，當(dāng)前機(jī)器人末端姿態(tài)調(diào)整旋轉(zhuǎn)矩陣在機(jī)器人工具坐標(biāo)系下為T(mén)R(k+1)(力傳感器坐標(biāo)系與工具坐標(biāo)系T重合)[9]：

k=0,1,2,…

3.3 模糊控制應(yīng)用于機(jī)器人阻抗控制

根據(jù)3.2節(jié)給出的方法，當(dāng)機(jī)器人經(jīng)過(guò)姿態(tài)調(diào)整達(dá)到期望姿態(tài)后，可計(jì)算出接觸面法向方向單位矢量N，則機(jī)器人末端沿N方向壓力Fn=FN，F(xiàn)為接觸合力，定義fn=‖F(xiàn)n‖，fd=‖F(xiàn)d‖，在阻抗控制式(式(1))中取Md=mdIl×l、Bd=bdIl×l、Kd=kdIl×l，其中，Il×l為單位矩陣，則得單一方向(N方向且與操作空間坐標(biāo)系中某一坐標(biāo)軸重合)上機(jī)器人阻抗控制滿(mǎn)足：

(3)

其中，ex=x-xd，ef=fn-fd分別表示N方向上機(jī)器人末端的位移、接觸力誤差；md、bd和kd分別表示系統(tǒng)在N方向上期望的慣性參數(shù)、阻尼和剛度值；對(duì)于任意方向上的阻抗控制，Md、Bd和Kd對(duì)角線(xiàn)上元素分別表示系統(tǒng)在操作空間各個(gè)自由度方向上的慣性參數(shù)、阻尼和剛度，即根據(jù)6個(gè)類(lèi)似式(3)的求解得到機(jī)器人沿任意方向上的力/位置控制參數(shù)。在機(jī)器人末端沿接觸面滑動(dòng)過(guò)程中，環(huán)境的阻尼、剛度會(huì)變化，且機(jī)器人在不同姿態(tài)時(shí)，末端等效阻尼、剛度也不同，因此，需實(shí)時(shí)調(diào)整阻抗模型參數(shù)以適應(yīng)環(huán)境變化，機(jī)器人受力控制時(shí)，運(yùn)動(dòng)速度慢，僅需考慮調(diào)整參數(shù)(bd,kd)。

(4)

圖2 機(jī)器人力/位置阻抗控制框圖

系統(tǒng)剛度參數(shù)調(diào)整表達(dá)式為

(5)

k=0,1,2,…

(6)

t=1,2,…

4 實(shí)驗(yàn)及結(jié)果

圖3 沿斜面力/位置控制實(shí)驗(yàn)圖

利用實(shí)驗(yàn)室改造的5自由度工業(yè)機(jī)器人設(shè)計(jì)實(shí)驗(yàn)，控制系統(tǒng)硬件平臺(tái)為武漢華中數(shù)控HNC-210B控制器和HSV-18伺服驅(qū)動(dòng)，軟件平臺(tái)基于DOS系統(tǒng)，位置控制周期為4ms，在機(jī)器人末端安裝有ATI Delta力傳感器(精度達(dá)0.1N，采樣頻率為7kHz)和作為探針的鋁制光滑圓棒。實(shí)驗(yàn)現(xiàn)場(chǎng)如圖3所示，將具有一定彈性、表面光滑的木板(表面為平面)以任意傾角(取約70°)固定于墻壁上，要求控制探針始終以期望姿態(tài)與木板平面接觸并沿木板平面滑動(dòng)，為便于計(jì)算，期望姿態(tài)取探針與木板平面垂直，期望接觸力沿木板法向方向，大小為2N，木板平面切向運(yùn)動(dòng)則由控制面板上方向鍵控制。具體實(shí)驗(yàn)時(shí)，先將機(jī)器人末端運(yùn)動(dòng)到與木板平面接近的位置，然后啟動(dòng)接觸力控制功能，由于當(dāng)前接觸力小于期望值(2N)，機(jī)器人末端向木板運(yùn)動(dòng)直到接觸力穩(wěn)定為2N，在探針與木板接觸過(guò)程中采用RLS算法估計(jì)環(huán)境參數(shù)(be,ke)，接觸穩(wěn)定后根據(jù)測(cè)量的接觸力矩值(期望姿態(tài)下的接觸力矩理論值為0)調(diào)整探針姿態(tài)，使探針與木板平面垂直，同時(shí)，根據(jù)各個(gè)關(guān)節(jié)角位移計(jì)算出當(dāng)前接觸點(diǎn)在機(jī)器人基坐標(biāo)系下的坐標(biāo)，最后由手動(dòng)操作控制面板上的方向鍵控制探針沿木板平面滑動(dòng)：相對(duì)基坐標(biāo)系先沿-x方向運(yùn)動(dòng)再沿+x、-z兩方向同時(shí)運(yùn)動(dòng)，系統(tǒng)速度設(shè)定為20mm/s。

表1 模糊控制規(guī)則表(α=0.5)

圖4 傾斜面法向接觸力

圖5 機(jī)器人末端(探針)在基坐標(biāo)系下位移

5 結(jié)語(yǔ)

本文設(shè)計(jì)了自適應(yīng)阻抗控制實(shí)現(xiàn)了機(jī)器人沿任意傾斜面的力/位置控制，解決了傾斜面法向方向未知以及環(huán)境阻尼、剛度未知影響機(jī)器人力/位置控制性能的問(wèn)題，具有編程簡(jiǎn)單易于實(shí)現(xiàn)的優(yōu)點(diǎn)。為減少接觸面摩擦力對(duì)機(jī)器人末端姿態(tài)調(diào)整的影響，在機(jī)器人與環(huán)境初始接觸穩(wěn)定時(shí)，根據(jù)當(dāng)前姿態(tài)下接觸力矩調(diào)整機(jī)器人末端到期望姿態(tài)；在機(jī)器人沿傾斜面滑動(dòng)過(guò)程中，設(shè)計(jì)具有規(guī)則自調(diào)整的模糊控制器，根據(jù)機(jī)器人末端位移誤差及其變化率和接觸力誤差實(shí)時(shí)調(diào)整系統(tǒng)的阻尼、剛度參數(shù)，以減小環(huán)境參數(shù)未知對(duì)機(jī)器人阻抗控制性能的影響。

[1] Karayiannidis Y, Doulgeri Z. An Adaptive Law for Slope Identification and Force/Position Regulation Using Motion Variables[C]//IEEE International Conference on Robotics and Automation. Orlando: IEEE, 2006: 3538-3543.

[2] Doulgeri Z, Karayiannidis Y. Force/position Regulation for a Robot in Compliant Contact Using Adaptive Surface Slope Identification[J]. IEEE Trans. On Automatic Control, 2008, 53(9):2116-2122.

[3] Karayiannidis Y, Doulgeri Z. Adaptive Control for Frictional Robot Contact Tasks on Uncertain Surface Slopes[C]//16th Mediterranean Conference on Control and Automation. Ajaccio: IEEE, 2008:932-937.

[4] Karayiannidis Y, Doulgeri Z. Robot/Force Position Tracking on a Surface of Unknown Orientation[C]//European Robotics Symposium.Heidelberg: Springer,2008:253-262.

[5] Olsson T, Bengtsson J, Johansson R. Force Control and Visual Servoing Using Planar Surface Identification[C]//IEEE International Conference on Robotics and Automation.Washington:IEEE,2002:4211- 4216.

[6] Erickson D, Weber M, Sharf I. Contact Stiffness and Damping Estimation for Robotic Systems[J]. Int. J. Robot. Res.,2003,22(1):41-57.

[7] Mallapragada V, Erol D, Sarkar N. A New Method of Force Control for Unknown Environments[J]. International Journal of Advanced Robotic Systems,2006,4(3):4509-4514.

[8] Hogan N. Impedance Control: an Approach to Manipulation: Part Ⅰ- Theory, Part Ⅱ-Implementation, Part Ⅲ-Application[J] . ASME Journal of Dynamic System, Measurement and Control,1985,107(1):1-24.

[9] Calvelli T, Bona B. How to Cope with a Closed Industrial Robot Control: a Practical Implementation for a 6-dof Articulated Robot[C]//SYROCO 2003. Wroclaw: IEEE,2003:637-642.

[10] Hu Zhongxu, Bicker R, Marshall C. Position/force Control of Manipulator Based on Force Measurement and Its Application to Gear Deburring[J]. Journal of Intelligent & Fuzzy Systems,2003,14:215-223.