基于Matlab機械臂力控系統(tǒng)仿真研究

覃海強,熊慶宇,2,石 欣,王 楷

QIN Haiqiang1,XIONG Qingyu1,2,SHI Xin1,WANG Kai1

1.重慶大學(xué) 自動化學(xué)院，重慶 400044

2.重慶大學(xué) 軟件學(xué)院，重慶 400044

1.School of Automation，Chongqing University，Chongqing，400044，China

2.School of Software Engineering，Chongqing University，Chongqing，400044，China

1 引言

隨著科技的進(jìn)步，機械臂在生產(chǎn)自動化中占有越來越重要的地位。機械臂在執(zhí)行裝配、拋光、去毛刺等任務(wù)時，都會與環(huán)境相互接觸，并且產(chǎn)生相互的作用力，太大的作用力會損壞機械臂及其加工工件, 因此必須對機械臂的受力進(jìn)行有效控制。為了有效地實現(xiàn)復(fù)雜機械臂系統(tǒng)的力控制并降低風(fēng)險節(jié)約成本，對機械臂力控制系統(tǒng)進(jìn)行仿真驗證是一種有效的途徑。SimMechanics是Matlab 中的機構(gòu)系統(tǒng)模塊集，它可以對各種運動副連接的剛體進(jìn)行建模與仿真，達(dá)到對機構(gòu)系統(tǒng)進(jìn)行分析與設(shè)計的目的。SimMechanics與其它仿真平臺相比，最大的優(yōu)點是無需建立機構(gòu)運動的數(shù)學(xué)模型和編制程序，并且具有系統(tǒng)建模方便直觀和仿真功能強大的特點，能大大減輕設(shè)計人員的工作，為機械系統(tǒng)的建模仿真提供一個強大而方便的平臺。因此，本文采用 SimMechanics仿真平臺，對二自由度機械臂力控系統(tǒng)進(jìn)行仿真與研究，進(jìn)而實現(xiàn)對工業(yè)機械臂在完成特定作業(yè)時與環(huán)境間接觸力的控制。

力控就是對機器人末端執(zhí)行器輸出力或關(guān)節(jié)力矩的控制。最早將力控用于多關(guān)節(jié)機器人上的是Whitney，他將力傳感器用在多關(guān)節(jié)機器人上，并用解運動速度的方法（RMRC）推導(dǎo)出力反饋控制的向量表達(dá)式[1]。P.Fraisse, F.Pierrot和P.Dauchez首先提出虛擬環(huán)境下的機械臂力控制, 此方法可以增強機械臂力控制方案的魯棒性[2]；Gabriel Afonso和J.Norberto Pires針對工業(yè)機械臂在進(jìn)行拋光作業(yè)的過程中提出一種行之有效的力控方法，使機械臂能夠以期望的接觸力在期望的位置上運動，最終完成機械臂的拋光作業(yè)[3]。

本文根據(jù)機械臂的拋光任務(wù)，以平面二自由度機械臂為研究對象，對其進(jìn)行動力學(xué)分析，并在Matlab/SimMechanics中建立其機械模型與力控系統(tǒng)，在平面中模擬機械臂末端與環(huán)境間的接觸面，實現(xiàn)對平面二自由度機械臂的力控，使機械臂在力控制下完成預(yù)期的運動軌跡。同時設(shè)計振蕩抑制控制器，實現(xiàn)機械臂與剛性環(huán)境碰撞接觸過程中沖擊振蕩階段的振蕩抑制。

2 機械臂動力學(xué)模型

不考慮摩擦力等外界干擾的作用，n自由度機械臂動力學(xué)方程為[4]：

其中，M(q)為正定的慣性矩陣；h(q, q˙)為離心力和哥氏力矢量；G(q)為重力矢量；q為關(guān)節(jié)位置；τ為關(guān)節(jié)驅(qū)動力矩。

圖1 平面二自由度機械臂

在平面中建立二自由度機械臂模型如圖 1所示，假設(shè)機械臂為剛性，兩個關(guān)節(jié)均為轉(zhuǎn)動關(guān)節(jié)，且每個連桿的質(zhì)量都集中在連桿的末端。兩個連桿的質(zhì)量分別為m1和m2；長度分別為l1和l2；關(guān)節(jié)角位置為；關(guān)節(jié)力矩為

平面二自由度機械臂動力學(xué)方程為：

在笛卡爾坐標(biāo)系中，當(dāng)機械臂末端與外部環(huán)境相互接觸時[5]，機械臂與外界將會產(chǎn)生相互的作用力Fe，為了保持機械臂的平衡狀態(tài)，必須對各個關(guān)節(jié)施加一定的驅(qū)動力矩τ，且有，其中，J為機械臂的雅可比矩陣，JT為機械臂雅可比矩陣的轉(zhuǎn)置，且有：

通過 JT可將作用在機械臂末端的笛卡爾力映射成為等效的關(guān)節(jié)力矩。與環(huán)境相接觸時，機械臂的動力學(xué)方程為：

3 平面二自由度機械臂力控

3.1 機械臂的力控

根據(jù)機械臂動力學(xué)，設(shè)計機械臂的力控系統(tǒng)如圖 2所示，其中，F(xiàn)d為笛卡爾坐標(biāo)系中機械臂末端所受的期望外力；Fe為力傳感器反饋的機械臂末端力信號； JT機械臂雅可比矩陣的轉(zhuǎn)置；Kfp和Kfi分別為比例增益與積分增益；CTH為力變換矩陣。

圖2 機械臂力控系統(tǒng)結(jié)構(gòu)圖

3.2 平面二自由度機械臂的力控

在 Matlab/SimMechanics環(huán)境下對平面二自由度機械臂系統(tǒng)進(jìn)行力控仿真與研究的思路為：在笛卡爾坐標(biāo)系中，機械臂末端沿著圓的內(nèi)側(cè)（接觸面）運動，在運動過程中，機械臂末端與圓內(nèi)側(cè)時刻保持接觸，且相互作用力為一恒定值Fe（忽略機械臂末端與環(huán)境間的摩擦力）。為了保持機械臂的在運動過程中的平衡狀態(tài)，必須對各個關(guān)節(jié)施加一定的驅(qū)動力矩τ（在此，平面二自由度機械臂的關(guān)節(jié)數(shù)恰與受控力的維數(shù)相匹配），根據(jù)式(5)可得所需施加的驅(qū)動力矩τ為：

其中，F(xiàn)x和Fy是機械臂末端與外界相互作用的期望力Fe在笛卡爾坐標(biāo)系上的分解，在機械臂運動過程中Fx和Fy時刻變化，合力Fe大小保持不變，方向為圓的法向且由圓心指向圓外，如圖 3所示。

整個力控過程即為控制機械臂在一定時間內(nèi)沿著一個圓的內(nèi)側(cè)運動，并保持在運動過程中，機械臂與圓內(nèi)側(cè)的相互作用力Fe為一恒定值。

圖3 平面二自由度機械臂力控圖

3.3 接觸力模型

當(dāng)機械臂末端與環(huán)境接觸時，根據(jù)胡克定律，環(huán)境施加于機械臂的作用力Fe可作為彈性恢復(fù)力來模擬：

其中，矢量ΔXE表示由接觸引起的環(huán)境局部變形，由圖 4可知，當(dāng)接觸時 Δ XE=X-XE；當(dāng)不接觸時，ΔXE=0。KE描述環(huán)境的剛度。式(7)中忽略了摩擦力的作用，環(huán)境剛度KE和變形量ΔXE是方便又理想的表示方法，接觸力Fe作為機械臂和環(huán)境集中變形來考慮，會更為準(zhǔn)確。

圖4 機械臂末端與環(huán)境接觸模型

3.4 沖擊振蕩階段的抑制控制

機械臂與剛性環(huán)境碰撞接觸的過程可分為三個階段：接近運動階段、沖擊振蕩階段、阻尼振蕩階段和穩(wěn)定階段[6]，如圖 5所示。

圖5 機械臂與剛性環(huán)境的碰撞接觸過程

在機械臂與環(huán)境處于沖擊振蕩階段時，兩者之間將會產(chǎn)生一系列的脈沖力，如果脈沖力過大，必將會造成機械臂或環(huán)境的損壞。因此，需設(shè)計沖擊振蕩抑制控制器以削弱沖擊振蕩階段的振動幅值，縮短沖擊振蕩持續(xù)時間。

沖擊振蕩階段的抑制控制原理如圖6所示，其中，圖 6的（a）表示沖擊振蕩過程中機械臂末端的位置，虛線部分表示開環(huán)控制時的位置，實線部分表示通過抑制控制所期望達(dá)到的效果。圖6的（c）表示向機械臂關(guān)節(jié)所提供的驅(qū)動力，整個抑制控制的過程即為依據(jù)機械臂末端位置的不同而改變關(guān)節(jié)驅(qū)動力的大小，最終實現(xiàn)振蕩的抑制。圖6的（b）為機械臂關(guān)節(jié)角速度-時間圖，用來確定τmax與τd的切換時機，為切換的速度閾值。

圖6 沖擊振蕩階段的抑制控制

圖 7詳細(xì)給出了一個完整的振蕩周期的抑制控制原理。ta、ts、tb、tc分別表示振蕩周期的開始、驅(qū)動力切換、零速和振蕩周期結(jié)束的時間。

圖7 一個振蕩周期的抑制控制原理

當(dāng)t∈[ta, ts]時，對機械臂施加能提供的最大關(guān)節(jié)驅(qū)動力τmax，τmax=JTFe，F(xiàn)e為反饋的機械臂末端的接觸力，以此實現(xiàn)最大化的振蕩抑制，降低后續(xù)振蕩能量；當(dāng)t∈[ts, tc]時，關(guān)節(jié)力矩則取較小的值τd，τmax=JTFd，F(xiàn)d為期望的機械臂末端接觸力，以此滿足對期望接觸力的控制。機械臂驅(qū)動力在τmax與τd之間切換，主要的依據(jù)是切換時間ts，分析沖擊振蕩過程可知，對ts的選取，需滿足以下兩個條件：

條件1：需保證當(dāng)機械臂末端速度q˙(t)≥0(t∈[tb, tc])時，驅(qū)動力τmax必須停止作用從而減小機械臂恢復(fù)運動的速度。

條件2：盡可能長地增加τmax的作用時間，從而最大限度地削弱系統(tǒng)的振蕩能量。

ts具體計算過程如下：

步驟 1：根據(jù)條件1，ts必須選在tb之前。如圖 7所示，定義δt為tb與ts的時間差，且在tb時刻機械臂關(guān)節(jié)角速度q˙為0，則有：

其中，Jm為機械臂等效慣量。

其中Fe為反饋的接觸力，F(xiàn)e=0表示未接觸狀態(tài)，F(xiàn)e＞0表示接觸狀態(tài)。

4 平面二自由度機械臂的力控仿真與結(jié)果

4.1 SimMechanics仿真

在 Matlab/SimMechanics中搭建的平面二自由度機械臂力控系統(tǒng)如圖 8所示，其中包括平面二自由度機械臂的機械模型、期望力、力傳感器以及力控制器模塊[8][9][9]。

圖8 基于Matlab/SimMechanics的力控系統(tǒng)

平面二自由度機械臂的機械模型如圖 9所示，其中，兩個Revolute為轉(zhuǎn)動副，表示機械臂的兩個轉(zhuǎn)動關(guān)節(jié)；兩個 body表示機械臂的兩個連桿，連桿的具體參數(shù)為：長度 l1=l2=0.35m ，質(zhì)量m1=m2=0.747kg ，相對于 Z軸的轉(zhuǎn)動慣量為IZ1=IZ2=0.0424kg*m2；Joint Sensor為關(guān)節(jié)傳感器，其輸出信號為關(guān)節(jié)的角度矢量q、角速度矢量以及角加速度矢量˙；Body Sensor輸出信號為機械臂末端笛卡爾坐標(biāo)系下的位置坐標(biāo)x、y以及z，由于二自由度的機械臂只能控制二維方向上的位置或力，因此不對z軸數(shù)據(jù)做處理。

圖9 基于SimMechanics的二自由度機械臂機械模型

根據(jù)式(7)，構(gòu)造機械臂末端力傳感器模型如圖10所示，取環(huán)境剛度KE=80000N/m；ΔXE=X -XE，XE設(shè)為期望機械臂末端的運動軌跡，即為圓的半徑，取R=XE=0.5m；X為笛卡爾坐標(biāo)系下，機械臂末端在運動過程中與軌跡圓心的距離（為了簡單起見，取軌跡圓心與機械臂第一個固定轉(zhuǎn)動關(guān)節(jié)重合）。由式(7)可知，當(dāng)機械臂與環(huán)境（即期望的軌跡）沒有接觸時，將不受到環(huán)境的外力；當(dāng)機械臂與環(huán)境接觸時，將產(chǎn)生相互的作用力Fe。這樣就能夠在平面中，模擬出機械臂與環(huán)境的接觸面。

圖10 基于SimMechanics的力傳感器模型

機械臂末端期望的受力模型如圖 11所示。在機械臂與環(huán)境接觸前，給機械臂末端施加一個沿著接觸面的法向，大小為8N的力Fe。在這個力的作用下，機械臂將向著期望的環(huán)境靠近，并最終與環(huán)境接觸。在接觸后，此期望力Fe即為機械臂與環(huán)境間相互作用的平衡力。

圖11 基于SimMechanics的機械臂末端期望力模型

力控制器則是在Matlab中，將M文件嵌入到Simulink 仿真環(huán)境中，依據(jù)式(6)的機械臂動力學(xué)方程，對系統(tǒng)進(jìn)行力控。同時，根據(jù)式(8)、(9)對機械臂與剛性環(huán)境碰撞接觸過程中的沖擊振蕩階段進(jìn)行抑制控制。

4.2 仿真結(jié)果

具體的仿真結(jié)果如圖 12、圖 13、圖 14、圖 15所示。SimMechanics仿真環(huán)境中的平面二自由度機械臂的機械模型如圖 12所示。

圖12 基于SimMechanics的二自由度機械臂模型

機械臂末端的運動軌跡如圖 13所示，通過對機械臂末端接觸力的控制，實現(xiàn)了機械臂末端運動軌跡的規(guī)劃。

圖13 機械臂末端運動軌跡

當(dāng)未對碰撞接觸過程中的沖擊振蕩階段進(jìn)行抑制控制時，機械臂末端與環(huán)境間的法向接觸力如圖 14所示?？梢?，機械臂與環(huán)境相接觸時，特別是在沖擊振蕩階段，兩者之間產(chǎn)生一系列的脈沖力，相互間的碰撞相當(dāng)劇烈，因此，有必要設(shè)計沖擊振蕩抑制控制器以削弱沖擊振蕩階段的振動幅值，縮短沖擊振蕩持續(xù)時間。

圖14 未進(jìn)行沖擊振蕩抑制控制的法向接觸力

對機械臂與環(huán)境碰撞接觸過程中的沖擊振蕩階段進(jìn)行抑制控制后，機械臂末端與環(huán)境間的法向接觸力如圖 15所示。實例仿真結(jié)果表明，沖擊振蕩抑制控制器的使用，有效消弱了沖擊振蕩階段的振動幅值、縮短沖擊振蕩持續(xù)時間。同時，在機械臂穩(wěn)定運動的過程中，其末端與接觸面之間的相互作用力保持為恒定的8N，很好的實現(xiàn)了特定作業(yè)下，機械臂與環(huán)境間接觸力的控制。

圖15 進(jìn)行沖擊振蕩抑制控制的法向接觸力

5 結(jié)論

本文主要研究了平面剛性二自由度機械臂與環(huán)境接觸力的控制問題，在 Matlab/SimMechanics中建立力控系統(tǒng)的仿真模型，解決了機械臂力控過程中的一些問題：（1）機械臂力控作業(yè)的環(huán)境描述和運動建模；（2）在平面中模擬出機械臂與環(huán)境的接觸面；（3）機械臂力控作業(yè)的力-運動軌跡生成；（4）實現(xiàn)機械臂末端執(zhí)行器輸出力以及關(guān)節(jié)力矩的控制；（5）實現(xiàn)機械臂與剛性環(huán)境碰撞接觸過程中的沖擊振蕩階段的振蕩抑制。此后的工作將要實現(xiàn)平面剛性二自由度機械臂力-位混合控制，并將機械臂由二自由度擴展到多自由度，最終實現(xiàn)多自由度機械臂的力-位混合控制。

[1]D.E.Whitney.Resolved motion rate control of manipulators and human prostheses[J].IEEE Transactions on Man-Machine Systems, vol.10, pp.47–53, 1969.

[2]P.Fraise, F.Pierrot, P.Dauchez.Virtual environment for robot force control[J].IEEE 1993.219- 224.

[3]G.Afonso, J.N.Pires, and N.Estrela.Force control experiments for industrial applications:A test case using an industrial deburring example[J].Assem.Autom.J., vol.26, no.2, pp.148–156, 2007.

[4]F.L.Lewis, C.T.Abdallah, and D.M.Dawson, Control of Robot Manipulators[M].New York:MacMillan,1993.69-71.

[5]J.T.Wen.Position and Force Control of Robot Arms[J].Proc.IEEE International Symposium on Intelligent Control, Albany, New York, pp.251-256, September 1989.

[6]Nakju Lett Doh,Wan Kyun Chung,Youngil Youm,and Youngwhan Oh.Two Transition Phase Control Methods for Hard Contact[J].ASME Journal of Dynamic System,Measurement and Control, Vol 129, pp.262-274, May 2007.

[7]Yuan Shaoqiang, Liu Zhong, Li Xingshan.Modeling and Simulation of Robot Based on Matlab/SimMechanics[J].Proc.IEEE Symp.Control, IEEE Press,July 2008,pp. 161-165,doi:10.1109/CHICC.2008.4604913.

[8]Wenbin Deng, Jae-Won Lee, Hyuk-Jin Lee.Kinematics simulation and control of a new 2 DOF parallel mechanism based on Matlab/SimMechanics[J].Proc.IEEE Symp. Computing,Communication,Control,and Management(CCCM), IEEE Press,Aug 2010, pp.233-236.doi:10.1109/CCCM.2009.5267894.

[9]薛定宇, 陳陽泉.基于MATLAB/ Simulink 的系統(tǒng)仿真技術(shù)與應(yīng)用[M].北京:清華大學(xué)出版社, 2002.