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

李正義 唐小琦 熊 爍 葉伯生

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

0 引言

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

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

1 機器人阻抗控制模型

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

(1)

E(t)=Fd-Fe(t)

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

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

(2)

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

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

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

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

Fe-Fd=Kd(X-Xd)

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

Fe=Ke(X-Xe)

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

Xd=Xe+Fd/Ke

定義機器人實際控制目標軌跡Xr為“參考軌跡”，取

Xr=Xe+Fd/Ke

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

易知當時間t→∞時，機器人末端位移：

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

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

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

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

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

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

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

3 沿傾斜面的機器人力/位置控制設計

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

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

式中，z為復變量；T為采樣時間。

對應差分方程為

k=1,2,…

將其改寫為

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)的計算式如下：

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

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

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

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

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

圖1 機器人與斜面接觸示意圖

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

k=0,1,2,…

3.3 模糊控制應用于機器人阻抗控制

根據(jù)3.2節(jié)給出的方法，當機器人經(jīng)過姿態(tài)調(diào)整達到期望姿態(tài)后，可計算出接觸面法向方向單位矢量N，則機器人末端沿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方向且與操作空間坐標系中某一坐標軸重合)上機器人阻抗控制滿足：

(3)

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

(4)

圖2 機器人力/位置阻抗控制框圖

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

(5)

k=0,1,2,…

(6)

t=1,2,…

4 實驗及結(jié)果

圖3 沿斜面力/位置控制實驗圖

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

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

圖4 傾斜面法向接觸力

圖5 機器人末端(探針)在基坐標系下位移

5 結(jié)語

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

[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.